void st7565_init(void) {
// set pin directions
SID_DDR |= _BV(SID);
SCLK_DDR |= _BV(SCLK);
A0_DDR |= _BV(A0);
RST_DDR |= _BV(RST);
CS_DDR |= _BV(CS);
// toggle RST low to reset; CS low so it'll listen to us
CS_PORT &= ~_BV(CS);
RST_PORT &= ~_BV(RST);
_delay_ms(500);
RST_PORT |= _BV(RST);
// LCD bias select
st7565_command(CMD_SET_BIAS_7);
// ADC select
st7565_command(CMD_SET_ADC_NORMAL);
// SHL select
st7565_command(CMD_SET_COM_NORMAL);
// black text on white background
st7565_command(CMD_SET_DISP_NORMAL);
// Initial display line
st7565_command(CMD_SET_DISP_START_LINE);
// turn on voltage converter (VC=1, VR=0, VF=0)
st7565_command(CMD_SET_POWER_CONTROL | 0x4);
_delay_ms(50);
// turn on voltage regulator (VC=1, VR=1, VF=0)
st7565_command(CMD_SET_POWER_CONTROL | 0x6);
_delay_ms(50);
// turn on voltage follower (VC=1, VR=1, VF=1)
st7565_command(CMD_SET_POWER_CONTROL | 0x7);
_delay_ms(10);
// set lcd operating voltage (regulator resistor, ref voltage resistor)
st7565_command(CMD_SET_RESISTOR_RATIO | 0x2);
//set brightness
st7565_set_brightness(0x15);
}
void st7565_init(void)
{
DispSelect(DISP_SELECT); // assert /CS1
DispReset(DISP_RESET);
Delay(50);
DispReset(DISP_NO_RESET);
Delay(50);
// LCD bias select
st7565_command(CMD_SET_BIAS_9);
// ADC select
st7565_command(CMD_SET_ADC_NORMAL); // do not flip x. x reference from left.
// SHL select
st7565_command(CMD_SET_COM_NORMAL); // do not flip y. y referenced from bottom
// Initial display line
st7565_command(CMD_SET_DISP_START_LINE); // start at line 0
// turn on voltage converter (VC=1, VR=0, VF=0)
st7565_command(CMD_SET_POWER_CONTROL | 0x4);
Delay(50);
// turn on voltage regulator (VC=1, VR=1, VF=0)
st7565_command(CMD_SET_POWER_CONTROL | 0x6);
Delay(50);
// turn on voltage follower (VC=1, VR=1, VF=1)
st7565_command(CMD_SET_POWER_CONTROL | 0x7);
Delay(50);
// set lcd operating voltage (regulator resistor, ref voltage resistor)
// with a 3.3 VDD we are getting approx 9v on VOUT with ratio = 001
st7565_command(CMD_SET_RESISTOR_RATIO | 0x1);
st7565_set_brightness(26); // 0 - 63
st7565_command(CMD_SET_ALLPTS_NORMAL);
st7565_command(CMD_DISPLAY_ON);
}
void do_menu_item(uint8_t menuitem, int8_t *values, uint8_t mult, menu_range_t range, int8_t offset, uint8_t text_link, bool servo_enable, int16_t servo_number)
{
mugui_size16_t size;
int16_t temp16;
int8_t i;
int16_t value = (int8_t)*values;
uint8_t display_update = 0;
uint8_t servo_update = 0;
uint8_t button_update = 0;
uint8_t button_inc = 0;
bool button_lock = false;
bool first_time = true;
// Multiply value for display only if style is 2
if (range.style == 2)
{
value = value * mult;
}
else mult = 1;
button = NONE;
// Reset servo to neutral unless it is for the throttle channel in CPPM mode
if (servo_enable && !((Config.Channel[servo_number].source_a == THROTTLE) && (Config.RxMode == CPPM_MODE)))
{
temp16 = Config.Limits[servo_number].trim;
temp16 = ((temp16 << 2) / 10); // Span back to what the output wants
// Give servos time to settle
for (i = 0; i < 25; i++)
{
cli();
output_servo_ppm_asm3(servo_number, temp16);
sei();
_delay_ms(10);
}
}
// This is a loop that cycles until Button 4 is pressed (Save)
// The GLCD updating slows servo updates down too much so only update the GLCD periodically
// When not updating the GLCD the servo should be updated at 50Hz (20ms)
while (button != ENTER)
{
// Increment loopcount so that we can time various things
display_update++;
servo_update++;
// Vary the button increment delay depending on the function
if (servo_enable)
{
button_inc = 20; // For servos
}
else
{
button_inc = 1; // For everything else
}
// Increment button timer when pressed
if (button != NONE)
{
button_update++;
// Release button lock after button_inc loops
if (button_update > button_inc)
{
button_lock = false;
button_update = 0;
}
}
// Remove lock when not pressed
else
{
button_update = 0;
button_lock = false;
}
// Display update
if (!servo_enable || // Non-servo value or
((display_update >= 32) && (button != NONE)) || // Servo value and 32 cycles passed but only with a button pressed or...
(first_time)) // First time into routine
{
display_update = 0;
first_time = false;
clear_buffer(buffer);
// Print title
gLCDprint_Menu_P((char*)pgm_read_word(&text_menu[menuitem]), (prog_uchar*)Verdana14, 0, 0);
// Print value
if ((range.style == 0) || (range.style == 2)) // numeric and numeric * 4
{
// Write numeric value, centered on screen
mugui_text_sizestring(itoa(value,pBuffer,10), (prog_uchar*)Verdana14, &size);
mugui_lcd_puts(itoa(value,pBuffer,10),(prog_uchar*)Verdana14,((128-size.x)/2)+offset,25);
}
else // text
{
// Write text, centered on screen
pgm_mugui_scopy((char*)pgm_read_word(&text_menu[text_link + value])); // Copy string to pBuffer
mugui_text_sizestring((char*)pBuffer, (prog_uchar*)Verdana14, &size);
LCD_Display_Text(text_link + value, (prog_uchar*)Verdana14,((128-size.x)/2),25);
}
// Print bottom markers
print_menu_frame(1);
// Write from buffer
write_buffer(buffer,1);
}
// Poll buttons when idle
// Don't use button acceleration when moving servos
// And don't block the code with poll_buttons()
if (servo_enable)
{
button = (PINB & 0xf0);
button_multiplier = 1;
}
else
{
poll_buttons(true);
}
// Handle cursor Up/Down limits
if (button == DOWN)
{
if (button_lock == false)
{
button_lock = true;
value = value - (range.increment * button_multiplier);
button_update = 0;
}
}
if (button == UP)
{
if (button_lock == false)
{
button_lock = true;
value = value + (range.increment * button_multiplier);
button_update = 0;
}
}
if (button == BACK)
{
value = (range.default_value * mult);
}
// Limit values to set ranges
if (value < (range.lower * mult))
{
value = range.lower * mult;
}
if (value > (range.upper * mult))
{
value = range.upper * mult;
}
// Update contrast setting
if (menuitem == CONTRAST)
{
st7565_set_brightness(value); // debug
}
// Set servo position if required and update every 4 * 5ms = 20ms
if ((servo_enable) && (servo_update >= 4))
{
servo_update = 0;
temp16 = scale_percent(value); // Convert to servo position (from %)
temp16 = (((temp16 << 2) + (int16_t)5) / (int16_t)10); // Span back to what the output wants
cli();
output_servo_ppm_asm3(servo_number, temp16);
sei();
}
// Loop rate = 5ms (200Hz)
_delay_ms(5);
} // while (button != ENTER)
// Exit
button = ENTER;
// Divide value from that displayed if style = 2
if (range.style == 2)
{
value = value / mult;
}
*values = (int8_t)value;
}
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 ST7565::begin(uint8_t contrast) {
st7565_init();
st7565_command(CMD_DISPLAY_ON);
st7565_command(CMD_SET_ALLPTS_NORMAL);
st7565_set_brightness(contrast);
}
void setup(void) {
//Serial.begin(9600);
// uart_init(BRRL_192);
//// turn on backlight
//BLA_DDR |= _BV(BLA);
//BLA_PORT |= _BV(BLA);
LED_DDR |= _BV(LED);
// Serial.println("init");
putstring_nl("init!");
st7565_init();
/*
while (1) {
spiwrite(0x55);
_delay_ms(100);
}
*/
//Serial.println("on");
putstring_nl("on");
st7565_command(CMD_DISPLAY_ON);
//Serial.println("normal");
putstring_nl("normal");
st7565_command(CMD_SET_ALLPTS_NORMAL);
//Serial.println("bright");
putstring_nl("bright");
st7565_set_brightness(0x18);
//Serial.println("clear");
putstring_nl("clear");
clear_screen();
//Serial.println("blit");
putstring_nl("blit");
write_buffer(buffer);
//putstring_nl("done");
//_delay_ms(250);
//clear_buffer(buffer);
//uint8_t i, j = 0;
//for(j=0; j<32; j+=8){
// for(i=0; i<128; i++){
// setpixel(buffer, i, j, 1) ;
// }
// }
//write_buffer(buffer);
//LED_PORT |= _BV(LED);
//drawrect(buffer, 10, 10, 2, 2, 1);
// testdrawrect(buffer);
// testfillrect(buffer);
// testdrawline(buffer);
//testdrawcircle(buffer);
//fillcircle(buffer, 32, 32, 10, 1);
//testdrawchar(buffer);
// drawstring(buffer, 0, 0, "Lorem ipsum dolor sit amet, consectetur adipisicing elit, sed do eiusmod tempor incididunt ut labore et dolore magna aliqua. Ut enim ad minim veniam, quis nostrud exercitation ullamco laboris nisi ut aliquip ex ea commodo consequat. Duis aute irure dolor in reprehenderit in voluptate velit esse cillum dolore eu fugiat nulla pariatur. Excepteur sint occaecat cupidatat non proident, sunt in culpa qui officia deserunt mollit anim id est laborum.");
////testdrawbitmap(buffer, logo_glcd_bmp, 16, 16);
// turn on backlight
BLA_DDR |= _BV(BLA);
BLA_PORT |= _BV(BLA);
//LED_PORT &= ~_BV(LED);
//write_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()
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 LCDST7565::updateValue()
{
switch(currentMenu)
{
case MENU_SYNTH_GAIN:
{
synth->setMasterGain(cSynthVal.fvalue / 100.f);
break;
}
case MENU_SYNTH_BASEFREQ:
{
synth->setBaseFrequency(cSynthVal.fvalue);
break;
}
case MENU_SYNTH_SEMIRANGE:
{
synth->setSemiRange((uint8_t) cSynthVal.value);
break;
}
case MENU_SYNTH_OSC_AMPLITUDE:
{
synth->setOSCAmplitude(selectedPartIndex, cSynthVal.fvalue / 100.f);
break;
}
case MENU_SYNTH_OSC_SEMI:
{
synth->setOSCSemitones(selectedPartIndex, cSynthVal.value);
break;
}
case MENU_SYNTH_OSC_CENT:
{
synth->setOSCCents(selectedPartIndex, cSynthVal.value);
break;
}
case MENU_SYNTH_OSC_DUTY:
{
synth->setOSCDutycycle(selectedPartIndex, cSynthVal.fvalue);
break;
}
case MENU_SYNTH_FILTER1_FREQ:
{
synth->setFilter1Freq(cSynthVal.fvalue);
break;
}
case MENU_SYNTH_FILTER1_RESO:
{
synth->setFilter1Reso(cSynthVal.fvalue);
break;
}
case MENU_DISP_CONTRAST:
{
st7565_set_brightness(cSynthVal.value * 63 / 100);
break;
}
case MENU_MIDI_CC_PITCH:
{
midi->setCC( CT_PITCH, cSynthVal.value );
break;
}
case MENU_MIDI_CC_VOL:
{
midi->setCC( CT_VOLUME, cSynthVal.value );
break;
}
case MENU_MIDI_NOTE:
{
midi->setBaseNote( cSynthVal.value );
break;
}
case MENU_MIDI_CHANNEL:
{
midi->setChannel( cSynthVal.value );
break;
}
case MENU_MIDI_VELOCITY:
{
midi->setVelocity( cSynthVal.value );
break;
}
}
}