void _attitude_read_tuning() {
uint8_t data[24];
uint8_t length = persist_read(PERSIST_SECTION_ATTITUDE, data, 24);
if (length == 24) {
state_x.q_angle = convert_bytes_to_double(data, 0);
state_x.q_gyro = convert_bytes_to_double(data, 4);
state_x.r_angle = convert_bytes_to_double(data, 8);
state_y.q_angle = convert_bytes_to_double(data, 12);
state_y.q_gyro = convert_bytes_to_double(data, 16);
state_y.r_angle = convert_bytes_to_double(data, 20);
} else {
status_error(STATUS_ERR_ATTITUDE_CHECKSUM);
_delay_ms(1000);
status_error(0x00);
state_x.q_angle = 0.001;
state_x.q_gyro = 0.003;
state_x.r_angle = 0.03;
state_y.q_angle = 0.001;
state_y.q_gyro = 0.003;
state_y.r_angle = 0.03;
}
}
void start_up() {
// Check and create our 400PLUS folder
status.folder_exists = check_create_folder();
// Recover persisting information
persist_read();
// Read settings from file
settings_read();
// If configured, start debug mode
if (settings.debug_on_poweron)
start_debug_mode();
// If configured, restore AEB
if (settings.persist_aeb)
send_to_intercom(IC_SET_AE_BKT, persist.aeb);
// Enable IR remote
// i'm not sure where to call this? perhaps this isn't the right place.
if (settings.remote_enable)
remote_on();
// Enable extended ISOs
// Enable (hidden) CFn.8 for ISO H
send_to_intercom(IC_SET_CF_EXTEND_ISO, 1);
// Enable realtime ISO change
send_to_intercom(IC_SET_REALTIME_ISO_0, 0);
send_to_intercom(IC_SET_REALTIME_ISO_1, 0);
// Set current language
enqueue_action(lang_pack_init);
// Read custom modes configuration from file
enqueue_action(cmodes_read);
// And optionally apply a custom mode
enqueue_action(cmode_recall);
// turn off the blue led after it was lighten by our hack_task_MainCtrl()
eventproc_EdLedOff();
#ifdef MEMSPY
debug_log("starting memspy task");
CreateTask("memspy", 0x1e, 0x1000, memspy_task, 0);
#endif
#if 0
debug_log("=== DUMPING DDD ===");
printf_DDD_log( (void*)(int)(0x00007604+0x38) );
debug_log("maindlg @ 0x%08X, handler @ 0x%08X", hMainDialog, hMainDialog->event_handler);
debug_log("dumping");
long *addr = (long*) 0x7F0000;
int file = FIO_OpenFile("A:/dump.bin", O_CREAT | O_WRONLY , 644);
if (file != -1) {
FIO_WriteFile(file, addr, 0xFFFF);
FIO_CloseFile(file);
beep();
}
#endif
}
