void write_buffer(uint8_t *buffer) {
uint8_t c, p;
for(p = 0; p < 8; p++) {
/*
putstring("new page! ");
uart_putw_dec(p);
putstring_nl("");
*/
st7565_command(CMD_SET_PAGE | pagemap[p]);
st7565_command(CMD_SET_COLUMN_LOWER | (0x0 & 0xf));
st7565_command(CMD_SET_COLUMN_UPPER | ((0x0 >> 4) & 0xf));
st7565_command(CMD_RMW);
st7565_data(0xff);
//st7565_data(0x80);
//continue;
for(c = 0; c < 128; c++) {
//uart_putw_dec(c);
//uart_putchar(' ');
st7565_data(buffer[(128*p)+c]);
}
}
}
void setup(void) {
st7565_init();
st7565_command(CMD_DISPLAY_ON);
st7565_command(CMD_SET_ALLPTS_NORMAL);
clear_screen();
}
void write_buffer(uint8_t *buf)
{
//uint8_t c, p;
unsigned p;
uint8_t * cs;
uint8_t * ce;
for(p = 0; p < 4; p++)
{
st7565_command(CMD_SET_PAGE | p);
st7565_command(CMD_SET_COLUMN_UPPER);
st7565_command(CMD_SET_COLUMN_LOWER);
DispMode(DISP_DATA_MODE);
cs = buf + (128 * p);
ce = cs + 128;
while (cs < ce)
{
while (SSP0CON->SR.TNF == 0);
SSP0CON->DR = *cs;
++cs;
}
// for(c = 0; c < 128; c++)
// {
// st7565_data(buffer[(128*p)+c]);
// }
}
}
void ST7565::display(void) {
uint8_t col, maxcol, p;
/*
Serial.print("Refresh ("); Serial.print(xUpdateMin, DEC);
Serial.print(", "); Serial.print(xUpdateMax, DEC);
Serial.print(","); Serial.print(yUpdateMin, DEC);
Serial.print(", "); Serial.print(yUpdateMax, DEC); Serial.println(")");
*/
for(p = 0; p < 8; p++) {
/*
putstring("new page! ");
uart_putw_dec(p);
putstring_nl("");
*/
#ifdef enablePartialUpdate
// check if this page is part of update
if ( yUpdateMin >= ((p+1)*8) ) {
continue; // nope, skip it!
}
if (yUpdateMax < p*8) {
break;
}
#endif
st7565_command(CMD_SET_PAGE | pagemap[p]);
#ifdef enablePartialUpdate
col = xUpdateMin;
maxcol = xUpdateMax;
#else
// start at the beginning of the row
col = 0;
maxcol = LCDWIDTH-1;
#endif
st7565_command(CMD_SET_COLUMN_LOWER | ((col+ST7565_STARTBYTES) & 0xf));
st7565_command(CMD_SET_COLUMN_UPPER | (((col+ST7565_STARTBYTES) >> 4) & 0x0F));
st7565_command(CMD_RMW);
for(; col <= maxcol; col++) {
//uart_putw_dec(col);
//uart_putchar(' ');
st7565_data(st7565_buffer[(128*p)+col]);
}
}
#ifdef enablePartialUpdate
xUpdateMin = LCDWIDTH - 1;
xUpdateMax = 0;
yUpdateMin = LCDHEIGHT-1;
yUpdateMax = 0;
#endif
}
void clear_screen(void) {
uint8_t p, c;
for(p = 0; p < 8; p++) {
st7565_command(CMD_SET_PAGE | p);
for(c = 0; c < 129; c++) {
st7565_command(CMD_SET_COLUMN_LOWER | (c & 0xf));
st7565_command(CMD_SET_COLUMN_UPPER | ((c >> 4) & 0xf));
st7565_data(0x0);
}
}
}
void clear_screen(void)
{
uint8_t p, c;
for(p = 0; p < 4; p++)
{
st7565_command(CMD_SET_PAGE | p);
st7565_command(CMD_SET_COLUMN_UPPER);
st7565_command(CMD_SET_COLUMN_LOWER);
for(c = 0; c < 128; c++)
{
st7565_data(0x0);
}
}
}
void write_buffer(uint8_t *buffer)
{
uint8_t c, p;
for(p = 0; p < 8; p++)
{
st7565_command(CMD_SET_PAGE | pagemap[p]);
st7565_command(CMD_SET_COLUMN_LOWER | (0x0 & 0xf));
st7565_command(CMD_SET_COLUMN_UPPER | ((0x0 >> 4) & 0xf));
st7565_command(CMD_RMW);
st7565_data(0xff);
for(c = 0; c < 128; c++)
st7565_data(buffer[(128*p)+c]);
}
}
// Clear screen (does not clear buffer)
void clear_screen(void)
{
uint8_t p, c;
for(p = 0; p < 8; p++)
{
st7565_command(CMD_SET_PAGE | p); // Set page to p
for(c = 0; c < 128; c++) // Was 129, which I think is wrong...
{
st7565_command(CMD_SET_COLUMN_LOWER | (c & 0xf));
st7565_command(CMD_SET_COLUMN_UPPER | ((c >> 4) & 0xf)); // Set column to c
st7565_data(0x00); // Clear data
}
}
}
// Write LCD buffer if type = 1 normal, 0 = logo.
void write_buffer(uint8_t *buffer, uint8_t type)
{
uint8_t c, p;
for(p = 0; p < 8; p++)
{
st7565_command(CMD_SET_PAGE | (uint8_t)pgm_read_byte(&pagemap[p])); // Page 7 to 0
st7565_command(CMD_SET_COLUMN_LOWER | (0x0 & 0xf)); // Column 0
st7565_command(CMD_SET_COLUMN_UPPER | ((0x0 >> 4) & 0xf)); // Column 0
st7565_command(CMD_RMW); // Sets auto-increment
for(c = 0; c < 128; c++)
{
st7565_data(buffer[(128*p)+c]);
}
}
}
// Initialise LCD
void st7565_init(void)
{
// Toggle RST low to reset and CS low so it'll listen to us
LCD_CSI = 0;
LCD_RES = 0;
_delay_ms(1); // Datasheet says 1us for 3.3V operation
LCD_RES = 1;
// Send command sequence
for (int i = 0; i < 14; i++)
{
st7565_command((uint8_t)pgm_read_byte(&lcd_commmands[i]));
}
st7565_set_brightness((uint8_t)Config.Contrast);
st7565_command(CMD_SET_COM_REVERSE); // For logo 0xC8
}
/**
* Flush a specific part of the buffer to the display
* @param x1 left coordinate of the area to flush
* @param y1 top coordinate of the area to flush
* @param x2 right coordinate of the area to flush
* @param y2 bottom coordinate of the area to flush
*/
static void st7565_flush(int32_t x1, int32_t y1, int32_t x2, int32_t y2)
{
spi_cs_en(ST7565_DRV);
uint8_t c, p;
for(p = y1 / 8; p <= y2 / 8; p++) {
st7565_command(CMD_SET_PAGE | pagemap[p]);
st7565_command(CMD_SET_COLUMN_LOWER | (x1 & 0xf));
st7565_command(CMD_SET_COLUMN_UPPER | ((x1 >> 4) & 0xf));
st7565_command(CMD_RMW);
for(c = x1; c <= x2; c++) {
st7565_data(lcd_fb[(ST7565_HOR_RES*p)+c]);
}
}
spi_cs_dis(ST7565_DRV);
}
void ST7565::st7565_init(void) {
// set pin directions
pinMode(sid, OUTPUT);
pinMode(sclk, OUTPUT);
pinMode(a0, OUTPUT);
pinMode(rst, OUTPUT);
pinMode(cs, OUTPUT);
digitalWrite(cs, HIGH);
digitalWrite(rst, LOW);
delay(500);
digitalWrite(rst, HIGH);
delay(500);
digitalWrite(rst, LOW);
// 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);
// 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);
// wait for 50% rising
delay(50);
// turn on voltage regulator (VC=1, VR=1, VF=0)
st7565_command(CMD_SET_POWER_CONTROL | 0x6);
// wait >=50ms
delay(50);
// turn on voltage follower (VC=1, VR=1, VF=1)
st7565_command(CMD_SET_POWER_CONTROL | 0x7);
// wait
delay(10);
// set lcd operating voltage (regulator resistor, ref voltage resistor)
st7565_command(CMD_SET_RESISTOR_RATIO | 0x6);
// initial display line
// set page address
// set column address
// write display data
// set up a bounding box for screen updates
updateBoundingBox(0, 0, LCDWIDTH-1, LCDHEIGHT-1);
}
// this doesnt touch the buffer, just clears the display RAM - might be handy
void ST7565::clear_display(void) {
uint8_t p, c;
for(p = 0; p < 8; p++) {
/*
putstring("new page! ");
uart_putw_dec(p);
putstring_nl("");
*/
st7565_command(CMD_SET_PAGE | p);
for(c = 0; c < 129; c++) {
//uart_putw_dec(c);
//uart_putchar(' ');
st7565_command(CMD_SET_COLUMN_LOWER | (c & 0xf));
st7565_command(CMD_SET_COLUMN_UPPER | ((c >> 4) & 0xf));
st7565_data(0x0);
}
}
}
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){
SIU.PCR[PIN_GLCD_CS].R = OUTPUT;
SIU.PCR[PIN_GLCD_RST].R = OUTPUT;
SIU.PCR[PIN_GLCD_AO].R = OUTPUT;
SIU.PCR[PIN_GLCD_SID].R = OUTPUT;
SIU.PCR[PIN_GLCD_SCLK].R = OUTPUT;
if(PIN_GLCD_CS > 0)
GPIO_SetState(PIN_GLCD_CS, LOW);
GPIO_SetState(PIN_GLCD_RST, LOW);
sdelay(1);
GPIO_SetState(PIN_GLCD_RST, HIGH);
// 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);
// 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);
// wait for 50% rising
mdelay(200);
// turn on voltage regulator (VC=1, VR=1, VF=0)
st7565_command(CMD_SET_POWER_CONTROL | 0x6);
// wait >=50ms
mdelay(200);
// turn on voltage follower (VC=1, VR=1, VF=1)
st7565_command(CMD_SET_POWER_CONTROL | 0x7);
// wait
mdelay(40);
// set lcd operating voltage (regulator resistor, ref voltage resistor)
st7565_command(CMD_SET_RESISTOR_RATIO | 0x6);
// initial display line
// set page address
// set column address
// write display data
updateBoundingBox(0, 0, LCDWIDTH-1, LCDHEIGHT-1);
}
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 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);
// 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);
// wait for 50% rising
_delay_ms(50);
// turn on voltage regulator (VC=1, VR=1, VF=0)
st7565_command(CMD_SET_POWER_CONTROL | 0x6);
// wait >=50ms
_delay_ms(50);
// turn on voltage follower (VC=1, VR=1, VF=1)
st7565_command(CMD_SET_POWER_CONTROL | 0x7);
// wait
_delay_ms(10);
// set lcd operating voltage (regulator resistor, ref voltage resistor)
st7565_command(CMD_SET_RESISTOR_RATIO | 0x6);
// initial display line
// set page address
// set column address
// write display data
}
void st7565_init(void)
{
// Set pin directions.
#ifdef SPI
SID_DDR |= _BV(SID);
SCLK_DDR |= _BV(SCLK);
#elif
DATA_DDR |= _BV(0xFF);
#endif
A0_DDR |= _BV(A0);
RST_DDR |= _BV(RST);
CS_DDR |= _BV(CS);
// Manually initiate the chip selection.
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);
// 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);
// Wait for 50% rising.
_delay_ms(50);
// turn on voltage regulator (VC=1, VR=1, VF=0).
st7565_command(CMD_SET_POWER_CONTROL | 0x6);
// Wait >=50ms.
_delay_ms(50);
// Turn on voltage follower (VC=1, VR=1, VF=1).
st7565_command(CMD_SET_POWER_CONTROL | 0x7);
// Short wait for the command to complete.
_delay_ms(10);
// Set LCD operating voltage (regulator resistor, ref voltage resistor)
st7565_command(CMD_SET_RESISTOR_RATIO | 0x6);
}
// Initialise LCD
void st7565_init(void)
{
// Toggle RST low to reset and CS low so it'll listen to us
LCD_CSI = 0;
LCD_RES = 0;
_delay_ms(500);
LCD_RES = 1;
// Send command sequence
for (int i = 0; i < 7; i++)
{
st7565_command(pgm_read_byte(&lcd_commmands[i]));
}
_delay_ms(50);
st7565_command(0x2E);
_delay_ms(50);
st7565_command(0x2F);
_delay_ms(10);
for (int i = 9; i < 14; i++)
{
st7565_command(pgm_read_byte(&lcd_commmands[i]));
}
}
// Set LCD brightness
void st7565_set_brightness(uint8_t val)
{
st7565_command(CMD_SET_VOLUME_FIRST); // 0x81
st7565_command(val);
}
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()
/**
* Initialize the ST7565
*/
void st7565_init(void)
{
io_set_pin_dir(ST7565_RST_PORT,ST7565_RST_PIN, IO_DIR_OUT);
io_set_pin_dir(ST7565_RS_PORT,ST7565_RS_PIN, IO_DIR_OUT);
io_set_pin(ST7565_RST_PORT,ST7565_RST_PIN, 1);
tick_wait_ms(10);
io_set_pin(ST7565_RST_PORT,ST7565_RST_PIN, 0);
tick_wait_ms(10);
io_set_pin(ST7565_RST_PORT,ST7565_RST_PIN, 1);
tick_wait_ms(10);
spi_set_baud(ST7565_DRV, ST7565_BAUD);
spi_cs_en(ST7565_DRV);
st7565_command(CMD_SET_BIAS_7);
st7565_command(CMD_SET_ADC_NORMAL);
st7565_command(CMD_SET_COM_NORMAL);
st7565_command(CMD_SET_DISP_START_LINE);
st7565_command(CMD_SET_POWER_CONTROL | 0x4);
tick_wait_ms(50);
st7565_command(CMD_SET_POWER_CONTROL | 0x6);
tick_wait_ms(50);
st7565_command(CMD_SET_POWER_CONTROL | 0x7);
tick_wait_ms(10);
st7565_command(CMD_SET_RESISTOR_RATIO | 0x6);
st7565_command(CMD_DISPLAY_ON);
st7565_command(CMD_SET_ALLPTS_NORMAL);
/*Set brightness*/
st7565_command(CMD_SET_VOLUME_FIRST);
st7565_command(CMD_SET_VOLUME_SECOND | (0x18 & 0x3f));
spi_cs_dis(ST7565_DRV);
memset(lcd_fb, 0x00, sizeof(lcd_fb));
}
void ST7565::st7565_set_brightness(uint8_t val) {
st7565_command(CMD_SET_VOLUME_FIRST);
st7565_command(CMD_SET_VOLUME_SECOND | (val & 0x3f));
}
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 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()
uint8_t GoLine(void)
{
register uint8_t li;
register uint8_t *lp;
register unsigned int bi;
//printf("\n#%d:%s\n",CLine,CmdInp);
//printf("\n*lnum=%d A=%d\n",CLine,Vars[0]);
Gp = CmdInp + 1;
switch(CmdInp[0]){
case STOP:
STOPPROG(ESTOP);
case BEEP:
lp = findchar(CmdInp, ',');
if(lp)
{
*lp = 0;
bi = ExpPars1();
Gp=lp+1;
Beep(bi,ExpPars1());
}
return(0);
case DIM:
Gp+=2;//(
li = ExpPars1();
if(CmdInp[1] != SIGNEDBYTE && CmdInp[1] != UNSIGNEDBYTE)//type of array
li = li << 1;//two byte on item
if(li <= (MAXBMEMSIZE-1)){
/* lp = Vars[TOVAR(CmdInp[1])];
if(lp > FirstPrgLine)
lfree(lp);*/
lp = lmalloc(li+1,LlP);
if(!lp)
STOPPROG(EALLOC);
*lp = CmdInp[1];//type of array
lp++;
//printf("\n*%p %c %d %d %s\n",lp,*(lp-1),*(lp-2),li,CmdInp);
li = CmdInp[2];//name of array
// if(SYMISVAR(li))
Vars[TOVAR(li)] = (unsigned int)lp;
return(0);
}
STOPPROG(EERROR);
return(0);
case LET:
lp = findchar(CmdInp, '=');
Gp = lp+1;
bi = ExpPars1();
// if(!SYMISVAR(li))
if(*(lp-1) == ')'){
*lp = 0;
Gp = CmdInp+2;// '('
li = ExpPars1();//array index
lp = (uint8_t *)Vars[TOVAR(CmdInp[1])];
//printf("\n+%p %c %d %d %d\n",lp,*(lp-1),*(lp-2),li,bi);
if(*(lp-2) > (li+2)){//
switch(*(lp-1)){//type of array
case SIGNEDBYTE:
*(((char *)lp + li)) = bi;
break;
case UNSIGNEDBYTE:
*(((uint8_t *)lp + li)) = bi;
break;
default:
// case SIGNEDWORD:
*(((int *)lp + li)) = bi;
break;
/* case UNSIGNEDWORD:
*(((unsigned int *)lp + li)) = bi;
break;
*/
}
}
else
STOPPROG(EERROR);//TODO
}
else
Vars[TOVAR(CmdInp[1])] = bi;
return(0);
case AT:
lp = findchar(CmdInp, ',');
if(lp)
{
*lp = 0;
li = ExpPars1();//x
if(li< LCDTWIDTH)
#ifdef AVR
xt=li*LCDSYMWIDTH;
else
xt=(LCDTWIDTH-1)*LCDSYMWIDTH;
#else
xt=li;
else
xt=(LCDTWIDTH-1);
#endif
Gp=lp+1;
li = ExpPars1();//y
if(li< LCDTHEIGHT)
yt=li;
else
yt=LCDTHEIGHT-1;
#ifdef AVR
st7565_command(CMD_SET_DISP_START_LINE | ((LCDTHEIGHT-1)*8)+8);
#else
printf( "%c[%d;%dH", 27, yt+1, xt+1 ); // установили курсор в позицию
fflush( stdout );
#endif
}
return(0);
case OUT:
lp = findchar(CmdInp, ',');
if(lp)
{
*lp = 0;
li = ExpPars1();//port
Gp=lp+1;
out_port(li,ExpPars1());
}
return(0);
case REM:
return(0);
case LOAD:
FreePrg();
ReplaceChar(CmdInp+2, '"', 0);
loadprg((const char *)(CmdInp+2));
PrgLineP = FirstPrgLine;
return(1);
case INPUT:
lp = (uint8_t *)(Vars + TOVAR(CmdInp[1]));//pointer to var
Gp++;//to '(' or 0
if(*Gp == '(')
li = ExpPars1();//index
else
li = 255;
lgets(CmdInp);
if(CmdInp[0]==BREAK_KEY)
STOPPROG(EINTERUPT);
Gp=CmdInp;
bi = ExpPars1();
if(li < MAXBMEMSIZE){
lp = (uint8_t *)(*((unsigned int *)lp));
if(*(lp-2) > (li+2)){//
switch(*(lp-1)){//type of array
case SIGNEDBYTE:
*(((char *)lp + li)) = bi;
break;
case UNSIGNEDBYTE:
*(((uint8_t *)lp + li)) = bi;
break;
default:
*(((int *)lp + li)) = bi;
break;
}
}
else
STOPPROG(EERROR);//TODO
}
else
*lp = (int)bi;
return(0);
case IF:
lp = findchar(CmdInp, THEN);
if(lp){
*lp = 0;
if(ExpPars1()){
strcpy((char *)CmdInp,(const char *)(lp+1));
return(2);;
}
}else
STOPPROG(EERROR);
return(0);
case PRINT:
li = strlen((const char *)CmdInp);
while(Gp && *Gp){
if(*Gp == '"'){
Gp++;
lp =findchar(Gp ,'"');
*lp = 0;
lputs((char *)(Gp));
Gp = lp+1;
}
else{
if(*Gp == '$'){
Gp++;
lputchar(ExpPars1());
}
else
lputint(ExpPars1());
}
//printf("\n++ %s\n",Gp);
Gp =findchar(Gp ,',');
if(Gp)
Gp++;
}
if(CmdInp[li-1]!=';')
lputchar('\n');
return(0);
case PAUSE:
bi = ExpPars1();
delay_ms(bi);
return(0);
case GOTO:
PrgLineP = GetPrgLine(ExpPars1(),0);
if(!PrgLineP){
STOPPROG(EGOTONOWHERE);
}
return(1) ;
case GOSUB:
SubStack[SubStackP] = PrgLineP->next;
SubStackP++;
if(SubStackP >= SMAX){
STOPPROG(EGSOVF);
}
PrgLineP = GetPrgLine(ExpPars1(),0);
if(!PrgLineP){
STOPPROG(EGOTONOWHERE);
}
return(1);
case RETURN:
if(SubStackP < 1){
STOPPROG(ERETWOG);
}
--SubStackP;
PrgLineP = SubStack[SubStackP];
return(1);
case FOR :
lp = findchar(CmdInp, TO);
if(lp){
*lp = 0;
Gp = CmdInp+ 3;
li = TOVAR(CmdInp[1]);
if(li>LMAX)
STOPPROG(ELOPSOVF);
Vars[li] = ExpPars1();
Gp = lp + 1;
LoopVar[li].var_to = ExpPars1();
LoopVar[li].line_begin = PrgLineP->next;
}else
STOPPROG(EERROR);
return(0);
case NEXT:
li = TOVAR(CmdInp[1]);
if(++Vars[li] <= LoopVar[li].var_to){
PrgLineP = LoopVar[li].line_begin;
return(1);
}
break;
}
void ST7565::begin(uint8_t contrast) {
st7565_init();
st7565_command(CMD_DISPLAY_ON);
st7565_command(CMD_SET_ALLPTS_NORMAL);
st7565_set_brightness(contrast);
}
