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 );
}
