/* initialize for touch & calibration */
int touch_calibration_init(void)
{
rt_uint8_t magic = 0;
calculate_data_t data;
struct rtgui_calibration_ops *ops;
ops = calibration_get_ops();
/* initialization the eeprom on chip */
EEPROM_Init();
/* initialization the touch driver */
rtgui_touch_hw_init("spi10");
/* set callback to save calibration data */
calibration_set_after(calibration_data_save);
/* initialization rtgui tonch server */
rtgui_touch_init(ops);
/* restore calibration data */
EEPROM_Read(0, CALIBRATION_DATA_PAGE, &magic, MODE_8_BIT, 1);
if (CALIBRATION_DATA_MAGIC != magic)
{
rt_kprintf("touch is not calibration,now calibration it please.\n");
calibration_init(RT_NULL);
}
else
{
EEPROM_Read(CALIBRATION_DATA_OFFSET, CALIBRATION_DATA_PAGE, &data, MODE_8_BIT, sizeof(calculate_data_t));
calibration_set_data(&data);
}
/* power down the EEPROM */
EEPROM_PowerDown(ENABLE);
return 0;
}
void rt_init_thread_entry(void* parameter)
{
#ifdef RT_USING_COMPONENTS_INIT
/* initialization RT-Thread Components */
rt_components_init();
#endif
#ifdef RT_USING_FINSH
finsh_set_device(RT_CONSOLE_DEVICE_NAME);
#endif /* RT_USING_FINSH */
/* Filesystem Initialization */
#if defined(RT_USING_DFS) && defined(RT_USING_DFS_ELMFAT)
/* mount sd card fat partition 1 as root directory */
if (dfs_mount("sd0", "/", "elm", 0, 0) == 0)
{
rt_kprintf("File System initialized!\n");
}
else
rt_kprintf("File System initialzation failed!\n");
#endif /* RT_USING_DFS */
#ifdef RT_USING_RTGUI
{
extern void rt_hw_lcd_init();
extern void rtgui_touch_hw_init(void);
rt_device_t lcd;
/* init lcd */
rt_hw_lcd_init();
/* init touch panel */
rtgui_touch_hw_init();
/* re-init device driver */
rt_device_init_all();
/* find lcd device */
lcd = rt_device_find("lcd");
/* set lcd device as rtgui graphic driver */
rtgui_graphic_set_device(lcd);
#ifndef RT_USING_COMPONENTS_INIT
/* init rtgui system server */
rtgui_system_server_init();
#endif
calibration_set_restore(cali_setup);
calibration_set_after(cali_store);
calibration_init();
}
#endif /* #ifdef RT_USING_RTGUI */
}
/* Alternative wrapper to SwStart, for use when the core is in a statically-
linked library rather than a DLL. See request 50940. Code duplication with
SwDllStart can be solved with some refactoring, but that can wait until
after this approach to supporting the static-link model has been
evaluated. This method might well disappear altogether. The prototype
for this function is not part of the public LE interface - it is not
seen by customers. */
void RIPCALL SwLibStart( SWSTART * start, DllFuncs * pDllFuncs )
{
int32 fSecurityPassed;
if( pDllFuncs )
{
pfnSwExit = pDllFuncs->pfnSwExit;
pfnSwReboot = pDllFuncs->pfnSwReboot;
pfnSwWarn = pDllFuncs->pfnSwWarn;
pfnSwStartTickleTimer = pDllFuncs->pfnSwStartTickleTimer;
pfnSwStopTickleTimer = pDllFuncs->pfnSwStopTickleTimer;
set_assert_handlers(pDllFuncs) ;
}
startTickleTimerWrap () ;
if (!dllskin_meminit(start) )
return ;
#ifdef HAS_FWOS
BootFrameWork();
#endif
if ( !unicode_init("Unicode",
&unicode_fileio, /*file handler*/
&unicode_memory /*memory handler*/) ) {
SecurityExit(1, (uint8 *)"Unicode initialisation failed") ;
return ;
}
if ( !calibration_init() ) {
SecurityExit(1, (uint8 *)"Calibration initialisation failed") ;
return ;
}
secDevDetermineMethod();
fSecurityPassed = startupDongleTestSilently();
fSecurityPassed = startupDongleTestReport(fSecurityPassed);
if (fSecurityPassed)
{
/* Allow ICU file functions to succeed from now on. */
swstart_called = TRUE ;
(void)SwStart( start );
}
}
int main(void) {
BootLoaderStart();
firstInit();
set_defaults_all(&Preset, &Calibration);
if (!start_load_all(&Preset, &Calibration))
set_errIO(ERRIO_LOADALL);
interface_init(Current_state.preset_name);
calibration_init(Current_state.calibration_name);
noteOffStoreInit();
TIM4_init();//keyboard timer
TIM6_init();//delay for temp messages
TIM7_init(); //delay for MSC
//Main loop
while (1) {
//Check note array to calculate velocity
checkNoteArray(&Preset);
checkSliders_events(Preset.sliders, Preset.AnalogMidiEnable);
checkButtons_events(Preset.buttons, Preset.AnalogMidiEnable);
receiveMidiData();
sendMidiData();
}
}
/**
****************************************************************************************
* @brief Setup the microcontroller system.
*
* Initialize the system clock and pins.
*****************************************************************************************
*/
void SystemInit(void)
{
/*
**************************
* Sub module clock setting
**************************
*/
syscon_SetIvrefX32WithMask(QN_SYSCON, SYSCON_MASK_DVDD12_SW_EN, MASK_ENABLE);
syscon_set_sysclk_src(CLK_XTAL, __XTAL);
#if __XTAL == XTAL_32MHz
calibration_init(XTAL_32M);
#else
calibration_init(XTAL_16M);
#endif
ref_pll_calibration();
clk32k_enable(__32K_TYPE);
#if QN_32K_RCO
rco_calibration();
#endif
// Reset SPI1 module(since the default register value was changed in bootloader)
syscon_SetCRSS(QN_SYSCON, SYSCON_MASK_USART1_RST);
syscon_SetCRSC(QN_SYSCON, SYSCON_MASK_USART1_RST);
/*
Disable all peripheral clock, will be enabled in the driver initilization.
The next function performs the equivalent effect of a collection of these functions.
timer_clock_off(QN_TIMER0);
timer_clock_off(QN_TIMER1);
timer_clock_off(QN_TIMER2);
timer_clock_off(QN_TIMER3);
uart_clock_off(QN_UART0);
uart_clock_off(QN_UART1);
spi_clock_off(QN_SPI0);
spi_clock_off(QN_SPI1);
flash_clock_off();
gpio_clock_off();
adc_clock_off();
dma_clock_off();
pwm_clock_off();
*/
syscon_SetCRSS(QN_SYSCON, SYSCON_MASK_GATING_TIMER0
| SYSCON_MASK_GATING_TIMER1
| SYSCON_MASK_GATING_TIMER2
| SYSCON_MASK_GATING_TIMER3
| SYSCON_MASK_GATING_UART0
| SYSCON_MASK_GATING_UART1
| SYSCON_MASK_GATING_SPI0
| SYSCON_MASK_GATING_SPI1
| SYSCON_MASK_GATING_SPI_AHB
| SYSCON_MASK_GATING_GPIO
| SYSCON_MASK_GATING_ADC
| SYSCON_MASK_GATING_DMA
| SYSCON_MASK_GATING_PWM);
// calibration changed system clock setting
// Configure sytem clock.
syscon_set_sysclk_src(CLK_XTAL, __XTAL);
syscon_set_ahb_clk(__AHB_CLK);
syscon_set_apb_clk(__APB_CLK);
syscon_set_ble_clk(__BLE_CLK);
syscon_set_timer_clk(__TIMER_CLK);
syscon_set_usart_clk((uint32_t)QN_UART0, __USART_CLK);
syscon_set_usart_clk((uint32_t)QN_UART1, __USART_CLK);
/*
**************************
* IO configuration
**************************
*/
SystemIOCfg();
/*
**************************
* Peripheral setting
**************************
*/
}
void calibration()
{
calibration_init();
}
int main(int argc, char **argv)
{
QApplication app(argc,argv);
QGst::init();
Analyzer left_stream,right_stream;
VideoIN left_cam, right_cam;
stereo_vision stereo;
BDTargeting valdymas;
Gui langas;
QObject::connect(langas.window.calibrate_button, SIGNAL(clicked()), &stereo, SLOT(calibration_init()));
QObject::connect(langas.window.display_undistorted, SIGNAL(toggled(bool)), &stereo, SLOT(show_undistorted(bool)));
QObject::connect(&stereo, SIGNAL(calibration_running(bool)), &langas, SLOT(calibration_status(bool)));
QObject::connect(&stereo, SIGNAL(calibration_running(bool)), langas.window.calibration_process, SLOT(setVisible(bool)));
QObject::connect(&stereo, SIGNAL(calibration_progress(int)), langas.window.calibration_process, SLOT(setValue(int)));
QObject::connect(langas.window.sad_window_size_spinbox, SIGNAL(valueChanged(int)), &stereo, SLOT(SADWindowSize_changed(int)));
QObject::connect(langas.window.number_of_disparities_spinbox, SIGNAL(valueChanged(int)), &stereo, SLOT(numberOfDisparities_changed(int)));
QObject::connect(langas.window.prefilter_size_spinbox, SIGNAL(valueChanged(int)), &stereo, SLOT(preFilterSize_changed(int)));
QObject::connect(langas.window.prefilter_cap_spinbox, SIGNAL(valueChanged(int)), &stereo, SLOT(preFilterCap_changed(int)));
QObject::connect(langas.window.min_disparity_spinbox, SIGNAL(valueChanged(int)), &stereo, SLOT(minDisparity_changed(int)));
QObject::connect(langas.window.texture_Threshold_spinbox, SIGNAL(valueChanged(int)), &stereo, SLOT(textureThreshold_changed(int)));
QObject::connect(langas.window.uniqueness_ratio_spinbox, SIGNAL(valueChanged(int)), &stereo, SLOT(uniquenessRatio_changed(int)));
QObject::connect(langas.window.speckle_window_size_spinbox, SIGNAL(valueChanged(int)), &stereo, SLOT(speckleWindowSize_changed(int)));
QObject::connect(langas.window.speckle_range_spinbox, SIGNAL(valueChanged(int)), &stereo, SLOT(speckleRange_changed(int)));
QObject::connect(langas.window.disp12_maxdiff_spinbox, SIGNAL(valueChanged(int)), &stereo, SLOT(disp12MaxDiff_changed(int)));
QObject::connect(langas.window.acumulate_sensitivity_spinbox, SIGNAL(valueChanged(int)), &left_stream, SLOT(change_acumulate_sensitivity(int)));
QObject::connect(langas.window.threshold_sensitivity_spinbox, SIGNAL(valueChanged(int)), &left_stream, SLOT(change_threshold_sensitivity(int)));
QObject::connect(langas.window.tabs, SIGNAL(currentChanged(int)), &langas, SLOT(refresh_view_in_tab(int)));
QObject::connect(&left_stream, SIGNAL(frame_analyze_length(int)), langas.window.time_to_calculate, SLOT(display(int)));
QObject::connect(&left_stream, SIGNAL(number_of_features_to_track(int)), langas.window.num_of_tracked_features, SLOT(display(int)));
QObject::connect(&left_stream, SIGNAL(tracked_object_x(int)), langas.window.horizontal_data, SLOT(setNum(int)));
QObject::connect(&left_stream, SIGNAL(tracked_object_y(int)), langas.window.vertical_data, SLOT(setNum(int)));
QObject::connect(langas.window.load_calibration_data_depth, SIGNAL(clicked(bool)), &stereo, SLOT(load_data()));
QObject::connect(langas.window.save_calibration_data_depth, SIGNAL(clicked(bool)), &stereo, SLOT(save_data()));
QObject::connect(&stereo, SIGNAL(refresh_depth_settings(cv::Ptr<CvStereoBMState>)), &langas, SLOT(refresh_depth_data(cv::Ptr<CvStereoBMState>)));
QObject::connect(langas.window.save_calibration_data_raw, SIGNAL(clicked(bool)), &stereo, SLOT(save_calibration_file()));
QObject::connect(langas.window.load_calibration_data_raw, SIGNAL(clicked(bool)), &stereo, SLOT(load_calibration_file()));
QObject::connect(&left_stream, SIGNAL(get_depth(cv::Point2f)), &stereo, SLOT(depth_report(cv::Point2f)));
QObject::connect(&stereo, SIGNAL(give_depth_to_gui(int)), langas.window.depth_data, SLOT(setNum(int)));
QObject::connect(langas.window.start_tracking, SIGNAL(clicked(bool)), &left_stream, SLOT(start_tracking_function(bool)));
QObject::connect(langas.window.calibrate_laser, SIGNAL(clicked(bool)), &left_stream, SLOT(laser_calibration(bool)));
QObject::connect(langas.window.x_koordinates, SIGNAL(valueChanged(int)), &left_stream, SLOT(laser_change_x(int)));
QObject::connect(langas.window.y_koordinates, SIGNAL(valueChanged(int)), &left_stream, SLOT(laser_change_y(int)));
QObject::connect(&left_stream, SIGNAL(laser_calibration_status(bool)), &langas, SLOT(laser_calibration_status(bool)));
QObject::connect(langas.window.fix_laser_position, SIGNAL(clicked(bool)), &left_stream, SLOT(fix_laser_position()));
QObject::connect(langas.window.config_but, SIGNAL(clicked(bool)), &langas, SLOT(check_config()));
QObject::connect(langas.window.test_but, SIGNAL(clicked(bool)), &langas, SLOT(test_device()));
QObject::connect(langas.window.send_but, SIGNAL(clicked(bool)), &langas, SLOT(DATA_SEND_EXAMPLE()));
QObject::connect(langas.window.circle_but, SIGNAL(clicked(bool)), &langas, SLOT(DATA_SEND_EXAMPLE_2()));
QObject::connect(langas.window.random_pos_but, SIGNAL(clicked(bool)), &langas, SLOT(DATA_SEND_EXAMPLE_3()));
QObject::connect(langas.window.left_cam_flip, SIGNAL(clicked(bool)), &left_cam, SLOT(flip_image()));
QObject::connect(langas.window.right_cam_flip, SIGNAL(clicked(bool)), &right_cam, SLOT(flip_image()));
// QObject::connect(langas.window, SIGNAL(), &langas, SLOT(close_app()));
left_cam.irenginys = "/dev/video1";
left_cam.create_pipeline();
left_cam.start_stream();
right_cam.irenginys = "/dev/video0";
right_cam.create_pipeline();
right_cam.start_stream();
int blink = 0;
langas.show();
libusb_device_handle *dev_handle; // irenginio handle
libusb_context *ctx = NULL; // libusb sesija, NULL kadangi vienas vartotojas
valdymas.open_device(&dev_handle, &ctx);
// valdymas.send_data(1,680,820,0,0,1,dev_handle);
// app.setAttribute(Qt::WA_QuitOnClose);
while(1)
{
app.processEvents();
left_cam.get_frame(left_stream.frame);
right_cam.get_frame(right_stream.frame);
left_cam.bandinys.copyTo(left_stream.frame);
right_cam.bandinys.copyTo(right_stream.frame);
if(left_cam.flip)
{
cv::flip(left_stream.frame,left_stream.frame,-1);
};
if(right_cam.flip)
{
cv::flip(right_stream.frame,right_stream.frame,-1);
};
if(!((left_stream.frame.empty())/*&&(right_stream.frame.empty())*/))
{
stereo.stereo_procedure(left_stream.frame,right_stream.frame);
if(stereo.undistort)
{
left_stream.analyze_loop(stereo.left_output);
langas.refresh_view(stereo.left_output, stereo.right_output, stereo.depth_map, left_stream.output);
}
else
{
left_stream.analyze_loop(left_stream.frame);
langas.refresh_view(left_stream.frame, right_stream.frame, stereo.depth_map, left_stream.output);
};
if(left_stream.laser_calibrating_process==true)
{
qDebug("Laser_value_x and y: %d \t %d", left_stream.laser_value_x, left_stream.laser_value_y);
valdymas.send_data(1, left_stream.laser_value_x, left_stream.laser_value_y,1,0,1,dev_handle);
};
if(left_stream.start_tracking)
{
if(blink++>=30)
{
valdymas.send_data(1, (int)(1.5*left_stream.target.x+164), (int)(-1.4*left_stream.target.y+1164), 1,0,1, dev_handle);
blink = 0;
}
else
{
valdymas.send_data(1, (int)(1.5*left_stream.target.x+164), (int)(-1.4*left_stream.target.y+1164), 0,0,1, dev_handle);
};
}
}
if(langas.close)
{
return app.exec();
};
};
}
/**
****************************************************************************************
* @brief Setup the microcontroller system.
*
* Initialize the system clock and pins.
*****************************************************************************************
*/
void SystemInit(void)
{
/*
**************************
* Sub module clock setting
**************************
*/
syscon_SetIvrefX32WithMask(QN_SYSCON, SYSCON_MASK_DVDD12_SW_EN, MASK_ENABLE);
syscon_set_sysclk_src(CLK_XTAL, __XTAL);
#if __XTAL == XTAL_32MHz
calibration_init(XTAL_32M);
#else
calibration_init(XTAL_16M);
#endif
ref_pll_calibration();
clk32k_enable(__32K_TYPE);
#if QN_32K_RCO
rco_calibration();
#endif
// Disable all peripheral clock, will be enabled in the driver initilization.
timer_clock_off(QN_TIMER0);
timer_clock_off(QN_TIMER1);
timer_clock_off(QN_TIMER2);
timer_clock_off(QN_TIMER3);
uart_clock_off(QN_UART0);
uart_clock_off(QN_UART1);
spi_clock_off(QN_SPI0);
usart_reset((uint32_t) QN_SPI1);
spi_clock_off(QN_SPI1);
flash_clock_off();
gpio_clock_off();
adc_clock_off();
dma_clock_off();
pwm_clock_off();
// calibration will change system clock setting
// Configure sytem clock.
syscon_set_sysclk_src(CLK_XTAL, __XTAL);
syscon_set_ahb_clk(__AHB_CLK);
syscon_set_apb_clk(__APB_CLK);
syscon_set_ble_clk(__BLE_CLK);
syscon_set_timer_clk(__TIMER_CLK);
syscon_set_usart_clk((uint32_t)QN_UART0, __USART_CLK);
syscon_set_usart_clk((uint32_t)QN_UART1, __USART_CLK);
/*
**************************
* IO configuration
**************************
*/
SystemIOCfg();
/*
**************************
* Peripheral setting
**************************
*/
}
int threads_linux_init(void)
{
int status = 0;
int n = 0;
int return_value = THREADS_LINUX_SUCCESS;
// Init data
status = sensors_init(&battery_data, &gps_data, &imu_data, &pitot_data, &pwm_read_data, &pwm_write_data, &scp1000_data, &sonar_data);
if(status != SENSORS_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = calibration_init(&calibration_local_coordinate_system_data, &calibration_local_fields_data, &calibration_altimeter_data);
if(status != CALIBRATION_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = gps_init(&gps_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = imu_init(&imu_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = magnetometer_init(&magnetometer_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = sonar_init(&sonar_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = estimation_init(ESTIMATION_DO_NOT_ESTIMATE_ACCEL_BIAS, &estimation_data);
if(status != ESTIMATION_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = control_init(&control_data);
if(status != CONTROL_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = protocol_init();
if(status != PROTOCOL_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = ui_init();
if(status != UI_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = datalogger_init();
if(status != DATALOGGER_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
// Main Loop
if(return_value != THREADS_LINUX_FAILURE)
{
threads_linux_timer_start_task_1();
usleep(0.5*task_1_period_us);
threads_linux_timer_start_task_2();
usleep(5*task_1_period_us);
while(quittask == 0)
{
#if ANU_COMPILE_FOR_OVERO
#else
if(datalogger_status() == DATALOGGER_RUNNING) datalogger_update_IPC();
#endif
if(++n>100)
{
if(datalogger_status() == DATALOGGER_RUNNING) datalogger_write_file();
n = 0;
}
usleep(5*task_1_period_us);
}
threads_linux_timer_stop_task_1();
usleep(TASK1_PERIOD_US);
threads_linux_timer_stop_task_2();
usleep(TASK2_PERIOD_US);
}
status = datalogger_close();
if(status != DATALOGGER_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = ui_close();
if(status != UI_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = protocol_close();
if(status != PROTOCOL_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = control_close();
if(status != CONTROL_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = estimation_close(&estimation_data);
if(status != ESTIMATION_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = magnetometer_close(&magnetometer_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
status = imu_close(&imu_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
status = gps_close(&gps_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
status = calibration_close(&calibration_local_coordinate_system_data, &calibration_local_fields_data, &calibration_altimeter_data);
if(status != CALIBRATION_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = sensors_close();
if(status != SENSORS_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
return return_value;
}
static void function_calibration(struct rtgui_widget* widget, void* parameter)
{
calibration_init();
}