void CalibrateGyrosFast(void)
{
uint8_t i;
// Clear gyro zeros
memset(&Config.gyroZero[ROLL],0,(sizeof(int16_t) * NUMBEROFAXIS));
// Calculate average over 32 reads
for (i=0; i<32; i++)
{
get_raw_gyros(); // Updates gyroADC[]
Config.gyroZero[ROLL] += gyroADC[ROLL];
Config.gyroZero[PITCH] += gyroADC[PITCH];
Config.gyroZero[YAW] += gyroADC[YAW];
}
// Average readings for all axis
for (i=0; i<NUMBEROFAXIS; i++)
{
Config.gyroZero[i] = (Config.gyroZero[i] >> 5); // Divide by 32
}
Save_Config_to_EEPROM();
}
bool Initial_EEPROM_Config_Load(void)
{
bool updated = false;
// Read eeProm data into RAM
eeprom_read_block((void*)&Config, (const void*)EEPROM_DATA_START_POS, sizeof(CONFIG_STRUCT));
// See if we know what to do with the current eeprom data
// Config.setup holds the magic number from the current EEPROM
switch(Config.setup)
{
case V1_0_SIGNATURE: // V1.0 detected
Update_V1_0_to_V1_1();
updated = true;
// Fall through...
case V1_1_SIGNATURE: // V1.1 detected
Update_V1_1_to_V1_2();
updated = true;
// Fall through...
case V1_2_SIGNATURE: // V1.2 detected
Update_V1_2_to_V1_3B1();
updated = true;
// Fall through...
case V1_3_B1_SIGNATURE: // V1.3 B1 detected
Update_V1_3B1_to_V1_3B14();
updated = true;
// Fall through...
case V1_3_B14_SIGNATURE: // V1.3 B14 detected
Update_V1_3B14_to_V1_3B15();
updated = true;
// Fall through...
case V1_3_B15_SIGNATURE: // V1.3 B15 detected
Update_V1_3B15_to_V1_3B17();
updated = true;
// Fall through...
case V1_3_B17_SIGNATURE: // V1.3 B15 detected
break;
default: // Unknown solution - restore to factory defaults
// Load factory defaults
Set_EEPROM_Default_Config();
break;
}
// Save back to eeprom
Save_Config_to_EEPROM();
// Return info regarding eeprom structure changes
return updated;
}
void menu_rc_setup(void)
{
uint8_t cursor = LINE0;
uint8_t top = RCSTART;
uint8_t temp = 0;
int8_t values[RCITEMS];
menu_range_t range;
uint8_t i = 0;
uint8_t text_link;
while(button != BACK)
{
// Load values from eeprom
memcpy(&values[0],&Config.TxSeq,sizeof(int8_t) * RCITEMS);
// Print menu
print_menu_items(top, RCSTART, &values[0], RCITEMS, (prog_uchar*)rc_menu_ranges, RCOFFSET, (prog_uchar*)RCMenuText, cursor);
// Handle menu changes
update_menu(RCITEMS, RCSTART, button, &cursor, &top, &temp);
range = get_menu_range ((prog_uchar*)rc_menu_ranges, temp - RCSTART);
if (button == ENTER)
{
text_link = pgm_read_byte(&RCMenuText[temp - RCSTART]);
values[temp - RCSTART] = do_menu_item(temp, values[temp - RCSTART], range, 0, text_link);
}
// Update value in config structure
memcpy(&Config.TxSeq,&values[0],sizeof(int8_t) * RCITEMS);
// Update Ch7. mixer with source from Config.FlapChan if in Aeroplane mode
if (Config.MixMode == AEROPLANE)
{
Config.Channel[CH7].source_a = Config.FlapChan;
}
if (button == ENTER)
{
// Update channel sequence
for (i = 0; i < MAX_RC_CHANNELS; i++)
{
if (Config.TxSeq == JRSEQ)
{
Config.ChannelOrder[i] = pgm_read_byte(&JR[i]);
}
else if (Config.TxSeq == FUTABASEQ)
{
Config.ChannelOrder[i] = pgm_read_byte(&FUTABA[i]);
}
}
Save_Config_to_EEPROM(); // Save value and return
}
}
}
void Initial_EEPROM_Config_Load(void)
{
// Load last settings from EEPROM
if(eeprom_read_byte((uint8_t*) EEPROM_DATA_START_POS )!=0x47)
{
Config.setup = 0x47;
Set_EEPROM_Default_Config();
// Write to eeProm
Save_Config_to_EEPROM();
} else {
// Read eeProm
eeprom_read_block(&Config, (void*) EEPROM_DATA_START_POS, sizeof(CONFIG_STRUCT));
}
}
void menu_battery(void)
{
uint8_t cursor = LINE0;
uint8_t top = BATTSTART;
uint8_t temp = 0;
int16_t values[BATTITEMS];
menu_range_t range;
uint8_t text_link = 0;
uint8_t temp_cells = 0;
uint16_t temp_minvoltage = 0;
while(button != BACK)
{
// Load values from eeprom
memcpy(&values[0],&Config.BatteryType,sizeof(int16_t) * BATTITEMS);
// Save pre-edited values
temp_cells = Config.BatteryCells;
temp_minvoltage = Config.MinVoltage;
// Print menu
print_menu_items_16(top, BATTSTART, &values[0], (prog_uchar*)batt_menu_ranges, BATTOFFSET, (prog_uchar*)BattMenuText, cursor);
// Handle menu changes
update_menu(BATTITEMS, BATTSTART, button, &cursor, &top, &temp);
range = get_menu_range ((prog_uchar*)batt_menu_ranges, temp - BATTSTART);
if (button == ENTER)
{
text_link = pgm_read_byte(&BattMenuText[temp - BATTSTART]);
values[temp - BATTSTART] = do_menu_item(temp, values[temp - BATTSTART], range, 0, text_link);
}
// See if cell number or min_volts has changed
if ((temp_cells != values[1]) || (temp_minvoltage != values[4]))
{
values[2] = values[1] * values[4];
Config.PowerTrigger = values[2];
}
// Update value in config structure
memcpy(&Config.BatteryType,&values[0],sizeof(int16_t) * BATTITEMS);
if (button == ENTER)
{
Save_Config_to_EEPROM(); // Save value and return
}
}
}
void menu_offsets(void)
{
int8_t *value_ptr;
uint16_t reference = OFFSETSSTART;
// If sub-menu item has changed, reset sub-menu positions
if (menu_flag)
{
sub_top = OFFSETSSTART;
menu_flag = 0;
}
while(button != BACK)
{
value_ptr = &Config.Offsets[0].Point1;
// Print top level menu
print_menu_items(sub_top, reference, value_ptr, (const unsigned char*)Offsets_menu_ranges, 0, (const uint16_t*)OffsetsMenuOffsets, (const uint16_t*)OffsetsMenuText, cursor);
// Handle menu navigation
update_menu(MAX_OUTPUTS, reference, 0, button, &cursor, &sub_top, &menu_temp);
// Edit selected curve
if (button == ENTER)
{
edit_curve_item(menu_temp - reference, OFFSET); // Curves after NUMBEROFCURVES are offsets
}
// Stop unwanted exit to main menu
if (button == ABORT)
{
Wait_BUTTON1(); // Wait for user's finger off the button
button = NONE;
}
// Update limits when exiting
if (button == ENTER)
{
Save_Config_to_EEPROM(); // Save value and return
Wait_BUTTON4(); // Wait for user's finger off the button
Wait_BUTTON1();
}
}
}
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);
}
// Center sticks on request from Menu
void CenterSticks(void)
{
uint8_t i, j;
uint16_t RxChannelZeroOffset[MAX_RC_CHANNELS] = {0,0,0,0,0,0,0,0};
// Take an average of eight readings
// RxChannel will auto-update every RC frame (normally 46Hz or so)
for (i=0; i<8; i++)
{
for (j=0; j<MAX_RC_CHANNELS; j++)
{
RxChannelZeroOffset[j] += RxChannel[j];
}
_delay_ms(100);
}
for (i=0;i<MAX_RC_CHANNELS;i++)
{
Config.RxChannelZeroOffset[i] = ((RxChannelZeroOffset[i] + 4) >> 3); // Round and divide by 8
}
Save_Config_to_EEPROM();
}
void menu_al_control(void)
{
uint8_t cursor = LINE0;
uint8_t top = AUTOSTART;
uint8_t temp = 0;
int8_t values[AUTOITEMS];
menu_range_t range;
uint8_t text_link = 0;
while(button != BACK)
{
// Load values from eeprom
memcpy(&values[0],&Config.AutoMode,sizeof(int8_t) * AUTOITEMS);
// Print menu
print_menu_items(top, AUTOSTART, &values[0], AUTOITEMS, (prog_uchar*)auto_menu_ranges, AUTOOFFSET, (prog_uchar*)AutoMenuText, cursor);
// Handle menu changes
update_menu(AUTOITEMS, AUTOSTART, button, &cursor, &top, &temp);
range = get_menu_range ((prog_uchar*)auto_menu_ranges, temp - AUTOSTART);
if (button == ENTER)
{
text_link = pgm_read_byte(&AutoMenuText[temp - AUTOSTART]);
values[temp - AUTOSTART] = do_menu_item(temp, values[temp - AUTOSTART], range, 0, text_link);
}
// Update value in config structure
memcpy(&Config.AutoMode,&values[0],sizeof(int8_t) * AUTOITEMS);
if (button == ENTER)
{
UpdateLimits(); // Update I-term limits and triggers based on percentages
Save_Config_to_EEPROM(); // Save value and return
}
}
}
void menu_mixer(uint8_t section)
{
static uint8_t mix_top = MIXERSTART;
static uint8_t old_section;
int8_t *value_ptr;
int8_t values[INPUTITEMS+1];
menu_range_t range;
uint8_t text_link = 0;
uint8_t offset; // Index into channel structure
uint8_t items; // Items in group
// If submenu item has changed, reset submenu positions
if (section != old_section)
{
mix_top = MIXERSTART;
old_section = section;
}
// Get mixer menu offsets
// 1 = input mixer data, 2 = output mixer data
switch(section)
{
case 1:
items = INPUTITEMS+1;
offset = 0;
value_ptr = &values[0];
break;
case 2:
items = OUTPUTITEMS+1;
offset = INPUTITEMS+1;
value_ptr = &values[0];
break;
default:
items = INPUTITEMS+1;
offset = 0;
value_ptr = &values[0];
break;
}
while(button != BACK)
{
// Load values from eeprom and insert channel number at the top of each - messy
values[0] = Config.MenuChannel;
if (section == 1)
{
memcpy(&values[1],&Config.Channel[Config.MenuChannel].source_a,(sizeof(int8_t) * INPUTITEMS));
}
else
{
memcpy(&values[1],&Config.Channel[Config.MenuChannel].switcher,(sizeof(int8_t) * OUTPUTITEMS));
}
// Print menu
print_menu_items(mix_top + offset, MIXERSTART + offset, value_ptr, 1, (prog_uchar*)mixer_menu_ranges[section - 1], 0, MIXOFFSET, (prog_uchar*)MixerMenuText[section - 1], cursor);
// Handle menu changes
update_menu(items, MIXERSTART, offset, button, &cursor, &mix_top, &menu_temp);
range = get_menu_range ((prog_uchar*)mixer_menu_ranges[section - 1], menu_temp - MIXERSTART - offset);
if (button == ENTER)
{
text_link = pgm_read_byte(&MixerMenuText[section - 1][menu_temp - MIXERSTART - offset]);
do_menu_item(menu_temp, value_ptr + (menu_temp - MIXERSTART - offset), 1, range, 0, text_link, false, 0);
}
// Save modified data back to Config
// Copy channel number back to global
switch(section)
{
case 1:
memcpy(&Config.Channel[Config.MenuChannel].source_a,&values[1],(sizeof(int8_t) * INPUTITEMS));
Config.MenuChannel = values[0];
break;
case 2:
memcpy(&Config.Channel[Config.MenuChannel].switcher,&values[1],(sizeof(int8_t) * OUTPUTITEMS));
Config.MenuChannel = values[0];
break;
default:
break;
}
// Save and exit
if (button == ENTER)
{
UpdateLimits(); // Update travel limits based on percentages
Save_Config_to_EEPROM(); // Save value and return
}
}
_delay_ms(200);
}
void menu_rc_setup(uint8_t section)
{
int8_t *value_ptr;
menu_range_t range;
uint8_t text_link;
uint8_t i = 0;
uint8_t mult = 1; // Multiplier
uint8_t offset; // Index into channel structure
uint8_t items; // Items in group
// If submenu item has changed, reset submenu positions
if (menu_flag)
{
sub_top = RCSTART;
menu_flag = 0;
}
while(button != BACK)
{
// Get menu offsets and load values from eeprom
// 1 = RC, 2 = Failsafe, 3 = General
switch(section)
{
case 1: // RC setup menu
offset = 0;
items = RCITEMS;
value_ptr = &Config.RxModeIn;
mult = 1;
break;
case 2: // Failsafe menu
offset = RCITEMS;
items = FSITEMS;
value_ptr = &Config.FailsafeType;
mult = 1;
break;
case 3: // General menu
offset = RCITEMS + FSITEMS;
items = GENERALITEMS;
value_ptr = &Config.MixMode;
mult = 1;
break;
default:
offset = 0;
items = RCITEMS;
value_ptr = &Config.RxModeIn;
mult = 1;
break;
}
// Save pre-edited values
int8_t temp_type = Config.MixMode;
int8_t temp_flapchan = Config.FlapChan;
int8_t temp_RxModeIn = Config.RxModeIn;
// Print menu
print_menu_items(sub_top + offset, RCSTART + offset, value_ptr, mult, (const unsigned char*)rc_menu_ranges[section - 1], 0, RCOFFSET, (const unsigned char*)RCMenuText[section - 1], cursor);
// Handle menu changes
update_menu(items, RCSTART, offset, button, &cursor, &sub_top, &menu_temp);
range = get_menu_range ((const unsigned char*)rc_menu_ranges[section - 1], (menu_temp - RCSTART - offset));
if (button == ENTER)
{
text_link = pgm_read_byte(&RCMenuText[section - 1][menu_temp - RCSTART - offset]);
do_menu_item(menu_temp, value_ptr + (menu_temp - RCSTART - offset), mult, range, 0, text_link, false, 0);
}
if (button == ENTER)
{
// Update Ch5. mixer with source from Config.FlapChan if in Aeroplane mode and source changed
if ((Config.MixMode == AEROPLANE) && (Config.FlapChan != temp_flapchan))
{
Config.Channel[CH5].source_a = Config.FlapChan;
}
// Reset serial in channel masks every time the input type is changed
if (temp_RxModeIn != Config.RxModeIn)
{
Xtreme_Chanmask = 0;
Xtreme_RSS = 0;
Spektrum_Chanmask_0 = 0;
Spektrum_Chanmask_1 = 0;
Spektrum_frameloss = 0;
SBUS_Flags = 0;
// Clear channel data
for (i = 0; i < MAX_RC_CHANNELS; i++)
{
RxChannel[i] = 0;
// Unused Spektrum channels set to NULL
if (Config.RxModeOut == SPEKTRUM)
{
ExtChannel[i] = 0xFFFF;
}
// Unused channels set to mid-way
else if (Config.RxModeOut == SBUS)
{
ExtChannel[i] = 0x400;
}
// Xtreme doesn't care
else
{
ExtChannel[i] = 0;
}
}
}
// If model type has changed, reload preset
if ((section == 3) && (temp_type != Config.MixMode))
{
switch(Config.MixMode) // Load selected mix
{
case AEROPLANE:
get_preset_mix(AEROPLANE_MIX);
break;
case FWING:
get_preset_mix(FLYING_WING_MIX);
break;
case MANUAL:
// Clear all channel info
memset(&Config.Channel[0].value,0,(sizeof(channel_t) * MAX_OUTPUTS));
// Preset important settings
for (i = 0; i < MAX_OUTPUTS; i++)
{
Config.Channel[i].source_a = i; // Set to mirror the inputs
Config.Channel[i].source_a_volume = 100;
Config.Channel[i].source_b = NOCHAN;
Config.Channel[i].output_b = UNUSED;
Config.Channel[i].output_c = UNUSED;
}
break;
default:
break;
}
}
init_int(); // In case RC type has changed, reinitialise interrupts
init_uart(); // and UART
UpdateLimits(); // Update I-term limits and triggers based on percentages
#ifdef KK21
// Update MPU6050 LPF
writeI2Cbyte(MPU60X0_DEFAULT_ADDRESS, MPU60X0_RA_CONFIG, (6 - Config.MPU6050_LPF));
#endif
// Update channel sequence
for (i = 0; i < MAX_RC_CHANNELS; i++)
{
if (Config.TxSeq == FUTABASEQ)
{
Config.ChannelOrder[i] = pgm_read_byte(&FUTABA[i]);
}
else
{
Config.ChannelOrder[i] = pgm_read_byte(&JR[i]);
}
}
Save_Config_to_EEPROM(); // Save value and return
Wait_BUTTON4(); // Wait for user's finger off the button
}
}
}
void menu_flight(uint8_t mode)
{
int8_t *value_ptr;
menu_range_t range;
uint8_t text_link;
int8_t temp_gyro_roll = 0;
int8_t temp_gyro_pitch = 0;
int8_t temp_gyro_yaw = 0;
// If submenu item has changed, reset submenu positions
if (menu_flag)
{
sub_top = FLIGHTSTART;
menu_flag = 0;
}
while(button != BACK)
{
value_ptr = &Config.FlightMode[mode-1].StabMode;
// Save pre-edited value for gyro types
temp_gyro_roll = Config.FlightMode[mode - 1].Roll_type;
temp_gyro_pitch = Config.FlightMode[mode - 1].Pitch_type;
temp_gyro_yaw = Config.FlightMode[mode - 1].Yaw_type;
// Print menu
print_menu_items(sub_top, FLIGHTSTART, value_ptr, 1, (const unsigned char*)flight_menu_ranges, 0, FLIGHTOFFSET, (const unsigned char*)FlightMenuText, cursor);
// Handle menu changes
update_menu(FLIGHTITEMS, FLIGHTSTART, 0, button, &cursor, &sub_top, &menu_temp);
range = get_menu_range ((const unsigned char*)flight_menu_ranges, (menu_temp - FLIGHTSTART));
if (button == ENTER)
{
text_link = pgm_read_byte(&FlightMenuText[menu_temp - FLIGHTSTART]);
do_menu_item(menu_temp, value_ptr + (menu_temp - FLIGHTSTART), 1, range, 0, text_link, false, 0);
}
// Preset I-limits when gyro mode changes
if (button == ENTER)
{
// If roll gyro type has changed, reset to an appropriate start point
if (temp_gyro_roll != Config.FlightMode[mode-1].Roll_type)
{
// Use Gyro type value to preset limits
if(Config.FlightMode[mode-1].Roll_type == LOCK)
{
Config.FlightMode[mode - 1].Roll_limit = 125;
}
else
{
Config.FlightMode[mode - 1].Roll_limit = 0;
}
}
if (temp_gyro_pitch != Config.FlightMode[mode-1].Pitch_type)
{
if(Config.FlightMode[mode-1].Pitch_type == LOCK)
{
Config.FlightMode[mode - 1].Pitch_limit = 125;
}
else
{
Config.FlightMode[mode - 1].Pitch_limit = 0;
}
}
if (temp_gyro_yaw != Config.FlightMode[mode-1].Yaw_type)
{
if(Config.FlightMode[mode-1].Yaw_type == LOCK)
{
Config.FlightMode[mode - 1].Yaw_limit = 125;
}
else
{
Config.FlightMode[mode - 1].Yaw_limit = 0;
}
}
UpdateLimits(); // Update I-term limits and triggers based on percentages
Save_Config_to_EEPROM(); // Save value and return
Wait_BUTTON4(); // Wait for user's finger off the button
}
}
}
void menu_servo_setup(uint8_t section)
{
int8_t *value_ptr = &Config.Servo_reverse[0];
menu_range_t range;
uint8_t text_link;
uint8_t i = 0;
bool servo_enable = false;
bool zero_setting = false;
// If submenu item has changed, reset submenu positions
if (menu_flag)
{
sub_top = SERVOSTART;
menu_flag = 0;
}
// Get menu offsets
// 1 = Reverse, 2 = Min, 3 = Max
while(button != BACK)
{
// Load values from eeprom
for (i = 0; i < SERVOITEMS; i++)
{
switch(section)
{
case 1:
break;
case 2:
value_ptr = &Config.min_travel[0];
servo_enable = true;
zero_setting = true;
break;
case 3:
value_ptr = &Config.max_travel[0];
servo_enable = true;
zero_setting = true;
break;
default:
break;
}
}
// Print menu
print_menu_items(sub_top, SERVOSTART, value_ptr, 1, (const unsigned char*)servo_menu_ranges[section - 1], 1, SERVOOFFSET, (const unsigned char*)ServoMenuText[section - 1], cursor);
// Handle menu changes
update_menu(SERVOITEMS, SERVOSTART, 0, button, &cursor, &sub_top, &menu_temp);
range = get_menu_range ((const unsigned char*)servo_menu_ranges[section - 1], 0);
if (button == ENTER)
{
text_link = pgm_read_byte(&ServoMenuText[section - 1][menu_temp - SERVOSTART]);
// Zero limits if adjusting
if (zero_setting)
{
value_ptr[menu_temp - SERVOSTART] = 0;
}
// Do not allow servo enable for throttle if in CPPM mode
if ((Config.Channel[menu_temp - SERVOSTART].P1_source_a == THROTTLE) && (Config.RxMode == CPPM_MODE))
{
servo_enable = false;
}
do_menu_item(menu_temp, value_ptr + (menu_temp - SERVOSTART), 1, range, 0, text_link, servo_enable, (menu_temp - SERVOSTART));
}
// Disable servos
servo_enable = false;
if (button == ENTER)
{
UpdateLimits(); // Update actual servo trims
Save_Config_to_EEPROM(); // Save value and return
}
}
}
void init(void)
{
uint8_t i;
bool updated;
//***********************************************************
// I/O setup
//***********************************************************
// Set port directions
DDRA = 0x30; // Port A
DDRB = 0x0A; // Port B
DDRC = 0xFC; // Port C
DDRD = 0xF2; // Port D
// Hold all PWM outputs low to stop glitches
// M5 and M6 are on PortA for KK2.1
MOTORS = 0;
M5 = 0;
M6 = 0;
// Preset I/O pins
LED1 = 0; // LED1 off
LVA = 0; // LVA alarm OFF
LCD_SCL = 1; // GLCD clock high
// Set/clear pull-ups (1 = set, 0 = clear)
PINB = 0xF5; // Set PB pull-ups
PIND = 0x0C; // Set PD pull-ups (Don't pull up RX yet)
//***********************************************************
// Spektrum receiver binding. Must be done immediately on power-up
//
// 3 low pulses: DSM2 1024/22ms
// 5 low pulses: DSM2 2048/11ms
// 7 low pulses: DSMX 1024/22ms
// 9 low pulses: DSMX 2048/11ms
//***********************************************************
PIND = 0x0C; // Release RX pull up on PD0
_delay_ms(63); // Pause while satellite wakes up
// and pull-ups have time to rise.
// Tweak until bind pulses about 68ms after power-up
// Bind as master if any single button pressed.
// NB: Have to wait until the button pull-ups rise before testing for a button press.
// Button 1
if ((PINB & 0xf0) == 0x70)
{
DDRD = 0xF3; // Switch PD0 to output
bind_master(3);
}
// Button 2
if ((PINB & 0xf0) == 0xb0)
{
DDRD = 0xF3; // Switch PD0 to output
bind_master(5);
}
// Button 3
if ((PINB & 0xf0) == 0xd0)
{
DDRD = 0xF3; // Switch PD0 to output
bind_master(7);
}
// Button 4
if ((PINB & 0xf0) == 0xE0)
{
DDRD = 0xF3; // Switch PD0 to output
bind_master(9);
}
DDRD = 0xF2; // Reset Port D directions
PIND = 0x0D; // Set PD pull-ups (now pull up RX as well)
//***********************************************************
// Timers
//***********************************************************
// Timer0 (8bit) - run @ 20MHz / 1024 = 19.531kHz or 51.2us - max 13.1ms
// Slow timer to extend Timer 1
TCCR0A = 0; // Normal operation
TCCR0B = 0x05; // Clk / 1024 = 19.531kHz or 51.2us - max 13.1ms
TIMSK0 |= (1 << TOIE0); // Enable interrupts
TCNT0 = 0; // Reset counter
// Timer1 (16bit) - run @ 2.5MHz (400ns) - max 26.2ms
// Used to measure Rx Signals & control ESC/servo output rate
TCCR1A = 0;
TCCR1B |= (1 << CS11); // Clk/8 = 2.5MHz
// Timer2 8bit - run @ 20MHz / 1024 = 19.531kHz or 51.2us - max 13.1ms
// Used to time arm/disarm intervals
TCCR2A = 0;
TCCR2B = 0x07; // Clk/1024 = 19.531kHz
TIMSK2 = 0;
TIFR2 = 0;
TCNT2 = 0; // Reset counter
//***********************************************************
// Interrupts and pin function setup
//***********************************************************
// Pin change interrupt enables PCINT1, PCINT2 and PCINT3 (Throttle, AUX and CPPM input)
PCICR = 0x0A; // PCINT8 to PCINT15 (PCINT1 group - AUX)
// PCINT24 to PCINT31 (PCINT3 group - THR)
PCIFR = 0x0F; // Clear PCIF0 interrupt flag
// Clear PCIF1 interrupt flag
// Clear PCIF2 interrupt flag
// Clear PCIF3 interrupt flag
// External interrupts INT0 (Elevator) and INT1 (Aileron) and INT2 (Rudder)
EICRA = 0x15; // Any change INT0
// Any change INT1
// Any change INT2
EIFR = 0x07; // Clear INT0 interrupt flag (Elevator)
// Clear INT1 interrupt flag (Aileron)
// Clear INT2 interrupt flag (Rudder/CPPM)
//***********************************************************
// Start up
//***********************************************************
// Preset important flags
Interrupted = false;
// Load EEPROM settings
updated = Initial_EEPROM_Config_Load(); // Config now contains valid values
//***********************************************************
// RX channel defaults for when no RC connected
// Not doing this can result in the FC trying (unsuccessfully) to arm
// and makes entry into the menus very hard
//***********************************************************
for (i = 0; i < MAX_RC_CHANNELS; i++)
{
RxChannel[i] = 3750;
}
RxChannel[THROTTLE] = 2500; // Min throttle
//***********************************************************
// GLCD initialisation
//***********************************************************
// Initialise the GLCD
st7565_init();
// Make sure the LCD is blank without clearing buffer (and so no logo)
clear_screen();
//***********************************************************
// ESC calibration
//***********************************************************
// Calibrate ESCs if ONLY buttons 1 and 4 pressed
if ((PINB & 0xf0) == 0x60)
{
// Display calibrating message
st7565_command(CMD_SET_COM_NORMAL); // For text (not for logo)
clear_buffer(buffer);
LCD_Display_Text(59,(const unsigned char*)Verdana14,10,25);
write_buffer(buffer);
clear_buffer(buffer);
// For each output
for (i = 0; i < MAX_OUTPUTS; i++)
{
// Check for motor marker
if (Config.Channel[i].Motor_marker == MOTOR)
{
// Set output to maximum pulse width
ServoOut[i] = MOTOR_100;
}
else
{
ServoOut[i] = SERVO_CENTER;
}
}
// Output HIGH pulse (1.9ms) until buttons released
while ((PINB & 0xf0) == 0x60)
{
// Pass address of ServoOut array and select all outputs
output_servo_ppm_asm(&ServoOut[0], 0xFF);
// Loop rate = 20ms (50Hz)
_delay_ms(20);
}
// Output LOW pulse (1.1ms) after buttons released
// For each output
for (i = 0; i < MAX_OUTPUTS; i++)
{
// Check for motor marker
if (Config.Channel[i].Motor_marker == MOTOR)
{
// Set output to maximum pulse width
ServoOut[i] = MOTOR_0;
}
}
// Loop forever here
while(1)
{
// Pass address of ServoOut array and select all outputs
output_servo_ppm_asm(&ServoOut[0], 0xFF);
// Loop rate = 20ms (50Hz)
_delay_ms(20);
}
}
//***********************************************************
// Reset EEPROM settings
//***********************************************************
// This delay prevents the GLCD flashing up a ghost image of old data
_delay_ms(300);
// Reload default eeprom settings if middle two buttons are pressed
if ((PINB & 0xf0) == 0x90)
{
// Display reset message
st7565_command(CMD_SET_COM_NORMAL); // For text (not for logo)
clear_buffer(buffer);
LCD_Display_Text(262,(const unsigned char*)Verdana14,40,25); // "Reset"
write_buffer(buffer);
clear_buffer(buffer);
// Reset EEPROM settings
Set_EEPROM_Default_Config();
Save_Config_to_EEPROM();
// Set contrast to the default value
st7565_set_brightness(Config.Contrast);
_delay_ms(500); // Save is now too fast to show the "Reset" text long enough
}
// Display message in place of logo when updating eeprom structure
if (updated)
{
st7565_command(CMD_SET_COM_NORMAL); // For text (not for logo)
clear_buffer(buffer);
LCD_Display_Text(259,(const unsigned char*)Verdana14,30,13); // "Updating"
LCD_Display_Text(260,(const unsigned char*)Verdana14,33,37); // "settings"
write_buffer(buffer);
clear_buffer(buffer);
_delay_ms(1000);
}
else
{
// Write logo from buffer
write_buffer(buffer);
_delay_ms(1000);
}
clear_buffer(buffer);
write_buffer(buffer);
st7565_init(); // Seems necessary for KK2 mini
//***********************************************************
// i2c init
//***********************************************************
i2c_init();
init_i2c_gyros();
init_i2c_accs();
//***********************************************************
// Remaining init tasks
//***********************************************************
// Display "Hold steady" message
clear_buffer(buffer);
st7565_command(CMD_SET_COM_NORMAL); // For text (not for logo)
LCD_Display_Text(263,(const unsigned char*)Verdana14,18,25); // "Hold steady"
write_buffer(buffer);
clear_buffer(buffer);
// Do startup tasks
Init_ADC();
init_int(); // Initialise interrupts based on RC input mode
init_uart(); // Initialise UART
// Initial gyro calibration
if (!CalibrateGyrosSlow())
{
clear_buffer(buffer);
LCD_Display_Text(61,(const unsigned char*)Verdana14,25,25); // "Cal. failed"
write_buffer(buffer);
_delay_ms(1000);
// Reset
cli();
wdt_enable(WDTO_15MS); // Watchdog on, 15ms
while(1); // Wait for reboot
}
// Update voltage detection
SystemVoltage = GetVbat(); // Check power-up battery voltage
UpdateLimits(); // Update travel and trigger limits
// Disarm on start-up if Armed setting is ARMABLE
if (Config.ArmMode == ARMABLE)
{
General_error |= (1 << DISARMED); // Set disarmed bit
}
// Check to see that throttle is low if RC detected
if (Interrupted)
{
RxGetChannels();
if (MonopolarThrottle > THROTTLEIDLE) // THROTTLEIDLE = 50
{
General_error |= (1 << THROTTLE_HIGH); // Set throttle high error bit
}
}
// Reset IMU
reset_IMU();
// Beep that init is complete
LVA = 1;
_delay_ms(25);
LVA = 0;
#ifdef ERROR_LOG
// Log reboot
add_log(REBOOT);
#endif
} // init()
void init(void)
{
//***********************************************************
// I/O setup
//***********************************************************
// Set port directions
// KK2.0 and KK2.1 are different
#ifdef KK21
DDRA = 0x30; // Port A
DDRC = 0xFC; // Port C
#else
DDRA = 0x00; // Port A
DDRC = 0xFF; // Port C
#endif
DDRB = 0x0A; // Port B
DDRD = 0xF2; // Port D
// Hold all PWM outputs low to stop glitches
// M5 and M6 are on PortA for KK2.1
MOTORS = 0;
M5 = 0;
M6 = 0;
// Preset I/O pins
LED1 = 0; // LED1 off
LVA = 0; // LVA alarm OFF
LCD_SCL = 1; // GLCD clock high
// Set/clear pull-ups (1 = set, 0 = clear)
PINB = 0xF5; // Set PB pull-ups
PIND = 0x0C; // Set PD pull-ups (Don't pull up RX yet)
//***********************************************************
// Spektrum receiver binding
//***********************************************************
_delay_ms(63); // Pause while satellite wakes up
// and pull-ups have time to rise.
// Tweak until bind pulses about 68ms after power-up
// Bind as master if ONLY button 4 pressed
if ((PINB & 0xf0) == 0xE0)
{
DDRD = 0xF3; // Switch PD0 to output
bind_master();
}
DDRD = 0xF2; // Reset Port D directions
// Set/clear pull-ups (1 = set, 0 = clear)
PIND = 0x0D; // Set PD pull-ups (now pull up RX as well)
//***********************************************************
// Timers
//***********************************************************
// Timer0 (8bit) - run @ 20MHz (50ns) - max 12.8us
// Fast timer for small, precise interval timing
TCCR0A = 0; // Normal operation
TCCR0B = (1 << CS00); // Clk / 1 = 20MHz = 50ns
TIMSK0 = 0; // No interrupts
// Timer1 (16bit) - run @ 2.5MHz (400ns) - max 26.2ms
// Used to measure Rx Signals & control ESC/servo output rate
TCCR1A = 0;
TCCR1B = (1 << CS11); // Clk/8 = 2.5MHz
// Timer2 8bit - run @ 20MHz / 1024 = 19.531kHz or 51.2us - max 13.1ms
// Used to time arm/disarm intervals
TCCR2A = 0;
TCCR2B = 0x07; // Clk/1024 = 19.531kHz
TIMSK2 = 0;
TIFR2 = 0;
TCNT2 = 0; // Reset counter
//***********************************************************
// Interrupts and pin function setup
//***********************************************************
// Pin change interrupt enables PCINT1, PCINT2 and PCINT3 (Throttle, AUX and CPPM input)
PCICR = 0x0A; // PCINT8 to PCINT15 (PCINT1 group - AUX)
// PCINT24 to PCINT31 (PCINT3 group - THR)
PCIFR = 0x0F; // Clear PCIF0 interrupt flag
// Clear PCIF1 interrupt flag
// Clear PCIF2 interrupt flag
// Clear PCIF3 interrupt flag
// External interrupts INT0 (Elevator) and INT1 (Aileron) and INT2 (Rudder)
EICRA = 0x15; // Any change INT0
// Any change INT1
// Any change INT2
EIFR = 0x07; // Clear INT0 interrupt flag (Elevator)
// Clear INT1 interrupt flag (Aileron)
// Clear INT2 interrupt flag (Rudder/CPPM)
//***********************************************************
// i2c init for KK2.1
//***********************************************************
#ifdef KK21
i2c_init();
init_i2c_gyros();
init_i2c_accs();
#endif
//***********************************************************
// Start up
//***********************************************************
// Preset important flags
Interrupted = false;
Main_flags |= (1 << FirstTimeIMU);
Main_flags |= (1 << FirstTimeFlightMode);
// Initialise the GLCD
st7565_init();
st7565_command(CMD_DISPLAY_ON);
st7565_command(CMD_SET_ALLPTS_NORMAL);
st7565_set_brightness(0x26);
st7565_command(CMD_SET_COM_REVERSE); // For logo
// Make sure the LCD is blank
clear_screen();
// This delay prevents the GLCD flashing up a ghost image of old data
_delay_ms(300);
// Reload default eeprom settings if middle two buttons are pressed (or all, for older users)
if (((PINB & 0xf0) == 0x90) || ((PINB & 0xf0) == 0x00))
{
// Display reset message
st7565_command(CMD_SET_COM_NORMAL); // For text (not for logo)
clear_buffer(buffer);
LCD_Display_Text(1,(prog_uchar*)Verdana14,40,25);
write_buffer(buffer,1);
clear_buffer(buffer);
Set_EEPROM_Default_Config();
Save_Config_to_EEPROM();
}
// Load "Config" global data structure
else
{
Initial_EEPROM_Config_Load();
}
// Now set contrast to the previously saved value
st7565_set_brightness((uint8_t)Config.Contrast);
#ifdef KK21
// Write logo from buffer
write_buffer(buffer,0);
_delay_ms(500);
#endif
#ifndef KK21
// Display "Hold steady" message for KK2.0
st7565_command(CMD_SET_COM_NORMAL); // For text (not for logo)
clear_buffer(buffer);
LCD_Display_Text(2,(prog_uchar*)Verdana14,18,25);
write_buffer(buffer,1);
clear_buffer(buffer);
#endif
// Do startup tasks
UpdateLimits(); // Update travel limts
UpdateIMUvalues(); // Update IMU factors
Init_ADC();
init_int(); // Intialise interrupts based on RC input mode
// Initialise UART
init_uart();
// Initial gyro calibration
CalibrateGyrosSlow();
// Check to see that gyros are stable
ReadGyros();
if ((gyroADC[ROLL] > GYROS_STABLE) || (gyroADC[ROLL] < -GYROS_STABLE) ||
(gyroADC[PITCH] > GYROS_STABLE) || (gyroADC[PITCH] < -GYROS_STABLE) ||
(gyroADC[YAW] > GYROS_STABLE) || (gyroADC[YAW] < -GYROS_STABLE))
{
General_error |= (1 << SENSOR_ERROR); // Set sensor error bit
}
// Check to see that throttle is low if in serial mode.
// Don't bother if in CamStab mode
_delay_ms(100);
if (
(
(Config.RxMode == CPPM_MODE) ||
(Config.RxMode == XTREME) ||
(Config.RxMode == SBUS) ||
(Config.RxMode == SPEKTRUM)
)
&& (Config.CamStab == OFF)
)
{
RxGetChannels();
if (RCinputs[THROTTLE] > -900)
{
General_error |= (1 << THROTTLE_HIGH); // Set throttle high error bit
}
}
// Flash LED
LED1 = 1;
_delay_ms(150);
LED1 = 0;
// Beep that all sensors have been handled
menu_beep(1);
// Set text display mode back to normal
st7565_command(CMD_SET_COM_NORMAL); // For text (not for logo)
} // init()
void 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 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 menu_rc_setup(uint8_t section)
{
static uint8_t rc_top = RCSTART;
int8_t *value_ptr;
menu_range_t range;
uint8_t text_link;
uint8_t i = 0;
uint8_t mult = 1; // Multiplier
uint8_t offset; // Index into channel structure
uint8_t items; // Items in group
uint16_t temp16_1;
// If submenu item has changed, reset submenu positions
if (menu_flag)
{
rc_top = RCSTART;
menu_flag = 0;
}
while(button != BACK)
{
// Get menu offsets and load values from eeprom
// 1 = RC, 2 = Failsafe, 3 = General, 4 = Battery
switch(section)
{
case 1: // RC setup menu
offset = 0;
items = RCITEMS;
value_ptr = &Config.RxMode;
mult = 1;
break;
case 2: // Failsafe menu
offset = RCITEMS;
items = FSITEMS;
value_ptr = &Config.FailsafeType;
mult = 1;
break;
case 3: // General menu
offset = RCITEMS + FSITEMS;
items = GENERALITEMS;
value_ptr = &Config.MixMode;
mult = 1;
break;
case 4: // Battery menu
offset = RCITEMS + FSITEMS + GENERALITEMS;
items = BATTITEMS;
value_ptr = &Config.BatteryType;
mult = 4;
break;
default:
offset = 0;
items = RCITEMS;
value_ptr = &Config.RxMode;
mult = 1;
break;
}
// Save pre-edited values
int8_t temp_type = Config.MixMode;
int8_t temp_cells = Config.BatteryCells;
int8_t temp_minvoltage = Config.MinVoltage;
int8_t temp_flapchan = Config.FlapChan;
// Print menu
print_menu_items(rc_top + offset, RCSTART + offset, value_ptr, mult, (prog_uchar*)rc_menu_ranges[section - 1], 0, RCOFFSET, (prog_uchar*)RCMenuText[section - 1], cursor);
// Handle menu changes
update_menu(items, RCSTART, offset, button, &cursor, &rc_top, &menu_temp);
range = get_menu_range ((prog_uchar*)rc_menu_ranges[section - 1], (menu_temp - RCSTART - offset));
if (button == ENTER)
{
text_link = pgm_read_byte(&RCMenuText[section - 1][menu_temp - RCSTART - offset]);
do_menu_item(menu_temp, value_ptr + (menu_temp - RCSTART - offset), mult, range, 0, text_link, false, 0);
}
if (button == ENTER)
{
// Update Ch7. mixer with source from Config.FlapChan if in Aeroplane mode and source changed
if ((Config.MixMode == AEROPLANE) && (Config.FlapChan != temp_flapchan))
{
Config.Channel[CH7].source_a = Config.FlapChan;
}
// See if cell number or min_volts has changed
if ((temp_cells != Config.BatteryCells) || (temp_minvoltage != Config.MinVoltage))
{
// Recalculate if more cells
temp16_1 = Config.MinVoltage;
temp16_1 = temp16_1 * Config.BatteryCells;
temp16_1 = temp16_1 / 10;
Config.PowerTrigger = (int8_t)temp16_1;
}
// If model type has changed, reload preset
if ((section == 3) && (temp_type != Config.MixMode))
{
switch(Config.MixMode) // Load selected mix
{
case AEROPLANE:
get_preset_mix(AEROPLANE_MIX);
break;
case FWING:
get_preset_mix(FLYING_WING_MIX);
break;
case CAMSTAB:
get_preset_mix(CAM_STAB);
break;
case MANUAL:
// Clear all channel info
memset(&Config.Channel[0].value,0,(sizeof(channel_t) * PSUEDO_OUTPUTS));
// Preset important settings
for (i = 0; i < PSUEDO_OUTPUTS; i++)
{
Config.Channel[i].source_a = NOCHAN;
Config.Channel[i].source_b = NOCHAN;
Config.Channel[i].output_b = UNUSED;
Config.Channel[i].output_c = UNUSED;
Config.Channel[i].output_d = UNUSED;
}
break;
default:
break;
}
}
init_int(); // In case RC type has changed, reinitialise interrupts
init_uart(); // and UART
UpdateIMUvalues(); // Update IMU variables
UpdateLimits(); // Update I-term limits and triggers based on percentages
#ifdef KK21
// Update MPU6050 LPF
writeI2Cbyte(MPU60X0_DEFAULT_ADDRESS, MPU60X0_RA_CONFIG, Config.MPU6050_LPF);
#endif
// Update channel sequence
for (i = 0; i < MAX_RC_CHANNELS; i++)
{
if (Config.TxSeq == FUTABASEQ)
{
Config.ChannelOrder[i] = (uint8_t)pgm_read_byte(&FUTABA[i]);
}
else
{
Config.ChannelOrder[i] = (uint8_t)pgm_read_byte(&JR[i]);
}
}
Save_Config_to_EEPROM(); // Save value and return
}
}
}
