void CalibrateGyrosSlow(void)
{
uint8_t axis;
uint8_t Gyro_seconds = 0;
uint8_t Gyro_TCNT2 = 0;
uint16_t Gyro_timeout = 0;
bool Gyros_Stable = false;
float GyroSmooth[NUMBEROFAXIS];
// Force recalculation
for (axis = 0; axis < NUMBEROFAXIS; axis++)
{
// Optimise starting point for each board
#ifdef KK21
GyroSmooth[axis] = 0;
#else
GyroSmooth[axis] = 500;
#endif
}
// Wait until gyros stable. Timeout after 5 seconds
while (!Gyros_Stable && (Gyro_seconds <= 5))
{
// Update status timeout
Gyro_timeout += (uint8_t) (TCNT2 - Gyro_TCNT2);
Gyro_TCNT2 = TCNT2;
// Count elapsed seconds
if (Gyro_timeout > SECOND_TIMER)
{
Gyro_seconds++;
Gyro_timeout = 0;
}
get_raw_gyros();
// Calculate very long rolling average
for (axis = 0; axis < NUMBEROFAXIS; axis++)
{
GyroSmooth[axis] = ((GyroSmooth[axis] * (float)999) + (float)(gyroADC[axis])) / (float)1000;
Config.gyroZero[axis] = (int16_t)GyroSmooth[axis];
}
// Check for movement
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))
{
Gyros_Stable = false;
}
else
{
Gyros_Stable = true;
}
}
}
void Display_sensors(void)
{
while(BUTTON1 != 0)
{
if (BUTTON4 == 0)
{
_delay_ms(500);
CalibrateAcc();
CalibrateGyros();
}
if (BUTTON3 == 0)
{
_delay_ms(500);
CalibrateInvAcc();
}
ReadGyros();
ReadAcc();
LCD_Display_Text(26,(prog_uchar*)Verdana8,0,0); // Gyro
LCD_Display_Text(30,(prog_uchar*)Verdana8,70,0); // Acc
LCD_Display_Text(27,(prog_uchar*)Verdana8,10,15); // X
LCD_Display_Text(28,(prog_uchar*)Verdana8,10,25); // Y
LCD_Display_Text(29,(prog_uchar*)Verdana8,10,35); // Z
mugui_lcd_puts(itoa(gyroADC[PITCH],pBuffer,10),(prog_uchar*)Verdana8,30,15);
mugui_lcd_puts(itoa(gyroADC[ROLL],pBuffer,10),(prog_uchar*)Verdana8,30,25);
mugui_lcd_puts(itoa(gyroADC[YAW],pBuffer,10),(prog_uchar*)Verdana8,30,35);
mugui_lcd_puts(itoa(accADC[PITCH],pBuffer,10),(prog_uchar*)Verdana8,80,15);
mugui_lcd_puts(itoa(accADC[ROLL],pBuffer,10),(prog_uchar*)Verdana8,80,25);
mugui_lcd_puts(itoa(accADC[YAW],pBuffer,10),(prog_uchar*)Verdana8,80,35);
// Print bottom markers
LCD_Display_Text(12, (prog_uchar*)Wingdings, 0, 57); // Left
LCD_Display_Text(157, (prog_uchar*)Verdana8, 75, 55); // Inverted Calibrate
LCD_Display_Text(60, (prog_uchar*)Verdana8, 108, 55); // Calibrate
// Update buffer
write_buffer(buffer,1);
clear_buffer(buffer);
_delay_ms(100);
}
}
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 Display_sensors(void)
{
bool first_time = true;
clear_buffer(buffer);
// While BACK not pressed
while(BUTTON1 != 0)
{
ReadGyros();
ReadAcc();
LCD_Display_Text(26,(const unsigned char*)Verdana8,37,0); // Gyro
LCD_Display_Text(30,(const unsigned char*)Verdana8,77,0); // Acc
//
LCD_Display_Text(27,(const unsigned char*)Verdana8,5,13); // Roll
LCD_Display_Text(28,(const unsigned char*)Verdana8,5,23); // Pitch
LCD_Display_Text(29,(const unsigned char*)Verdana8,5,33); // Yaw/Z
//
mugui_lcd_puts(itoa(gyroADC[ROLL],pBuffer,10),(const unsigned char*)Verdana8,40,13);
mugui_lcd_puts(itoa(gyroADC[PITCH],pBuffer,10),(const unsigned char*)Verdana8,40,23);
mugui_lcd_puts(itoa(gyroADC[YAW],pBuffer,10),(const unsigned char*)Verdana8,40,33);
mugui_lcd_puts(itoa(accADC[ROLL],pBuffer,10),(const unsigned char*)Verdana8,80,13);
mugui_lcd_puts(itoa(accADC[PITCH],pBuffer,10),(const unsigned char*)Verdana8,80,23);
mugui_lcd_puts(itoa(accADC[YAW],pBuffer,10),(const unsigned char*)Verdana8,80,33);
// Print bottom markers
LCD_Display_Text(12, (const unsigned char*)Wingdings, 0, 57); // Left
LCD_Display_Text(37, (const unsigned char*)Verdana8, 75, 55); // Inverted Calibrate
LCD_Display_Text(60, (const unsigned char*)Verdana8, 108, 55); // Calibrate
// Update buffer
write_buffer(buffer);
clear_buffer(buffer);
if (first_time)
{
// Wait until finger off button
Wait_BUTTON4();
first_time = false;
}
// Normal calibrate button pressed
if (BUTTON4 == 0)
{
// Wait until finger off button
Wait_BUTTON4();
// Pause until steady
_delay_ms(250);
// Calibrate sensors
CalibrateGyrosFast();
CalibrateAcc(NORMAL);
}
// Inverted calibrate button pressed
if (BUTTON3 == 0)
{
// Wait until button snap dissipated
_delay_ms(250);
CalibrateAcc(REVERSED);
}
}
}
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()
