int control_loop_setup( int ms_period, int ctrl_gain, int ubisense, double *offset_r, double *offset_p ) { /* initialize global variables */ running = 1; period = ms_period / 1000.0; us_period = ms_period * 1000; heli_state = HELI_STATE_SHUTDOWN; heli_settled = 1; yaw_wn_imu = 0; yaw_wn_cmd = 0; new_data_z = 0; cmd_roll = 0; cmd_pitch = 0; next_period = 0; motor_speed_revving = 0; motor_speed_liftoff = 0; /* initialize filter objects */ iir_lp_filter_init ( &iir_acc_x, "ACC_X", IIR_GAIN_LACC ); iir_lp_filter_init ( &iir_acc_y, "ACC_Y", IIR_GAIN_LACC ); iir_lp_filter_init ( &iir_acc_z, "ACC_Z", IIR_GAIN_LACC ); iir_lp_filter_init ( &iir_cmd_roll, "CMD_Roll", IIR_GAIN_RCMD ); iir_lp_filter_init ( &iir_cmd_pitch, "CMD_Pitch", IIR_GAIN_RCMD ); iir_lp_filter_init ( &iir_cmd_yaw, "CMD_Yaw", IIR_GAIN_RCMD ); iir_lp_filter_init ( &iir_cmd_z, "CMD_Z", IIR_GAIN_RCMD ); median_filter_init ( &med_bat_level, "Bat_Level", MED_SIZE_BATT ); position_ekf_init ( &ekf_pos_z, "POS_Z", EKF_POS_STD_P, EKF_POS_STD_V, EKF_POS_STD_A, period ); /* initialize controller objects */ controller_init ( &ctrl_roll, "Roll", CTRL_PIDD_DEF, period ); controller_init ( &ctrl_pitch, "Pitch", CTRL_PIDD_DEF, period ); controller_init ( &ctrl_yaw, "Yaw", CTRL_PIDD_DEF, period ); controller_init ( &ctrl_z, "Z", CTRL_PIDD_DEF, period ); /* initialize transformations */ transformation_init( ); /* clear data structures */ memset( &command_data, 0, sizeof( command_data ) ); memset( &javiator_data, 0, sizeof( javiator_data ) ); memset( &sensor_data, 0, sizeof( sensor_data ) ); memset( &motor_signals, 0, sizeof( motor_signals ) ); memset( &motor_offsets, 0, sizeof( motor_offsets ) ); return( 0 ); }
void ADC_init_all() { //variables DMA_InitTypeDef DMA_InitStructure; ADC_CommonInitTypeDef ADC_CommonInitStructure; ADC_InitTypeDef ADC_InitStructure; RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2, ENABLE); //RCC_AHB1PeriphClockCmd(RCC_AHB1ENR_GPIOCEN, ENABLE); ADC_DeInit(); RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE); ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent; ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div2; ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled; ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_10Cycles; ADC_CommonInit(&ADC_CommonInitStructure); ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; ADC_InitStructure.ADC_ScanConvMode = ENABLE; ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_T1_CC1; ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; ADC_InitStructure.ADC_NbrOfConversion = 3; ADC_Init(ADC1, &ADC_InitStructure); ADC_RegularChannelConfig(ADC1, ADC_Channel_10, 1, ADC_SampleTime_84Cycles); ADC_RegularChannelConfig(ADC1, ADC_Channel_11, 2, ADC_SampleTime_84Cycles); ADC_RegularChannelConfig(ADC1, ADC_Channel_12, 3, ADC_SampleTime_84Cycles); // DMA DMA_DeInit(DMA2_Stream4);//Can be used for ADC1 DMA_InitStructure.DMA_Channel = 0;//Can be used for ADC1 DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) &ADC1->DR; DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t) &ADC_DMA_buffer; //DMA buffer Address DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;//From ADC to memory DMA_InitStructure.DMA_BufferSize = 3;// 3 DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; DMA_InitStructure.DMA_Priority = DMA_Priority_High; DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable; DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull; DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single; DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; DMA_Init(DMA2_Stream4, &DMA_InitStructure); DMA_Cmd(DMA2_Stream4, ENABLE); while(DMA_GetCmdStatus(DMA2_Stream4)==DISABLE){}; ADC_DMACmd(ADC1, ENABLE); ADC_Cmd(ADC1, ENABLE); ADC_DMARequestAfterLastTransferCmd(ADC1, ENABLE); ADC_ContinuousModeCmd(ADC1,ENABLE); ADC_SoftwareStartConv(ADC1); median_filter_init(); }
int main(int argc, char *argv[]) { SDL_Surface *screen; static struct option long_options[] = { {"no-kinect", no_argument, 0, 'k'}, {"fullscreen", optional_argument, 0, 'f'}, {"help", no_argument, 0, 'h'}, {0, 0, 0, 0} }; int option_index = 0, opt; bool init_kinect = true; bool fullscreen_mode = false; char *fullscreen_resolution = NULL; while ((opt = getopt_long(argc, argv, "khf:", long_options, &option_index)) != -1) { switch (opt) { case 'k': init_kinect = false; printf("Not initializing kinect (-k passed)\n"); break; case 'f': printf("Starting in fullscreen mode\n"); fullscreen_mode = true; if (optarg) fullscreen_resolution = strdup(optarg); break; case 'h': printf("Syntax: %s [-k] [-h]\n", argv[0]); printf("\t--no-kinect\tDisables initializing kinect\n"); printf("\t--fullscreen\tEnable fullscreen mode (default is windowed)\n"); printf("\t\t\t(--fullscreen=1024x768 to overwrite the resolution)\n"); exit(0); break; } } median_filter_init(); glow_filter_init(); if (init_kinect) kinect_init(); mask_rgb_init(); loadimg_init(); /* Initialize SDL */ SDL_Init(SDL_INIT_VIDEO); TTF_Init(); SDL_GL_SetAttribute(SDL_GL_RED_SIZE, 8); SDL_GL_SetAttribute(SDL_GL_GREEN_SIZE, 8); SDL_GL_SetAttribute(SDL_GL_BLUE_SIZE, 8); SDL_GL_SetAttribute(SDL_GL_DEPTH_SIZE, 16); SDL_GL_SetAttribute(SDL_GL_BUFFER_SIZE, 32); SDL_GL_SetAttribute(SDL_GL_DOUBLEBUFFER, 1); SDL_GL_SetAttribute(SDL_GL_SWAP_CONTROL, 1); /* Initialize the screen / window */ if (fullscreen_mode && fullscreen_resolution != NULL) { if (sscanf(fullscreen_resolution, "%dx", &SCREEN_WIDTH) != 1) { fprintf(stderr, "Invalid resolution specified: %s (needs to be WxH, e.g. 1024x768)\n", fullscreen_resolution); exit(1); } printf("Setting width to %d\n", SCREEN_WIDTH); } int flags = SDL_OPENGL | SDL_HWSURFACE | SDL_NOFRAME | SDL_DOUBLEBUF; if (fullscreen_mode) flags |= SDL_FULLSCREEN; screen = SDL_SetVideoMode(SCREEN_WIDTH, SCREEN_HEIGHT, SCREEN_DEPTH, flags); if (screen == 0) { printf("set failed: %s\n", SDL_GetError()); return 1; } SDL_WM_SetCaption("kinectboard", ""); glewInit(); /* Setup viewport */ glEnable(GL_TEXTURE_2D); glClearColor(0.0f, 0.0f, 0.0f, 0.0f); glViewport(0, 0, SCREEN_WIDTH, SCREEN_HEIGHT); glClear(GL_COLOR_BUFFER_BIT); glMatrixMode(GL_PROJECTION); glLoadIdentity(); gluOrtho2D(0, SCREEN_WIDTH, 0, SCREEN_HEIGHT); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); kb_ui_init(); // Register callbacks kb_ui_register_void_callback("Exit",exit_callback); kb_ui_register_void_callback("Calibrate",run_calibration_callback); kb_ui_register_void_callback("StartCalibration",start_calibration_callback); kb_ui_register_void_callback("EndCalibration",end_calibration_callback); kb_ui_register_void_callback("ImageRight",kb_images_scroll_right); kb_ui_register_void_callback("ImageLeft",kb_images_scroll_left); kb_ui_register_value_callback("SetDistanceThreshold", set_distance_threshold_callback); kb_ui_register_value_callback("SetDepthMultiplier", set_depth_multiplier_callback); kb_ui_register_value_callback("SetDepthDifferenceThreshold", set_depth_difference_threshold_callback); kb_ui_register_value_callback("SetGlowAreaStart", set_glow_area_start_callback); kb_ui_register_value_callback("SetGlowAreaEnd", set_glow_area_end_callback); kb_ui_call_javascript("SetRGB", "142,51,19"); // The CUDA Device Info requires a valid UI since the info is displayed there print_cuda_device_info(); /* Allocate textures and buffers to draw into (from the GPU) */ allocateGLTexture(&rawDepthBufferID, &rawDepthTextureID); allocateGLTexture(&medianBufferID, &medianTextureID); allocateGLTexture(&maskedMedianBufferID, &maskedMedianTextureID); allocateGLTexture(&glowBufferID, &glowTextureID); allocateGLTexture(&rawRgbBufferID, &rawRgbTextureID); allocateGLTexture(&maskRgbBufferID, &maskRgbTextureID); allocateGLTexture(&contRgbBufferID, &contRgbTextureID); kb_image_create("Raw depth image", rawDepthBufferID, rawDepthTextureID); kb_image_create("Median-filtered depth image", medianBufferID, medianTextureID); kb_image_create("Masked depth image", maskedMedianBufferID, maskedMedianTextureID); kb_image_create("Glowing depth", glowBufferID, glowTextureID); kb_image_create("Raw RGB image", rawRgbBufferID, rawRgbTextureID); kb_image_create("Masked kinect RGB image", maskRgbBufferID, maskRgbTextureID); kb_image_create("Cont RGB image", contRgbBufferID, contRgbTextureID); // Load a Texture //loadTextureFromFile("../data/calibration.bmp", &calibrationBufferID, &calibrationTextureID); //kb_image_create("Calibration", calibrationBufferID, calibrationTextureID); SDL_Surface* surface = SDL_LoadBMP("../data/calibration.bmp"); cudaMalloc((void**)&(backgrounds[1]), 640 * 480 * 3 * sizeof(uint8_t)); loadimg_convert((uint8_t*)surface->pixels, backgrounds[1]); surface = SDL_LoadBMP("../data/malen_haus.bmp"); cudaMalloc((void**)&(backgrounds[2]), 640 * 480 * 3 * sizeof(uint8_t)); loadimg_convert((uint8_t*)surface->pixels, backgrounds[2]); surface = SDL_LoadBMP("../data/malen_stern.bmp"); cudaMalloc((void**)&(backgrounds[3]), 640 * 480 * 3 * sizeof(uint8_t)); loadimg_convert((uint8_t*)surface->pixels, backgrounds[3]); surface = SDL_LoadBMP("../data/empty.bmp"); cudaMalloc((void**)&(backgrounds[4]), 640 * 480 * 3 * sizeof(uint8_t)); loadimg_convert((uint8_t*)surface->pixels, backgrounds[4]); printf("gl set up.\n"); uchar4 *gpu_median_output, *gpu_masked_median_output, *gpu_glow_output, *gpu_mask_rgb_output, *gpu_raw_depth_output, *gpu_raw_rgb_output, *gpu_cont_rgb_output; int fps = 0; int last_time = 0; int current_time; while (1) { /* FPS counter */ current_time = SDL_GetTicks(); if ((current_time - last_time) >= 1000) { static char buffer[20] = {0}; sprintf(buffer, "%d FPS", fps); SDL_WM_SetCaption(buffer, 0); kb_ui_call_javascript("SetFPS",buffer); fps = 0; last_time = current_time; } //kb_poll_events(list); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT); /* Reset viewport for rendering our images, it was modified by * kb_ui_render(). */ glViewport(0, 0, SCREEN_WIDTH, SCREEN_HEIGHT); glClear(GL_COLOR_BUFFER_BIT); glMatrixMode(GL_PROJECTION); glLoadIdentity(); gluOrtho2D(0, SCREEN_WIDTH, 0, SCREEN_HEIGHT); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); kb_poll_events(); gpu_median_output = NULL; gpu_masked_median_output = NULL; gpu_glow_output = NULL; gpu_mask_rgb_output = NULL; gpu_raw_depth_output = NULL; gpu_raw_rgb_output = NULL; gpu_cont_rgb_output = NULL; cutilSafeCall(cudaGLMapBufferObject((void**)&gpu_raw_depth_output, rawDepthBufferID)); cutilSafeCall(cudaGLMapBufferObject((void**)&gpu_median_output, medianBufferID)); cutilSafeCall(cudaGLMapBufferObject((void**)&gpu_masked_median_output, maskedMedianBufferID)); cutilSafeCall(cudaGLMapBufferObject((void**)&gpu_glow_output, glowBufferID)); cutilSafeCall(cudaGLMapBufferObject((void**)&gpu_mask_rgb_output, maskRgbBufferID)); cutilSafeCall(cudaGLMapBufferObject((void**)&gpu_raw_rgb_output, rawRgbBufferID)); cutilSafeCall(cudaGLMapBufferObject((void**)&gpu_cont_rgb_output, contRgbBufferID)); // XXX: Potential for optimization: We currently call functions like // median_filter(), median_mask() and mask_rgb() which are all // blocking. However, we could launch the kernel and perform more work // on the CPU while waiting for the kernel to complete (or maybe even // launch some in parallel and/or use async events). median_filter(take_depth_image(), gpu_median_output, gpu_raw_depth_output); done_depth_image(); median_mask(calibration, gpu_median_output, gpu_masked_median_output); glow_filter(gpu_masked_median_output, gpu_glow_output, glow_start, glow_end); mask_rgb(gpu_glow_output, take_rgb_image(), gpu_mask_rgb_output, gpu_raw_rgb_output, gpu_cont_rgb_output, reference_color, FILTER_DISTANCE, backgrounds[current_background], calibrated_offset); done_rgb_image(); cutilSafeCall(cudaGLUnmapBufferObject(maskedMedianBufferID)); cutilSafeCall(cudaGLUnmapBufferObject(medianBufferID)); cutilSafeCall(cudaGLUnmapBufferObject(glowBufferID)); cutilSafeCall(cudaGLUnmapBufferObject(maskRgbBufferID)); cutilSafeCall(cudaGLUnmapBufferObject(rawDepthBufferID)); cutilSafeCall(cudaGLUnmapBufferObject(rawRgbBufferID)); cutilSafeCall(cudaGLUnmapBufferObject(contRgbBufferID)); if(fullscreen_canvas) { kb_images_render_canvas_only(); } else { kb_images_render(); kb_ui_update(); kb_ui_render(); } SDL_GL_SwapBuffers(); fps++; } }
int control_loop_setup( int ms_period, int ctrl_gain, int ubisense, double *offset_r, double *offset_p ) { struct sigaction act; /* initialize global variables */ running = 1; period = ms_period / 1000.0; us_period = ms_period * 1000; pos_ctrl_gain = ctrl_gain; ubisense_enabled = ubisense; heli_state = HELI_STATE_SHUTDOWN; heli_mode = HELI_MODE_POS_CTRL; heli_settled = 1; yaw_wn_imu = 0; yaw_wn_cmd = 0; new_data_x = 0; new_data_y = 0; new_data_z = 0; offset_roll = offset_r; offset_pitch = offset_p; cmd_roll = 0; cmd_pitch = 0; next_period = 0; motor_speed_revving = 0; motor_speed_liftoff = 0; act.sa_handler = signal_handler; act.sa_handler = int_handler; /* initialize signal handlers */ if( sigaction( SIGUSR1, &act, NULL ) || sigaction( SIGINT, &act, NULL ) ) { perror( "sigaction" ); } /* initialize filter objects */ outlier_filter_init( &cof_out_x, "OUT_X", COF_MDIFF_POS, COF_LIMIT_POS ); outlier_filter_init( &cof_out_y, "OUT_Y", COF_MDIFF_POS, COF_LIMIT_POS ); iir_lp_filter_init ( &iir_acc_x, "ACC_X", IIR_GAIN_LACC ); iir_lp_filter_init ( &iir_acc_y, "ACC_Y", IIR_GAIN_LACC ); iir_lp_filter_init ( &iir_acc_z, "ACC_Z", IIR_GAIN_LACC ); iir_lp_filter_init ( &iir_cmd_roll, "CMD_Roll", IIR_GAIN_RCMD ); iir_lp_filter_init ( &iir_cmd_pitch, "CMD_Pitch", IIR_GAIN_RCMD ); iir_lp_filter_init ( &iir_cmd_yaw, "CMD_Yaw", IIR_GAIN_RCMD ); iir_lp_filter_init ( &iir_cmd_z, "CMD_Z", IIR_GAIN_RCMD ); average_filter_init( &avg_bmu_maps, "BMU_MAPS", AVG_SIZE_MAPS ); median_filter_init ( &med_bmu_temp, "BMU_Temp", MED_SIZE_TEMP ); median_filter_init ( &med_bmu_batt, "BMU_Batt", MED_SIZE_BATT ); attitude_ekf_init ( &ekf_att_roll, "ATT_Roll", EKF_ATT_STD_E, EKF_ATT_STD_W, EKF_ATT_PH_SH, period ); attitude_ekf_init ( &ekf_att_pitch, "ATT_Pitch", EKF_ATT_STD_E, EKF_ATT_STD_W, EKF_ATT_PH_SH, period ); attitude_ekf_init ( &ekf_att_yaw, "ATT_Yaw", EKF_ATT_STD_E, EKF_ATT_STD_W, EKF_ATT_PH_SH, period ); position_ekf_init ( &ekf_pos_x, "POS_X", EKF_POS_STD_P, EKF_POS_STD_V, EKF_POS_STD_A, period ); position_ekf_init ( &ekf_pos_y, "POS_Y", EKF_POS_STD_P, EKF_POS_STD_V, EKF_POS_STD_A, period ); position_ekf_init ( &ekf_pos_z, "POS_Z", EKF_POS_STD_P, EKF_POS_STD_V, EKF_POS_STD_A, period ); /* initialize controller objects */ controller_init ( &ctrl_roll, "Roll", CTRL_PIDD_DEF, period ); controller_init ( &ctrl_pitch, "Pitch", CTRL_PIDD_DEF, period ); controller_init ( &ctrl_yaw, "Yaw", CTRL_PIDD_DEF, period ); controller_init ( &ctrl_x, "X", CTRL_PIDD_X_Y, period ); controller_init ( &ctrl_y, "Y", CTRL_PIDD_X_Y, period ); controller_init ( &ctrl_z, "Z", CTRL_PIDD_DEF, period ); /* initialize transformations */ transformation_init( ); /* clear data structures */ memset( &command_data, 0, sizeof( command_data ) ); memset( &javiator_data, 0, sizeof( javiator_data ) ); memset( &sensor_data, 0, sizeof( sensor_data ) ); memset( &motor_signals, 0, sizeof( motor_signals ) ); memset( &motor_offsets, 0, sizeof( motor_offsets ) ); memset( &trace_data, 0, sizeof( trace_data ) ); return( 0 ); }