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 }