void main()
{
//reset_globals();
setGameInputToEnabled(true, true);
setAirbrakeRelatedInputToBlocked(false, true);
write_text_to_log_file("Setting up calls");
set_periodic_feature_call(update_features);
write_text_to_log_file("Loading settings");
load_settings();
init_vehicle_feature();
write_text_to_log_file("Loaded settings OK");
// this creates a new locale based on the current application default
// (which is either the one given on startup, but can be overriden with
// std::locale::global) - then extends it with an extra facet that
// controls numeric output.
std::locale comma_locale(std::locale(), new comma_numpunct());
// tell cout to use our new locale.
std::cout.imbue(comma_locale);
set_status_text("~HUD_COLOUR_MENU_YELLOW~ENT ~HUD_COLOUR_WHITE~is ready!");
while (true)
{
if (trainer_switch_pressed())
{
menu_beep();
set_menu_showing(true);
process_main_menu();
set_menu_showing(false);
}
else if (airbrake_switch_pressed())
{
menu_beep();
process_airbrake_menu();
}
update_features();
WAIT(0);
}
}
void Save_Config_to_EEPROM(void)
{
// Write to eeProm
cli();
eeprom_write_block_changes((const void*) &Config, (void*) EEPROM_DATA_START_POS, sizeof(CONFIG_STRUCT));
sei();
menu_beep(1);
LED1 = !LED1;
_delay_ms(500);
LED1 = !LED1;
}
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 menu_mixer(uint8_t i)
{
uint8_t cursor = LINE0;
uint8_t top = MIXERSTART;
int8_t values[MIXERITEMS];
menu_range_t range;
uint8_t temp = 0;
uint8_t text_link = 0;
while(button != BACK)
{
// Load values from eeprom
memcpy(&values[0],&Config.Channel[i].source_a,sizeof(int8_t) * MIXERITEMS);
// Print menu
print_menu_items(top, MIXERSTART, &values[0], MIXERITEMS,(prog_uchar*)mixer_menu_ranges, MIXOFFSET, (prog_uchar*)MixerMenuText, cursor);
// Handle menu changes
update_menu(MIXERITEMS, MIXERSTART, button, &cursor, &top, &temp);
range = get_menu_range ((prog_uchar*)mixer_menu_ranges, temp - MIXERSTART);
if (button == ENTER)
{
text_link = pgm_read_byte(&MixerMenuText[temp - MIXERSTART]);
values[temp - MIXERSTART] = do_menu_item(temp, values[temp - MIXERSTART], range, 0, text_link);
}
// Update value in config structure
memcpy(&Config.Channel[i].source_a,&values[0],sizeof(int8_t) * MIXERITEMS);
if (button == ENTER)
{
UpdateLimits(); // Update travel limits based on percentages
Save_Config_to_EEPROM(); // Save value and return
}
}
menu_beep(1);
_delay_ms(200);
}
void main()
{
set_periodic_feature_call(updateStuff);
featurePlayerBlips = config->get_trainer_config()->setting_player_blips;
featurePlayerHeadDisplay = config->get_trainer_config()->setting_player_head_display;
featurePlayerBlipCone = config->get_trainer_config()->setting_player_blip_cone;
featurePlayerNotifications = config->get_trainer_config()->setting_player_notifications;
featureShowVoiceChatSpeaker = config->get_trainer_config()->setting_show_voice_chat_speaker;
while (true)
{
if (is_menu_showing())
{
menu_beep(0);
process_main_menu();
set_menu_showing(false);
}
updateStuff();
WAIT(0);
}
}
void update_menu(uint8_t items, uint8_t start, uint8_t offset, uint8_t button, uint8_t* cursor, uint8_t* top, uint8_t* temp)
{
// Temporarily add in offset :(
*top = *top + offset;
start = start + offset;
// Calculate which function has been requested
if (button == ENTER)
{
switch(*cursor)
{
case LINE0:
*temp = *top;
break;
case LINE1:
*temp = *top + 1;
break;
case LINE2:
*temp = *top + 2;
break;
case LINE3:
*temp = *top + 3;
break;
default:
break;
}
menu_beep(1);
_delay_ms(200);
}
// Handle cursor Up/Down limits
if (button == DOWN)
{
switch(*cursor)
{
case LINE0:
if (items > 1) *cursor = LINE1;
break;
case LINE1:
if (items > 2) *cursor = LINE2;
break;
case LINE2:
if (items > 3) *cursor = LINE3;
break;
case LINE3:
if (items > 4) *cursor = NEXTLINE;
break;
default:
*cursor = NEXTLINE;
break;
}
menu_beep(1);
_delay_ms(200);
}
if (button == UP)
{
switch(*cursor)
{
case LINE3:
*cursor = LINE2;
break;
case LINE2:
*cursor = LINE1;
break;
case LINE1:
*cursor = LINE0;
break;
case LINE0:
*cursor = PREVLINE;
break;
default:
*cursor = PREVLINE;
break;
}
menu_beep(1);
_delay_ms(200);
}
// When cursor is at limits and button pressed
if (*cursor == PREVLINE) // Up
{
*cursor = LINE0;
if (*top > start) *top = *top - 1; // Shuffle list up
}
if (*cursor == NEXTLINE) // Down
{
*cursor = LINE3;
if ((*top+3) < ((start + items)-1)) *top = *top + 1;// Shuffle list down
}
// Remove temporary offset
*top = *top - offset;
}
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 do_main_menu_item(uint8_t menuitem)
{
switch(menuitem)
{
case MAINSTART:
menu_general();
break;
case MAINSTART+1:
menu_rc_setup();
break;
case MAINSTART+2:
menu_stab_control();
break;
case MAINSTART+3:
menu_al_control();
break;
case MAINSTART+4:
menu_expo();
break;
case MAINSTART+5:
menu_battery();
break;
case MAINSTART+6:
Display_rcinput();
break;
case MAINSTART+7:
Display_sensors();
break;
case MAINSTART+8:
Display_balance();
break;
case MAINSTART+9:
menu_mixer(0);
break;
case MAINSTART+10:
menu_mixer(1);
break;
case MAINSTART+11:
menu_mixer(2);
break;
case MAINSTART+12:
menu_mixer(3);
break;
case MAINSTART+13:
menu_mixer(4);
break;
case MAINSTART+14:
menu_mixer(5);
break;
case MAINSTART+15:
menu_mixer(6);
break;
case MAINSTART+16:
menu_mixer(7);
break;
default:
break;
} // Switch
menu_beep(1);
_delay_ms(200);
}
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);
}
//Test for airbrake command.
void process_airbrake_menu()
{
exitFlag = false;
const float lineWidth = 250.0;
const int lineCount = 1;
bool loadedAnims = false;
std::string caption = "Airbrake";
//draw_menu_header_line(caption,350.0f,50.0f,15.0f,0.0f,15.0f,false);
DWORD waitTime = 150;
Ped playerPed = PLAYER::PLAYER_PED_ID();
bool inVehicle = PED::IS_PED_IN_ANY_VEHICLE(playerPed, 0) ? true : false;
if (!inVehicle)
{
STREAMING::REQUEST_ANIM_DICT(AIRBRAKE_ANIM_A);
while (!STREAMING::HAS_ANIM_DICT_LOADED(AIRBRAKE_ANIM_A))
{
WAIT(0);
}
loadedAnims = true;
}
while (true && !exitFlag)
{
// timed menu draw, used for pause after active line switch
DWORD maxTickCount = GetTickCount() + waitTime;
do
{
// draw menu
draw_menu_header_line(caption, 350.0f, 50.0f, 15.0f, 0.0f, 15.0f, false);
//draw_menu_line(caption, lineWidth, 15.0, 18.0, 0.0, 5.0, false, true);
make_periodic_feature_call();
WAIT(0);
} while (GetTickCount() < maxTickCount);
waitTime = 0;
airbrake(inVehicle);
//// process buttons
//bool bSelect, bBack, bUp, bDown;
//get_button_state(&bSelect, &bBack, &bUp, &bDown, NULL, NULL);
if (airbrake_switch_pressed())
{
menu_beep();
break;
}
}
if (!inVehicle)
{
AI::CLEAR_PED_TASKS_IMMEDIATELY(PLAYER::PLAYER_PED_ID());
}
exitFlag = false;
}
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()
