void initialise_board(void) {
int i;
central_data_t *central_data=central_data_get_pointer_to_struct();
irq_initialize_vectors();
cpu_irq_enable();
Disable_global_interrupt();
// Initialize the sleep manager
sleepmgr_init();
sysclk_init();
INTC_init_interrupts();
delay_init(sysclk_get_cpu_hz());
time_keeper_init();
// time_keeper_init_synchronisation();
init_LEDs();
switch_LED(1, LED_ON);
switch_LED(0, LED_ON);
pwm_servos_init(true);
// set up UART for main telemetry
uart_int_set_usart_conf(0, asv_xbee_uart_conf());
uart_int_init(0);
uart_int_register_write_stream(uart_int_get_uart_handle(0), ¢ral_data->xbee_out_stream);
//init_UART_DMA(0);
//register_write_stream_dma(get_UART_handle(0), ¢ral_data->xbee_out_stream);
buffer_make_buffered_stream(&(central_data->xbee_in_buffer), &(central_data->xbee_in_stream));
uart_int_register_read_stream(uart_int_get_uart_handle(0), &(central_data->xbee_in_stream));
uart_int_set_usart_conf(4, asv_debug_uart_conf());
uart_int_init(4);
uart_int_register_write_stream(uart_int_get_uart_handle(4), ¢ral_data->wired_out_stream);
//init_UART_DMA(0);
//register_write_stream_dma(get_UART_handle(0), ¢ral_data->wired_out_stream);
buffer_make_buffered_stream(&(central_data->wired_in_buffer), &(central_data->wired_in_stream));
uart_int_register_read_stream(uart_int_get_uart_handle(4), &(central_data->wired_in_stream));
// set up UART for GPS
uart_int_set_usart_conf(3, asv_gps_uart_conf());
uart_int_init(3);
uart_int_register_write_stream(uart_int_get_uart_handle(3), ¢ral_data->gps_out_stream);
buffer_make_buffered_stream(&(central_data->gps_in_buffer), &(central_data->gps_in_stream));
uart_int_register_read_stream(uart_int_get_uart_handle(3), &(central_data->gps_in_stream));
multistream_init(¢ral_data->multi_telemetry_down_stream);
multistream_init(¢ral_data->multi_telemetry_up_stream);
multistream_add_stream(¢ral_data->multi_telemetry_down_stream, ¢ral_data->wired_out_stream);
multistream_add_stream(¢ral_data->multi_telemetry_down_stream, ¢ral_data->xbee_out_stream);
multistream_add_stream(¢ral_data->multi_telemetry_up_stream, ¢ral_data->wired_in_stream);
multistream_add_stream(¢ral_data->multi_telemetry_up_stream, ¢ral_data->xbee_in_stream);
/*
can_bus_init(1, asv_can1_conf());
can_bus_register_write_stream(¢ral_data->ext_can_out_stream_data, ¢ral_data->ext_can_out_stream, 1, MAVLINK_COMPONENT_ID, 0x3ff);
can_bus_register_read_stream(¢ral_data->ext_can_in_stream_data, ¢ral_data->ext_can_in_stream, 1, MAVLINK_COMPONENT_ID, 0x00);
*/
// set telemetry stream for mavlink
//central_data->telemetry_down_stream=&(central_data->xbee_out_stream);
//central_data->telemetry_up_stream =&(central_data->xbee_in_stream);
//central_data->telemetry_down_stream=&(central_data->wired_out_stream);
//central_data->telemetry_up_stream =&(central_data->wired_in_stream);
central_data->telemetry_down_stream= multistream_get_stream(¢ral_data->multi_telemetry_down_stream);
central_data->telemetry_up_stream = multistream_get_stream(¢ral_data->multi_telemetry_up_stream);
central_data->debug_out_stream=¢ral_data->wired_out_stream;
central_data->debug_in_stream=¢ral_data->wired_in_stream;
//central_data->debug_out_stream = ¢ral_data->xbee_out_stream;
//central_data->debug_in_stream = ¢ral_data->xbee_in_stream;
print_util_dbg_print_init(central_data->debug_out_stream);
// init error handler as plain text until mavlink is configured
error_handler_init_plaintext(central_data->debug_out_stream);
// init mavlink
// Init mavlink communication
mavlink_communication_conf_t mavlink_config =
{
.scheduler_config =
{
.max_task_count = 30,
.schedule_strategy = ROUND_ROBIN,
.debug = true
},
.mavlink_stream_config =
{
.rx_stream = central_data->telemetry_up_stream,
.tx_stream = central_data->telemetry_down_stream,
.sysid = MAVLINK_SYS_ID,
.compid = MAVLINK_COMPONENT_ID,
.use_dma = false
},
.message_handler_config =
void boardsupport_init(central_data_t *central_data)
{
irq_initialize_vectors();
cpu_irq_enable();
Disable_global_interrupt();
// Initialize the sleep manager
sleepmgr_init();
sysclk_init();
board_init();
delay_init(sysclk_get_cpu_hz());
time_keeper_init();
INTC_init_interrupts();
// Switch on the red LED
LED_On(LED2);
// servo_pwm_hardware_init();
pwm_servos_init( CS_ON_SERVO_7_8 );
// Init UART 0 for XBEE communication
xbee_init(UART0);
// Init UART 4 for wired communication
//console_init(CONSOLE_UART4);
// Init USB for wired communication
console_init(CONSOLE_USB);
// connect abstracted aliases to hardware ports
central_data->telemetry_down_stream = xbee_get_out_stream();
central_data->telemetry_up_stream = xbee_get_in_stream();
central_data->debug_out_stream = console_get_out_stream();
central_data->debug_in_stream = console_get_in_stream();
// init debug output
print_util_dbg_print_init(central_data->debug_out_stream);
print_util_dbg_print("Debug stream initialised\r\n");
// RC receiver initialization
spektrum_satellite_init();
// init imu & compass
i2c_driver_init(I2C0);
lsm330dlc_init();
print_util_dbg_print("LSM330 initialised \r\n");
hmc5883l_init_slow();
print_util_dbg_print("HMC5883 initialised \r\n");
// init radar or ultrasound (not implemented yet)
//i2c_driver_init(I2C1);
// init 6V enable
gpio_enable_gpio_pin(AVR32_PIN_PA04);
gpio_set_gpio_pin(AVR32_PIN_PA04);
Enable_global_interrupt();
// Init piezo speaker
piezo_speaker_init_binary();
print_util_dbg_print("Board initialised\r\n");
}
bool Mavrimini::init(void)
{
bool init_success = true;
bool ret;
// -------------------------------------------------------------------------
// Init clock
// -------------------------------------------------------------------------
clock_setup();
time_keeper_init();
// -------------------------------------------------------------------------
// Init LEDs
// -------------------------------------------------------------------------
ret = green_led_gpio.init();
ret = red_led_gpio.init();
green_led.on();
red_led.on();
// -------------------------------------------------------------------------
// Init SERIAL1
// -------------------------------------------------------------------------
ret = serial_1.init();
init_success &= ret;
// -------------------------------------------------------------------------
// Init SERIAL2
// -------------------------------------------------------------------------
ret = serial_2.init();
init_success &= ret;
// -------------------------------------------------------------------------
// Init stream for USB debug stream TODO: remove
p_dbg_serial = &serial_1;
//p_dbg_serial = &serial_2;
dbg_stream_.get = NULL;
dbg_stream_.put = &serial2stream;
dbg_stream_.flush = NULL;
dbg_stream_.buffer_empty = NULL;
dbg_stream_.data = NULL;
print_util_dbg_print_init(&dbg_stream_);
// -------------------------------------------------------------------------
print_util_dbg_sep('%');
p_dbg_serial->flush();
print_util_dbg_sep('-');
p_dbg_serial->flush();
print_util_dbg_print("[MAVRIMINI] ...\r\n");
p_dbg_serial->flush();
print_util_dbg_sep('-');
p_dbg_serial->flush();
print_util_dbg_init_msg("[SERIAL1]", ret);
print_util_dbg_init_msg("[SERIAL2]", ret);
p_dbg_serial->flush();
// -------------------------------------------------------------------------
// Init Servos
// -------------------------------------------------------------------------
ret = pwm_0.init();
print_util_dbg_init_msg("[PWM0]", ret);
init_success &= ret;
servo_0.failsafe();
p_dbg_serial->flush();
ret = pwm_1.init();
print_util_dbg_init_msg("[PWM1]", ret);
init_success &= ret;
servo_1.failsafe();
p_dbg_serial->flush();
ret = pwm_2.init();
print_util_dbg_init_msg("[PWM2]", ret);
init_success &= ret;
servo_2.failsafe();
ret = pwm_3.init();
print_util_dbg_init_msg("[PWM3]", ret);
init_success &= ret;
servo_3.failsafe();
p_dbg_serial->flush();
ret = pwm_4.init();
print_util_dbg_init_msg("[PWM4]", ret);
init_success &= ret;
servo_4.failsafe();
p_dbg_serial->flush();
ret = pwm_5.init();
print_util_dbg_init_msg("[PWM5]", ret);
init_success &= ret;
servo_5.failsafe();
p_dbg_serial->flush();
ret = pwm_6.init();
print_util_dbg_init_msg("[PWM6]", ret);
init_success &= ret;
servo_6.failsafe();
p_dbg_serial->flush();
ret = pwm_7.init();
print_util_dbg_init_msg("[PWM7]", ret);
init_success &= ret;
servo_7.failsafe();
p_dbg_serial->flush();
// -------------------------------------------------------------------------
// Init spektrum_satelitte
// -------------------------------------------------------------------------
ret = spektrum_satellite.init();
print_util_dbg_init_msg("[SAT]", ret);
init_success &= ret;
p_dbg_serial->flush();
print_util_dbg_sep('-');
p_dbg_serial->flush();
print_util_dbg_init_msg("[MAVRIMINI]", init_success);
p_dbg_serial->flush();
print_util_dbg_sep('-');
p_dbg_serial->flush();
return init_success;
}
void boardsupport_init(central_data_t *central_data)
{
irq_initialize_vectors();
cpu_irq_enable();
Disable_global_interrupt();
// Initialize the sleep manager
sleepmgr_init();
sysclk_init();
board_init();
delay_init(sysclk_get_cpu_hz());
time_keeper_init();
INTC_init_interrupts();
// Switch on the red LED
LED_On(LED2);
// servo_pwm_hardware_init();
pwm_servos_init( CS_ON_SERVO_7_8 );
// Init UART 0 for XBEE communication
xbee_init(UART0);
// Init UART 3 for GPS communication
gps_ublox_init(&(central_data->gps), UART3);
// Init UART 4 for wired communication
//console_init(CONSOLE_UART4);
// Init USB for wired communication
console_init(CONSOLE_USB);
// connect abstracted aliases to hardware ports
central_data->telemetry_down_stream = xbee_get_out_stream();
central_data->telemetry_up_stream = xbee_get_in_stream();
central_data->debug_out_stream = console_get_out_stream();
central_data->debug_in_stream = console_get_in_stream();
// init debug output
print_util_dbg_print_init(central_data->debug_out_stream);
print_util_dbg_print("Debug stream initialised\r\n");
// Bind RC receiver with remote
// spektrum_satellite_bind();
// RC receiver initialization
spektrum_satellite_init();
// Init analog rails
analog_monitor_conf_t analog_monitor_config = analog_monitor_default_config;
//analog_monitor_config.conv_factor[ANALOG_RAIL_6] = 0.00023485f * 6.6f;
//analog_monitor_config.conv_factor[ANALOG_RAIL_7] = 0.00023485f * 6.6f;
//analog_monitor_config.conv_factor[ANALOG_RAIL_10] = -0.0002409f * 11.0f;
//analog_monitor_config.conv_factor[ANALOG_RAIL_11] = -0.0002409f * 11.0f;
analog_monitor_init(¢ral_data->analog_monitor, &analog_monitor_config);
// init imu & compass
i2c_driver_init(I2C0);
lsm330dlc_init();
print_util_dbg_print("LSM330 initialised \r\n");
hmc5883l_init_slow();
print_util_dbg_print("HMC5883 initialised \r\n");
bmp085_init(¢ral_data->pressure);
// init radar or ultrasound (not implemented yet)
//i2c_driver_init(I2C1);
// init 6V enable
gpio_enable_gpio_pin(AVR32_PIN_PA04);
gpio_set_gpio_pin(AVR32_PIN_PA04);
Enable_global_interrupt();
// Init piezo speaker
piezo_speaker_init_binary();
print_util_dbg_print("Board initialised\r\n");
}
