void boardsupport_init(void) { central_data_t *central_data; irq_initialize_vectors(); cpu_irq_enable(); Disable_global_interrupt(); sysclk_init(); //delay_init(sysclk_get_cpu_hz()); INTC_init_interrupts(); central_data=central_data_get_pointer_to_struct(); #ifdef TELEMETRY_USE_UART uart_int_set_usart_conf(TELEMETRY_UART_INDEX, asv_debug_uart_conf()); //uart configuration uart_int_init(TELEMETRY_UART_INDEX); uart_int_register_write_stream(uart_int_get_uart_handle(TELEMETRY_UART_INDEX), &(central_data->wired_out_stream)); // Registering streams buffer_make_buffered_stream_lossy(&(wired_in_buffer), &(central_data->wired_in_stream)); uart_int_register_read_stream(uart_int_get_uart_handle(TELEMETRY_UART_INDEX), &(central_data->wired_in_stream)); central_data->debug_in_stream=¢ral_data->wired_in_stream; central_data->debug_out_stream=¢ral_data->wired_out_stream; #endif central_data_init(); Enable_global_interrupt(); }
void initialisation() { central_data = central_data_get_pointer_to_struct(); boardsupport_init(central_data); central_data_init(); mavlink_telemetry_init(); //onboard_parameters_write_parameters_to_flashc(¢ral_data->mavlink_communication.onboard_parameters); onboard_parameters_read_parameters_from_flashc(¢ral_data->mavlink_communication.onboard_parameters); central_data->state.mav_state = MAV_STATE_STANDBY; central_data->imu.calibration_level = OFF; piezo_speaker_quick_startup(); // Switch off red LED LED_Off(LED2); print_util_dbg_print("OK. Starting up.\r\n"); }