void Display_status(void) { int16_t temp, range, scale; uint16_t vbat_temp; int8_t pos1, pos2, pos3; mugui_size16_t size; clear_buffer(buffer); // Display text LCD_Display_Text(4,(const unsigned char*)Verdana8,0,0); // Preset LCD_Display_Text(3,(const unsigned char*)Verdana8,0,11); // Version text LCD_Display_Text(138,(const unsigned char*)Verdana8,0,22); // RX sync LCD_Display_Text(139,(const unsigned char*)Verdana8,0,33); // RX sync LCD_Display_Text(6,(const unsigned char*)Verdana8,0,44); // Profile // Display menu and markers LCD_Display_Text(9, (const unsigned char*)Wingdings, 0, 59);// Down LCD_Display_Text(14,(const unsigned char*)Verdana8,10,55); // Menu // Display values print_menu_text(0, 1, (22 + Config.MixMode), 45, 0); print_menu_text(0, 1, (48 + Config.RxModeIn), 45, 22); print_menu_text(0, 1, (48 + Config.RxModeOut), 45, 33); mugui_lcd_puts(itoa((Config.Flight + 1),pBuffer,10),(const unsigned char*)Verdana8,45,44); // Draw battery drawrect(buffer, 100,4, 28, 50, 1); // Battery body drawrect(buffer, 110,0, 8, 5, 1); // Battery terminal vbat_temp = GetVbat(); // Calculate battery voltage limits range = SystemVoltage - Config.PowerTriggerActual; scale = range / 50; // Look out for that divide-by-zero... :) if ((vbat_temp >= Config.PowerTriggerActual) && (scale > 0)) { temp = (vbat_temp - Config.PowerTriggerActual) / scale; } else { temp = 0; } if (temp > 50) temp = 50; fillrect(buffer, 100,54-temp, 28, temp, 1); // Battery filler (max is 60) // Display voltage uint8_t x_loc = 102; // X location of voltage display uint8_t y_loc = 55; // Y location of voltage display temp = vbat_temp/100; // Display whole decimal part first mugui_text_sizestring(itoa(temp,pBuffer,10), (const unsigned char*)Verdana8, &size); mugui_lcd_puts(itoa(temp,pBuffer,10),(const unsigned char*)Verdana8,x_loc,y_loc); pos1 = size.x; vbat_temp = vbat_temp - (temp * 100); // Now display the parts to the right of the decimal point LCD_Display_Text(7,(const unsigned char*)Verdana8,(x_loc + pos1),y_loc); mugui_text_sizestring(".", (const unsigned char*)Verdana8, &size); pos3 = size.x; mugui_text_sizestring("0", (const unsigned char*)Verdana8, &size); pos2 = size.x; if (vbat_temp >= 10) { mugui_lcd_puts(itoa(vbat_temp,pBuffer,10),(const unsigned char*)Verdana8,(x_loc + pos1 + pos3),y_loc); } else { LCD_Display_Text(8,(const unsigned char*)Verdana8,(x_loc + pos1 + pos3),y_loc); mugui_lcd_puts(itoa(vbat_temp,pBuffer,10),(const unsigned char*)Verdana8,(x_loc + pos1 + pos2 + pos3),y_loc); } // Draw error messages, if any if ((General_error != 0) || (Flight_flags & (1 << FailsafeFlag))) { // Create message box fillrect(buffer, 14,8, 96, 48, 0); // White box drawrect(buffer, 14,8, 96, 48, 1); // Outline // Prioritise error from top to bottom if (General_error & (1 << LVA_ALARM)) { LCD_Display_Text(134,(const unsigned char*)Verdana14,33,14); // Battery LCD_Display_Text(119,(const unsigned char*)Verdana14,46,34); // Low } else if (Flight_flags & (1 << FailsafeFlag)) { LCD_Display_Text(75,(const unsigned char*)Verdana14,51,13); // No LCD_Display_Text(76,(const unsigned char*)Verdana14,39,33); // Signal } else if (General_error & (1 << THROTTLE_HIGH)) { LCD_Display_Text(105,(const unsigned char*)Verdana14,28,14); // Throttle LCD_Display_Text(121,(const unsigned char*)Verdana14,46,34); // High } } // Write buffer to complete write_buffer(buffer); clear_buffer(buffer); }
void Display_status(void) { int16_t temp; uint16_t vbat_temp; int8_t pos1, pos2, pos3; mugui_size16_t size; clear_buffer(buffer); // Display text LCD_Display_Text(3,(const unsigned char*)Verdana8,0,0); // Version text LCD_Display_Text(5,(const unsigned char*)Verdana8,0,16); // RX sync LCD_Display_Text(6,(const unsigned char*)Verdana8,0,27); // Profile LCD_Display_Text(23,(const unsigned char*)Verdana8,88,27); // Pos LCD_Display_Text(133,(const unsigned char*)Verdana8,0,38); // Battery // Display menu and markers LCD_Display_Text(9, (const unsigned char*)Wingdings, 0, 59); // Down LCD_Display_Text(14,(const unsigned char*)Verdana8,10,55); // Menu // Display values print_menu_text(0, 1, (62 + Config.RxMode), 45, 16); // Rx mode mugui_lcd_puts(itoa(transition,pBuffer,10),(const unsigned char*)Verdana8,110,27); // Raw transition value if (Config.RxMode == PWM) { LCD_Display_Text(24,(const unsigned char*)Verdana8,77,38); // Interrupt counter text mugui_lcd_puts(itoa(InterruptCount,pBuffer,10),(const unsigned char*)Verdana8,110,38); // Interrupt counter } // Display transition point if (transition <= 0) { LCD_Display_Text(48,(const unsigned char*)Verdana8,45,27); } else if (transition >= 100) { LCD_Display_Text(50,(const unsigned char*)Verdana8,45,27); } else if (transition == Config.Transition_P1n) { LCD_Display_Text(49,(const unsigned char*)Verdana8,45,27); } else if (transition < Config.Transition_P1n) { LCD_Display_Text(51,(const unsigned char*)Verdana8,45,27); } else if (transition > Config.Transition_P1n) { LCD_Display_Text(52,(const unsigned char*)Verdana8,45,27); } // Display voltage uint8_t x_loc = 45; // X location of voltage display uint8_t y_loc = 38; // Y location of voltage display vbat_temp = GetVbat(); temp = vbat_temp/100; // Display whole decimal part first mugui_text_sizestring(itoa(temp,pBuffer,10), (const unsigned char*)Verdana8, &size); mugui_lcd_puts(itoa(temp,pBuffer,10),(const unsigned char*)Verdana8,x_loc,y_loc); pos1 = size.x; vbat_temp = vbat_temp - (temp * 100); // Now display the parts to the right of the decimal point LCD_Display_Text(7,(const unsigned char*)Verdana8,(x_loc + pos1),y_loc); mugui_text_sizestring(".", (const unsigned char*)Verdana8, &size); pos3 = size.x; mugui_text_sizestring("0", (const unsigned char*)Verdana8, &size); pos2 = size.x; if (vbat_temp >= 10) { mugui_lcd_puts(itoa(vbat_temp,pBuffer,10),(const unsigned char*)Verdana8,(x_loc + pos1 + pos3),y_loc); } else { LCD_Display_Text(8,(const unsigned char*)Verdana8,(x_loc + pos1 + pos3),y_loc); mugui_lcd_puts(itoa(vbat_temp,pBuffer,10),(const unsigned char*)Verdana8,(x_loc + pos1 + pos2 + pos3),y_loc); } // Display error messages if (General_error != 0) { // Create message box fillrect(buffer, 14,8, 96, 48, 0); // White box drawrect(buffer, 14,8, 96, 48, 1); // Outline // Prioritise error from top to bottom if((General_error & (1 << LVA_ALARM)) != 0) { LCD_Display_Text(134,(const unsigned char*)Verdana14,33,14); // Battery LCD_Display_Text(73,(const unsigned char*)Verdana14,46,34); // Low } else if((General_error & (1 << NO_SIGNAL)) != 0) { LCD_Display_Text(75,(const unsigned char*)Verdana14,51,13); // No LCD_Display_Text(76,(const unsigned char*)Verdana14,39,33); // Signal } else if((General_error & (1 << THROTTLE_HIGH)) != 0) { LCD_Display_Text(105,(const unsigned char*)Verdana14,28,14); // Throttle LCD_Display_Text(55,(const unsigned char*)Verdana14,46,34); // High } else if((General_error & (1 << DISARMED)) != 0) { LCD_Display_Text(18,(const unsigned char*)Verdana14,25,24); // Disarmed } } // Write buffer to complete write_buffer(buffer,1); clear_buffer(buffer); }
void Display_status(void) { int16_t temp; uint16_t vbat_temp; int8_t pos1, pos2, pos3; mugui_size16_t size; clear_buffer(buffer); // Display text LCD_Display_Text(264,(const unsigned char*)Verdana8,0,0); // Version text LCD_Display_Text(266,(const unsigned char*)Verdana8,0,12); // RX sync LCD_Display_Text(267,(const unsigned char*)Verdana8,0,24); // Profile LCD_Display_Text(23,(const unsigned char*)Verdana8,88,24); // Pos // Display menu and markers LCD_Display_Text(9, (const unsigned char*)Wingdings, 0, 59);// Down LCD_Display_Text(14,(const unsigned char*)Verdana8,10,55); // Menu // Display values print_menu_text(0, 1, (62 + Config.RxMode), 45, 12); // Rx mode mugui_lcd_puts(itoa(transition,pBuffer,10),(const unsigned char*)Verdana8,110,24); // Raw transition value if (Config.RxMode == PWM) { LCD_Display_Text(24,(const unsigned char*)Verdana8,77,12); // Interrupt counter text mugui_lcd_puts(itoa(InterruptCount,pBuffer,10),(const unsigned char*)Verdana8,110,12); // Interrupt counter } // Display transition point if (transition <= 0) { LCD_Display_Text(48,(const unsigned char*)Verdana8,45,24); } else if (transition >= 100) { LCD_Display_Text(50,(const unsigned char*)Verdana8,45,24); } else if (transition == Config.Transition_P1n) { LCD_Display_Text(49,(const unsigned char*)Verdana8,45,24); } else if (transition < Config.Transition_P1n) { LCD_Display_Text(51,(const unsigned char*)Verdana8,45,24); } else { LCD_Display_Text(52,(const unsigned char*)Verdana8,45,24); } // Don't display battery text if there are error messages if (General_error == 0) { // Display voltage uint8_t x_loc = 45; // X location of voltage display uint8_t y_loc = 36; // Y location of voltage display LCD_Display_Text(289,(const unsigned char*)Verdana8,0,36); // Battery vbat_temp = GetVbat(); temp = vbat_temp/100; // Display whole decimal part first mugui_text_sizestring(itoa(temp,pBuffer,10), (const unsigned char*)Verdana8, &size); mugui_lcd_puts(itoa(temp,pBuffer,10),(const unsigned char*)Verdana8,x_loc,y_loc); pos1 = size.x; vbat_temp = vbat_temp - (temp * 100); // Now display the parts to the right of the decimal point LCD_Display_Text(268,(const unsigned char*)Verdana8,(x_loc + pos1),y_loc); mugui_text_sizestring(".", (const unsigned char*)Verdana8, &size); pos3 = size.x; mugui_text_sizestring("0", (const unsigned char*)Verdana8, &size); pos2 = size.x; if (vbat_temp >= 10) { mugui_lcd_puts(itoa(vbat_temp,pBuffer,10),(const unsigned char*)Verdana8,(x_loc + pos1 + pos3),y_loc); } else { LCD_Display_Text(269,(const unsigned char*)Verdana8,(x_loc + pos1 + pos3),y_loc); mugui_lcd_puts(itoa(vbat_temp,pBuffer,10),(const unsigned char*)Verdana8,(x_loc + pos1 + pos2 + pos3),y_loc); } // Display vibration info is set to "ON" if (Config.Vibration == ON) { // Create message box fillrect(buffer, 29,11, 70, 42, 0); // White box drawrect(buffer, 29,11, 70, 42, 1); // Outline // Display vibration data temp = (int16_t)GyroAvgNoise; // Work out pixel size of number to display mugui_text_sizestring(itoa(temp,pBuffer,10), (const unsigned char*)Verdana22, &size); // Center the number in the box automatically mugui_lcd_puts(itoa(temp,pBuffer,10),(const unsigned char*)Verdana22,64 - (size.x / 2),20); } // if (Config.Vibration == ON) } // Display error messages else { // Prioritise error from top to bottom if(General_error & (1 << LVA_ALARM)) { LCD_Display_Text(134,(const unsigned char*)Verdana14,15,37); // Battery LCD_Display_Text(271,(const unsigned char*)Verdana14,79,37); // low } else if(General_error & (1 << NO_SIGNAL)) { LCD_Display_Text(75,(const unsigned char*)Verdana14,30,37); // No LCD_Display_Text(272,(const unsigned char*)Verdana14,55,37); // signal } else if(General_error & (1 << THROTTLE_HIGH)) { LCD_Display_Text(105,(const unsigned char*)Verdana14,11,37); // Throttle LCD_Display_Text(270,(const unsigned char*)Verdana14,82,37); // high } else if(General_error & (1 << DISARMED)) { LCD_Display_Text(18,(const unsigned char*)Verdana14,25,37); // Disarmed } } // Write buffer to complete write_buffer(buffer); clear_buffer(buffer); }
void Display_status(void) { int16_t temp, min, max, range, scale; int8_t pos1, pos2, pos3; mugui_size16_t size; clear_buffer(buffer); // Display text LCD_Display_Text(4,(prog_uchar*)Verdana8,0,0); // Mode LCD_Display_Text(3,(prog_uchar*)Verdana8,0,11); // Version text LCD_Display_Text(5,(prog_uchar*)Verdana8,0,22); // Input LCD_Display_Text(46,(prog_uchar*)Verdana8,0,33); // Stability LCD_Display_Text(47,(prog_uchar*)Verdana8,0,44); // Autolevel // Display menu and markers LCD_Display_Text(9, (prog_uchar*)Wingdings, 0, 59); // Down LCD_Display_Text(14,(prog_uchar*)Verdana8,10,55); // Menu // Display values print_menu_text(0, 1, (18 + Config.RxMode), 50, 22); LCD_Display_Text(0,(prog_uchar*)Verdana8,50,11); print_menu_text(0, 1, (22 + Config.MixMode), 33, 0); print_menu_text(0, 1, (101 + Stability), 50, 44); print_menu_text(0, 1, (101 + AutoLevel), 50, 33); // Draw battery drawrect(buffer, 100,4, 28, 50, 1); // Battery body drawrect(buffer, 110,0, 8, 4, 1); // Battery terminal GetVbat(); min = Config.MinVoltage * Config.BatteryCells; // Calculate battery voltage limits max = Config.MaxVoltage * Config.BatteryCells; range = max - min; scale = range / 50; if (vBat >= min) { temp =(vBat - min) / scale; } else { temp = 0; } if (temp <= 0) temp = 0; if (temp > 50) temp = 50; fillrect(buffer, 100,54-temp, 28, temp, 1); // Battery filler (max is 60) // Display voltage uint8_t x_loc = 102; // X location of voltage display uint8_t y_loc = 55; // Y location of voltage display temp = vBat/100; // Display whole decimal part first mugui_text_sizestring(itoa(temp,pBuffer,10), (prog_uchar*)Verdana8, &size); mugui_lcd_puts(itoa(temp,pBuffer,10),(prog_uchar*)Verdana8,x_loc,y_loc); pos1 = size.x; vBat = vBat - (temp * 100); // Now display the parts to the right of the decimal point LCD_Display_Text(7,(prog_uchar*)Verdana8,(x_loc + pos1),y_loc); mugui_text_sizestring(".", (prog_uchar*)Verdana8, &size); pos3 = size.x; mugui_text_sizestring("0", (prog_uchar*)Verdana8, &size); pos2 = size.x; if (vBat >= 10) { mugui_lcd_puts(itoa(vBat,pBuffer,10),(prog_uchar*)Verdana8,(x_loc + pos1 + pos3),y_loc); } else { LCD_Display_Text(8,(prog_uchar*)Verdana8,(x_loc + pos1 + pos3),y_loc); mugui_lcd_puts(itoa(vBat,pBuffer,10),(prog_uchar*)Verdana8,(x_loc + pos1 + pos2 + pos3),y_loc); } // Draw error messages, if any if (General_error != 0) { // Create message box fillrect(buffer, 14,8, 96, 48, 0); // White box drawrect(buffer, 14,8, 96, 48, 1); // Outline // Prioritise error from top to bottom if((General_error & (1 << SENSOR_ERROR)) != 0) { LCD_Display_Text(72,(prog_uchar*)Verdana14,35,14); // Sensor LCD_Display_Text(98,(prog_uchar*)Verdana14,43,34); // Error menu_beep(9); } else if((General_error & (1 << LOW_BATT)) != 0) { LCD_Display_Text(82,(prog_uchar*)Verdana14,33,14); // Battery LCD_Display_Text(119,(prog_uchar*)Verdana14,46,34); // Low } else if((General_error & (1 << NO_SIGNAL)) != 0) { LCD_Display_Text(75,(prog_uchar*)Verdana14,51,13); // No LCD_Display_Text(76,(prog_uchar*)Verdana14,39,33); // Signal menu_beep(3); } else if((General_error & (1 << LOST_MODEL)) != 0) { LCD_Display_Text(99,(prog_uchar*)Verdana14,45,14); // Lost LCD_Display_Text(100,(prog_uchar*)Verdana14,40,34);// Model } else if((General_error & (1 << THROTTLE_HIGH)) != 0) { LCD_Display_Text(105,(prog_uchar*)Verdana14,28,14); // Throttle LCD_Display_Text(120,(prog_uchar*)Verdana14,46,34); // High menu_beep(6); } } // Write buffer to complete write_buffer(buffer,1); clear_buffer(buffer); }
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()