// Pop up the Idle screen
void idle_screen(void)
{
clear_buffer();
LCD_Display_Text(121,(prog_uchar*)Verdana14,40,12); // "Press"
LCD_Display_Text(122,(prog_uchar*)Verdana14,24,32); // "for status"
write_buffer();
};
// Pop up the Idle screen
void idle_screen(void)
{
clear_buffer(buffer);
// Change Status screen depending on arm mode
LCD_Display_Text(122,(const unsigned char*)Verdana14,40,10); // "Press"
LCD_Display_Text(123,(const unsigned char*)Verdana14,24,30); // "for status"
write_buffer(buffer);
}
// Pop up the Idle screen
void idle_screen(void)
{
clear_buffer(buffer);
// Change Status screen depending on arm mode
LCD_Display_Text(121,(const unsigned char*)Verdana14,41,3); // "Press"
LCD_Display_Text(122,(const unsigned char*)Verdana14,24,23); // "for status."
if ((General_error & (1 << DISARMED)) != 0) // Disarmed
{
LCD_Display_Text(139,(const unsigned char*)Verdana14,20,43); // "(Disarmed)"
}
else
{
LCD_Display_Text(138,(const unsigned char*)Verdana14,28,43); // "(Armed)"
}
write_buffer(buffer,1);
};
void menu_main(void)
{
static uint8_t cursor = LINE0; // These are now static so as to remember the menu position
static uint8_t top = MAINSTART;
static uint8_t temp = 0;
button = NONE;
// Wait until user's finger is off button 1
while(BUTTON1 == 0)
{
_delay_ms(50);
}
while(button != BACK)
{
// Clear buffer before each update
clear_buffer(buffer);
// Print menu
print_menu_frame(0); // Frame
LCD_Display_Text(top,(prog_uchar*)Verdana8,ITEMOFFSET,LINE0); // First line
LCD_Display_Text(top+1,(prog_uchar*)Verdana8,ITEMOFFSET,LINE1); // Second line
LCD_Display_Text(top+2,(prog_uchar*)Verdana8,ITEMOFFSET,LINE2); // Third line
LCD_Display_Text(top+3,(prog_uchar*)Verdana8,ITEMOFFSET,LINE3); // Fourth line
print_cursor(cursor); // Cursor
write_buffer(buffer,1);
// Poll buttons when idle
poll_buttons();
// Handle menu changes
update_menu(MAINITEMS, MAINSTART, button, &cursor, &top, &temp);
if (button == ENTER)
{
do_main_menu_item(temp);
button = NONE;
}
}
menu_beep(1);
_delay_ms(200);
}
void print_menu_text(int16_t values, uint8_t style, uint8_t text_link, uint8_t x, uint8_t y)
{
if ((style == 0) || (style == 2)) // Numeral
{
mugui_lcd_puts(itoa(values,pBuffer,10),(prog_uchar*)Verdana8,x,y);
}
else if (style == 1) // Text
{
LCD_Display_Text(text_link, (prog_uchar*)Verdana8,x,y);
}
}
void menu_main(void)
{
static uint8_t main_cursor = LINE0; // These are now static so as to remember the main menu position
static uint8_t main_temp = 0;
static uint8_t old_menu = 0;
button = NONE;
// Wait until user's finger is off button 1
Wait_BUTTON1();
while(button != BACK)
{
// Clear buffer before each update
clear_buffer(buffer);
// Print menu
print_menu_frame(0); // Frame
for (uint8_t i = 0; i < 4; i++)
{
LCD_Display_Text(main_top+i,(const unsigned char*)Verdana8,ITEMOFFSET,(uint8_t)pgm_read_byte(&lines[i])); // Lines
}
print_cursor(main_cursor); // Cursor
write_buffer(buffer,1);
// Poll buttons when idle
poll_buttons(true);
// Handle menu changes
update_menu(MAINITEMS, MAINSTART, 0, button, &main_cursor, &main_top, &main_temp);
// If main menu item has changed, reset sub-menu positions
// and flag to sub-menus that positions need to be reset
if (main_temp != old_menu)
{
cursor = LINE0;
menu_temp = 0;
old_menu = main_temp;
menu_flag = 1;
}
// If ENTER pressed, jump to menu
if (button == ENTER)
{
do_main_menu_item(main_temp);
button = NONE;
// Wait until user's finger is off button 1
Wait_BUTTON1();
}
}
}
// Pop up the Idle screen
void idle_screen(void)
{
clear_buffer(buffer);
// Change Status screen depending on arm mode
LCD_Display_Text(121,(const unsigned char*)Verdana14,41,3); // "Press"
LCD_Display_Text(122,(const unsigned char*)Verdana14,24,23); // "for status."
// Display most important error
if ((General_error & (1 << LVA_ALARM)) != 0) // Low voltage
{
LCD_Display_Text(134,(const unsigned char*)Verdana14,12,43);// "Battery"
LCD_Display_Text(118,(const unsigned char*)Verdana14,80,43); // "Low"
}
else if ((General_error & (1 << NO_SIGNAL)) != 0) // No signal
{
LCD_Display_Text(75,(const unsigned char*)Verdana14,28,43); // "No"
LCD_Display_Text(76,(const unsigned char*)Verdana14,54,43); // "Signal"
}
else if ((General_error & (1 << THROTTLE_HIGH)) != 0) // Throttle high
{
LCD_Display_Text(105,(const unsigned char*)Verdana14,10,43);// "Throttle"
LCD_Display_Text(55,(const unsigned char*)Verdana14,81,43); // "High"
}
else if ((General_error & (1 << DISARMED)) != 0) // Disarmed
{
LCD_Display_Text(139,(const unsigned char*)Verdana14,20,43);// "(Disarmed)"
}
else
{
LCD_Display_Text(138,(const unsigned char*)Verdana14,28,43);// "(Armed)"
}
write_buffer(buffer);
}
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);
}
}
//************************************************************
// Print basic menu frame
// style = menu style (0 = main, 1 = sub)
//************************************************************
void print_menu_frame(uint8_t style)
{
// Print bottom markers
if (style == 0)
{
LCD_Display_Text(12, (prog_uchar*)Wingdings, 0, 57); // Left
LCD_Display_Text(10, (prog_uchar*)Wingdings, 38, 59); // Up
LCD_Display_Text(9, (prog_uchar*)Wingdings, 80, 59); // Down
LCD_Display_Text(11, (prog_uchar*)Wingdings, 120, 57); // Right
}
else
{
LCD_Display_Text(16, (prog_uchar*)Verdana8, 0, 54); // Clear
LCD_Display_Text(10, (prog_uchar*)Wingdings, 38, 59); // Up
LCD_Display_Text(9, (prog_uchar*)Wingdings, 80, 59); // Down
LCD_Display_Text(17, (prog_uchar*)Verdana8, 103, 54); // Save
}
// Write from buffer
write_buffer(buffer,1);
}
//**********************************************************************
// Print menu items primary subroutine
//
// Usage:
// top = position in submenu list
// start = start of submenu text list. (top - start) gives the offset into the list.
// values = pointer to array of values to change
// multiplier = display/actual if type = 2, otherwise defaults to 1
// menu_ranges = pointer to array of min/max/inc/style/defaults
// rangetype = unique (0) all values are different, copied (1) all values are the same
// MenuOffsets = originally an array, now just a fixed horizontal offset for the value text
// text_link = pointer to the text list for the values if not numeric
// cursor = cursor position
//**********************************************************************
void print_menu_items(uint8_t top, uint8_t start, int8_t values[], uint8_t mult, prog_uchar* menu_ranges, uint8_t rangetype, uint8_t MenuOffsets, prog_uchar* text_link, uint8_t cursor)
{
menu_range_t range1;
uint8_t multiplier;
// Clear buffer before each update
clear_buffer(buffer);
print_menu_frame(0);
// Print each line
for (uint8_t i = 0; i < 4; i++)
{
LCD_Display_Text(top+i,(prog_uchar*)Verdana8,ITEMOFFSET,(uint8_t)pgm_read_byte(&lines[i]));
// Handle unique or copied ranges (to reduce space)
if (rangetype == 0)
{
// Use each unique entry
memcpy_P(&range1, &menu_ranges[(top+i - start)* sizeof(range1)], sizeof(range1));
}
else
{
// Use just the first entry in array for all
memcpy_P(&range1, &menu_ranges[0], sizeof(range1));
}
if (range1.style == 2)
{
multiplier = mult;
}
else
{
multiplier = 1;
}
print_menu_text((values[top+i - start] * multiplier), range1.style, (pgm_read_byte(&text_link[top+i - start]) + values[top+i - start]), MenuOffsets, (uint8_t)pgm_read_byte(&lines[i]));
}
print_cursor(cursor); // Cursor
write_buffer(buffer,1);
poll_buttons(true);
}
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;
}
}
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 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
// 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 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 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_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 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_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 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 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 print_cursor(uint8_t line)
{
LCD_Display_Text(13, (prog_uchar*)Wingdings, CURSOROFFSET, line);
}
void Display_balance(void)
{
int16_t x_pos, y_pos;
int8_t roll_axis, pitch_axis;
while(BUTTON1 != 0)
{
ReadAcc();
// Refresh accSmooth values
// Note that because it takes 4.096ms to refresh the whole GLCD this loop cannot run
// faster than 244Hz, but that's close enough to the actual loop time so that the
// actual Acc LPF effect is closely mirrored on the balance meter.
getEstimatedAttitude();
// HORIZONTAL: Pitch = X, Roll = Y
// UPSIDEDOWN: Pitch = X, Roll = Y
// AFT: Pitch = X, Roll = Y
// VERTICAL: Pitch = Y, Roll = X
// SIDEWAYS: Pitch = Y, Roll = X
if ((Config.Orientation == VERTICAL) || (Config.Orientation == SIDEWAYS))
{
roll_axis = PITCH;
pitch_axis = ROLL;
}
else
{
roll_axis = ROLL;
pitch_axis = PITCH;
}
// We need to reverse the polarity reversal so that the meter is once again
// related to the KK2.0, not the model.
// For some reason, pitch has to be reversed on he KK2.1
#ifdef KK21
x_pos = ((int8_t)pgm_read_byte(&Acc_Pol[Config.Orientation][pitch_axis]) * -accSmooth[pitch_axis]) + 32;
#else
x_pos = ((int8_t)pgm_read_byte(&Acc_Pol[Config.Orientation][pitch_axis]) * accSmooth[pitch_axis]) + 32;
#endif
y_pos = ((int8_t)pgm_read_byte(&Acc_Pol[Config.Orientation][roll_axis]) * accSmooth[roll_axis]) + 64;
if (x_pos < 0) x_pos = 0;
if (x_pos > 64) x_pos = 64;
if (y_pos < 0) y_pos = 0;
if (y_pos > 128) y_pos = 128;
// Print bottom markers
LCD_Display_Text(12, (prog_uchar*)Wingdings, 2, 55); // Left
// Draw balance meter
drawrect(buffer, 0, 0, 128, 64, 1); // Border
drawrect(buffer, 54, 22, 21, 21, 1); // Target
drawline(buffer, 64, 8, 64, 56, 1); // Crosshairs
drawline(buffer, 32, 32, 96, 32, 1);
fillcircle(buffer, y_pos, x_pos, 8, 1); // Bubble
// Refresh GLCD
write_buffer(buffer,1);
clear_buffer(buffer);
}
}
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_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);
}
}
