/// test few results known as good static void test_t1(void **state) { (void) state; // unused ptr_exp2 = 0; tv_exp1.tv_sec = 0; tv_exp1.tv_usec = 0; tv_exp2.tv_sec = 0; tv_exp2.tv_usec = 0; timer_t *timer = timer_create(); assert_non_null(timer); assert_true(timer_register(timer, callback1, (void *)1000)); tv_exp1.tv_usec = 1; timer_tick(timer, false, 1); timer_cancel(timer, callback1, (void *)1000); assert_true(timer_register(timer, callback1, (void *)1000)); assert_false(timer_register(timer, callback1, (void *)1000)); assert_true(timer_register(timer, callback2, (void *)0x100)); assert_true(timer_register(timer, callback2, (void *)0x200)); timer_cancel(timer, callback1, (void *)1000); timer_cancel(timer, callback1, (void *)1000); tv_exp1.tv_usec = -1; tv_exp2.tv_usec = 3; timer_tick(timer, false, 2); assert_int_equal(ptr_exp2, 0x300); timer_destroy(timer); }
int ikev2_msg_send(struct iked *env, struct iked_message *msg) { struct iked_sa *sa = msg->msg_sa; struct ibuf *buf = msg->msg_data; u_int32_t natt = 0x00000000; int isnatt = 0; struct ike_header *hdr; struct iked_message *m; if (buf == NULL || (hdr = ibuf_seek(msg->msg_data, msg->msg_offset, sizeof(*hdr))) == NULL) return (-1); isnatt = (msg->msg_natt || (msg->msg_sa && msg->msg_sa->sa_natt)); log_info("%s: %s from %s to %s, %ld bytes%s", __func__, print_map(hdr->ike_exchange, ikev2_exchange_map), print_host(&msg->msg_local, NULL, 0), print_host(&msg->msg_peer, NULL, 0), ibuf_length(buf), isnatt ? ", NAT-T" : ""); if (isnatt) { if (ibuf_prepend(buf, &natt, sizeof(natt)) == -1) { log_debug("%s: failed to set NAT-T", __func__); return (-1); } msg->msg_offset += sizeof(natt); } if ((sendto(msg->msg_fd, ibuf_data(buf), ibuf_size(buf), 0, (struct sockaddr *)&msg->msg_peer, msg->msg_peerlen)) == -1) { log_warn("%s: sendto", __func__); return (-1); } if (!sa) return (0); if ((m = ikev2_msg_copy(env, msg)) == NULL) { log_debug("%s: failed to copy a message", __func__); return (-1); } m->msg_exchange = hdr->ike_exchange; if (hdr->ike_flags & IKEV2_FLAG_RESPONSE) { TAILQ_INSERT_TAIL(&sa->sa_responses, m, msg_entry); timer_initialize(env, &m->msg_timer, ikev2_msg_response_timeout, m); timer_register(env, &m->msg_timer, IKED_RESPONSE_TIMEOUT); } else { TAILQ_INSERT_TAIL(&sa->sa_requests, m, msg_entry); timer_initialize(env, &m->msg_timer, ikev2_msg_retransmit_timeout, m); timer_register(env, &m->msg_timer, IKED_RETRANSMIT_TIMEOUT); } return (0); }
int conn_pool_init(size_t count) { int res = 0; pid_t pid; pid = getpid(); //申请一个给连接用的内存池 conn_pool = genpool_init(sizeof(conn_t), count); if(conn_pool == NULL){ log(g_log, "genpool init error\n"); return -1; } INIT_LIST_HEAD(&read_client_head); INIT_LIST_HEAD(&write_mysql_head); INIT_LIST_HEAD(&read_mysql_write_client_head); INIT_LIST_HEAD(&prepare_mysql_head); INIT_LIST_HEAD(&idle_head); srand(pid * time(NULL)); connid = rand(); //注册各种事件的超时函数 if( (res = timer_register(read_client_timeout_timer, 30, \ "read_client_timeout_timer", 1) < 0) ){ log(g_log, "read_client_timeout_timer register error\n"); return res; } if( (res = timer_register(write_mysql_timeout_timer, 30, \ "write_mysql_timeout_timer", 1) < 0) ){ log(g_log, "write_mysql_timeout_timer register error\n"); return res; } if( (res = timer_register(read_mysql_write_client_timeout_timer, 30, \ "read_mysql_write_client_timeout_timer", 1) < 0) ){ log(g_log, "read_mysql_write_client_timeout_timer register error\n"); return res; } if( (res = timer_register(prepare_mysql_timeout_timer, 30, \ "prepare_mysql_timeout_timer", 1) < 0) ){ log(g_log, "prepare_mysql_timeout_timer register error\n"); return res; } if( (res = timer_register(idle_timeout_timer, 30, \ "idle_timeout_timer", 1) < 0) ){ log(g_log, "idle_timeout_timer register error\n"); return res; } return res; }
int main(void) { static console_command_t commands[2]; commands[0].callback = command_callback; commands[1].callback = branch_hub_command_callback; commands[1].help_callback = branch_hub_help_callback; ADCON1 = 0x0F; CMCON = 0x07; TRISA = 0x00; PORTA = 0x00; interrupt_initialize(); timer_initialize(); timer_register(timer_callback, 1000, &timer_id); timer_enable(timer_id); console_initialize(); console_register_command(commands[0], "main"); console_register_command(commands[1], "hub"); hub_init(HUB_ADDRESS); while(1) { console_process_tasks(); } }
void ledStringInit() { ledStringTimer = timer_register(0,0,0); if(ledStringTimer != INVALID_TIMERID) timer_update(0, 0,50, ledStringTimer); timer_start(ledStringTimer); }
int ikev2_msg_retransmit_response(struct iked *env, struct iked_sa *sa, struct iked_message *msg) { if ((sendto(msg->msg_fd, ibuf_data(msg->msg_data), ibuf_size(msg->msg_data), 0, (struct sockaddr *)&msg->msg_peer, msg->msg_peerlen)) == -1) { log_warn("%s: sendto", __func__); sa_free(env, sa); return (-1); } timer_register(env, &msg->msg_timer, IKED_RESPONSE_TIMEOUT); return (0); }
// handle initilization code for changing states void change_state() { switch (ui_state) { case LED_DEMO: setup_led_gpio(); led_timer = timer_register(500, &toggle_led, GE_PERIODIC); timer_start(led_timer); lcd_clear(); lcd_goto(0, 0); lcd_puts("LED Demo"); printf("LED Demo\n"); break; case PWM_DEMO: // enable pwm pins pwm_set_pin(PE9); pwm_set_pin(PE11); pwm_set_pin(PE13); pwm_set_pin(PE14); lcd_clear(); lcd_goto(0, 0); lcd_puts("PWM Demo"); printf("PWM Demo\n"); break; case ADC_DEMO: lcd_clear(); lcd_goto(0, 0); lcd_puts("ADC Demo"); printf("ADC Demo\n"); break; case USART_DEMO: lcd_clear(); lcd_goto(0, 0); lcd_puts("USART Demo"); lcd_goto(0, 1); printf("USART Demo. Echo user input.\n"); break; default: break; } }
void ikev2_msg_retransmit_timeout(struct iked *env, void *arg) { struct iked_message *msg = arg; struct iked_sa *sa = msg->msg_sa; if (msg->msg_tries < IKED_RETRANSMIT_TRIES) { if ((sendto(msg->msg_fd, ibuf_data(msg->msg_data), ibuf_size(msg->msg_data), 0, (struct sockaddr *)&msg->msg_peer, msg->msg_peerlen)) == -1) { log_warn("%s: sendto", __func__); sa_free(env, sa); return; } /* Exponential timeout */ timer_register(env, &msg->msg_timer, IKED_RETRANSMIT_TIMEOUT * (2 << (msg->msg_tries++))); } else { log_debug("%s: retransmit limit reached", __func__); sa_free(env, sa); } }
/** * @brief Main program. * @param None * @retval None */ int main(void) { NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // Initialize library ge_init(); // Initialize LEDs setup_led_gpio(); led_state = false; led_speed = false; // Initialize the USER button as an input gpio_setup_pin(GE_PBTN2, GPIO_INPUT, false, false); // Initialize PBTN1 gpio_setup_pin(GE_PBTN1, GPIO_INPUT, false, false); // Print to serial port printf("Hello, World!\n"); // Print Hello World lcd_clear(); lcd_goto(0, 0); lcd_puts("Hello, World!"); // Setup timer library // Set minimum timestep to 1ms (number of counts referecned to // a 72MHz clock) timer_set_timestep(72000); // register callback for toggling LEDs every 500ms led_timer = timer_register(500, &toggle_led, GE_PERIODIC); timer_start(led_timer); // set mode to the LED demo ui_state = LED_DEMO; // set pwm level for PWM demo float pwm_level = 0.0; // setup PWM library pwm_freq(10000.0); // setup ADC library // set sampling rate to 10kHz adc_set_fs(10000); // register callback method adc_callback(&my_adc_callback); // enable ADC channels adc_enable_channels(chan_to_conv, NUM_ADC); adc_initialize_channels(); adc_start(); // keep track of how many times the UI has looped num_refresh = 0; /* Infinite loop */ /** * Handles the user interface state machine */ while (1) { switch (ui_state) { /** * User can toggle the flashing speed of the Discovery board * LEDs. This demos how the timer library can be used. */ case LED_DEMO: //check if button depressed if (!gpio_read_pin(GE_PBTN2)) { if (led_speed) { timer_set_period(led_timer, 500); led_speed = false; } else { timer_set_period(led_timer, 100); led_speed = true; } // wait for button to be released while (!gpio_read_pin(GE_PBTN2)); } break; case PWM_DEMO: pwm_level = pwm_level + .05; if (pwm_level > 1.0) pwm_level = 0.0; pwm_set(PWM_CHAN1, pwm_level); pwm_set(PWM_CHAN2, pwm_level); pwm_set(PWM_CHAN3, pwm_level); pwm_set(PWM_CHAN4, pwm_level); break; case ADC_DEMO: // print results every 10 refreshes num_refresh++; if (num_refresh >= 10) { num_refresh = 0; printf("%u\t%u\t%u\t%u\n", val[0], val[1], val[2], val[3]); // printf("%u\t%u\n", val[0], val[1]); } break; case USART_DEMO: ; if (ge_uart_available()) { uint8_t c = ge_uart_get(); printf("%c\n", c); lcd_putc(c); } break; default: break; } // check whether to change state if (!gpio_read_pin(GE_PBTN1)) { // stop LED timer if necessary if (ui_state == LED_DEMO) timer_stop(led_timer); ui_state++; if (ui_state >= NUM_STATES) ui_state = LED_DEMO; change_state(); // wait for button to be released while (!gpio_read_pin(GE_PBTN1)); } delay_ms(50); } }
int ikev2_pld_notify(struct iked *env, struct ikev2_payload *pld, struct iked_message *msg, off_t offset) { struct ikev2_notify *n; u_int8_t *buf, md[SHA_DIGEST_LENGTH]; size_t len; u_int32_t spi32; u_int64_t spi64; struct iked_spi *rekey; u_int16_t type; u_int16_t group; if ((n = ibuf_seek(msg->msg_data, offset, sizeof(*n))) == NULL) return (-1); type = betoh16(n->n_type); log_debug("%s: protoid %s spisize %d type %s", __func__, print_map(n->n_protoid, ikev2_saproto_map), n->n_spisize, print_map(type, ikev2_n_map)); len = betoh16(pld->pld_length) - sizeof(*pld) - sizeof(*n); if ((buf = ibuf_seek(msg->msg_data, offset + sizeof(*n), len)) == NULL) return (-1); print_hex(buf, 0, len); if (!ikev2_msg_frompeer(msg)) return (0); switch (type) { case IKEV2_N_NAT_DETECTION_SOURCE_IP: case IKEV2_N_NAT_DETECTION_DESTINATION_IP: if (ikev2_nat_detection(env, msg, md, sizeof(md), type) == -1) return (-1); if (len != sizeof(md) || memcmp(buf, md, len) != 0) { log_debug("%s: %s detected NAT, enabling " "UDP encapsulation", __func__, print_map(type, ikev2_n_map)); /* * Enable UDP encapsulation of ESP packages if * the check detected NAT. */ if (msg->msg_sa != NULL) msg->msg_sa->sa_udpencap = 1; } print_hex(md, 0, sizeof(md)); break; case IKEV2_N_INVALID_KE_PAYLOAD: if (len != sizeof(group)) { log_debug("%s: malformed notification", __func__); return (-1); } if (!msg->msg_sa->sa_hdr.sh_initiator) { log_debug("%s: not an initiator", __func__); sa_free(env, msg->msg_sa); msg->msg_sa = NULL; return (-1); } memcpy(&group, buf, len); group = betoh16(group); if ((msg->msg_policy->pol_peerdh = group_get(group)) == NULL) { log_debug("%s: unable to select DH group %d", __func__, group); return (-1); } log_debug("%s: responder selected DH group %d", __func__, group); sa_free(env, msg->msg_sa); msg->msg_sa = NULL; timer_initialize(env, &env->sc_inittmr, ikev2_init_ike_sa, NULL); timer_register(env, &env->sc_inittmr, IKED_INITIATOR_INITIAL); break; case IKEV2_N_NO_ADDITIONAL_SAS: /* This makes sense for Child SAs only atm */ if (msg->msg_sa->sa_stateflags & IKED_REQ_CHILDSA) { ikev2_disable_rekeying(env, msg->msg_sa); msg->msg_sa->sa_stateflags &= ~IKED_REQ_CHILDSA; } break; case IKEV2_N_REKEY_SA: if (len != n->n_spisize) { log_debug("%s: malformed notification", __func__); return (-1); } rekey = &msg->msg_parent->msg_rekey; if (rekey->spi != 0) { log_debug("%s: rekeying of multiple SAs not supported", __func__); return (-1); } switch (n->n_spisize) { case 4: memcpy(&spi32, buf, len); rekey->spi = betoh32(spi32); break; case 8: memcpy(&spi64, buf, len); rekey->spi = betoh64(spi64); break; default: log_debug("%s: invalid spi size %d", __func__, n->n_spisize); return (-1); } rekey->spi_size = n->n_spisize; rekey->spi_protoid = n->n_protoid; log_debug("%s: rekey %s spi %s", __func__, print_map(n->n_protoid, ikev2_saproto_map), print_spi(rekey->spi, n->n_spisize)); break; } return (0); }
/*------------------------------------------------------------------------- Do all one time initializations. ---------------------------------------------------------------------------*/ void neogeo_initialize ( void ) { neogeo_audio_track = NULL; /* Create and register the watchdog timer */ timer_create ( &neogeo_watchdog_timer, neogeo_watchdog_callback ); timer_register ( &neogeo_watchdog_timer ); /* Create and register the drawline timer. */ timer_create ( &neogeo_drawline_timer, neogeo_drawline_callback ); timer_register ( &neogeo_drawline_timer ); /* Create and register the screen timer. */ timer_create ( &neogeo_screen_timer, neogeo_screen_callback ); timer_register ( &neogeo_screen_timer ); /* Create and register the VBL timer */ timer_create ( &neogeo_VBL_timer, neogeo_VBL_callback ); timer_register ( &neogeo_VBL_timer ); /* Create and register the HBL timer */ timer_create ( &neogeo_HIRQ_timer, neogeo_HIRQ_callback ); timer_register ( &neogeo_HIRQ_timer ); /* Create and register the CDROM IRQ1 Timer */ timer_create ( &neogeo_cdrom_irq1_timer, neogeo_cdrom_irq1_callback ); timer_register ( &neogeo_cdrom_irq1_timer ); /* Create and register the CDROM IRQ2 Timer */ timer_create ( &neogeo_cdrom_irq2_timer, neogeo_cdrom_irq2_callback ); timer_register ( &neogeo_cdrom_irq2_timer ); /* Create and register the timer for audio commands */ timer_create ( &neogeo_audio_command_timer, neogeo_audio_command_timer_callback ); timer_register ( &neogeo_audio_command_timer ); /* Create and register the YM2610 timers */ timer_create ( &neogeo_ym2610_timer_a, neogeo_ym2610_timer_callback ); timer_set_data ( &neogeo_ym2610_timer_a, 0 ); timer_register ( &neogeo_ym2610_timer_a ); timer_create ( &neogeo_ym2610_timer_b, neogeo_ym2610_timer_callback ); timer_set_data ( &neogeo_ym2610_timer_b, 1 ); timer_register ( &neogeo_ym2610_timer_b ); /* Initialize the memory lookup table */ memory_init_lookup(); /* Initialize the 68000 emulation core */ m68k_set_cpu_type ( M68K_CPU_TYPE_68000 ); m68k_init(); /* Inizialize the z80 core */ z80_init ( 0, Z80_CLOCK, NULL, neogeo_z80_irq_callback ); /* Initialize the YM2610 */ YM2610Init ( 8000000, neogeo_audio_frequency, neogeo_PCM_RAM, 0x100000, YM2610TimerHandler, YM2610IrqHandler ); /* Patch protection check 1 (Disc recognition) */ m68k_debug_write_memory_16 ( 0xC0D280, 0x4E71 ); /* Patch protection check 2 ("Now loading" screen) */ m68k_debug_write_memory_16 ( 0xC0EB82, 0x4E71 ); /* CD-Loading "speed hack" ^.^ */ m68k_debug_write_memory_32 ( 0xC0E764, 0x4E722000 ); m68k_debug_write_memory_16 ( 0xC0E768, 0x4E71 ); /* Reset all internal register and variables to a known state */ neogeo_cold_reset(); /* Test if the underlying OS has accurate sleep() */ neogeo_test_delay_accurate(); }
/** * @brief Main function. */ int main(void) { __enable_irq(); snapshot_buffer = BuildCameraImageBuffer(snapshot_buffer_mem); /* load settings and parameters */ global_data_reset_param_defaults(); global_data_reset(); /* init led */ LEDInit(LED_ACT); LEDInit(LED_COM); LEDInit(LED_ERR); LEDOff(LED_ACT); LEDOff(LED_COM); LEDOff(LED_ERR); /* enable FPU on Cortex-M4F core */ SCB_CPACR |= ((3UL << 10 * 2) | (3UL << 11 * 2)); /* set CP10 Full Access and set CP11 Full Access */ /* init timers */ timer_init(); /* init usb */ USBD_Init( &USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_CDC_cb, &USR_cb); /* init mavlink */ communication_init(); /* initialize camera: */ img_stream_param.size.x = FLOW_IMAGE_SIZE; img_stream_param.size.y = FLOW_IMAGE_SIZE; img_stream_param.binning = 4; { camera_image_buffer buffers[5] = { BuildCameraImageBuffer(image_buffer_8bit_1), BuildCameraImageBuffer(image_buffer_8bit_2), BuildCameraImageBuffer(image_buffer_8bit_3), BuildCameraImageBuffer(image_buffer_8bit_4), BuildCameraImageBuffer(image_buffer_8bit_5) }; camera_init(&cam_ctx, mt9v034_get_sensor_interface(), dcmi_get_transport_interface(), mt9v034_get_clks_per_row(64, 4) * 1, mt9v034_get_clks_per_row(64, 4) * 64, 2.0, &img_stream_param, buffers, 5); } /* gyro config */ gyro_config(); /* usart config*/ usart_init(); /* i2c config*/ i2c_init(); /* sonar config*/ float sonar_distance_filtered = 0.0f; // distance in meter float sonar_distance_raw = 0.0f; // distance in meter bool distance_valid = false; sonar_config(); /* reset/start timers */ timer_register(sonar_update_fn, SONAR_POLL_MS); timer_register(system_state_send_fn, SYSTEM_STATE_MS); timer_register(system_receive_fn, SYSTEM_STATE_MS / 2); timer_register(send_params_fn, PARAMS_MS); timer_register(send_video_fn, global_data.param[PARAM_VIDEO_RATE]); timer_register(take_snapshot_fn, 500); //timer_register(switch_params_fn, 2000); /* variables */ uint32_t counter = 0; result_accumulator_ctx mavlink_accumulator; result_accumulator_init(&mavlink_accumulator); uint32_t fps_timing_start = get_boot_time_us(); uint16_t fps_counter = 0; uint16_t fps_skipped_counter = 0; uint32_t last_frame_index = 0; /* main loop */ while (1) { /* check timers */ timer_check(); if (snap_capture_done) { snap_capture_done = false; camera_snapshot_acknowledge(&cam_ctx); snap_ready = true; if (snap_capture_success) { /* send the snapshot! */ LEDToggle(LED_COM); mavlink_send_image(&snapshot_buffer); } } /* calculate focal_length in pixel */ const float focal_length_px = (global_data.param[PARAM_FOCAL_LENGTH_MM]) / (4.0f * 0.006f); //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled /* new gyroscope data */ float x_rate_sensor, y_rate_sensor, z_rate_sensor; int16_t gyro_temp; gyro_read(&x_rate_sensor, &y_rate_sensor, &z_rate_sensor,&gyro_temp); /* gyroscope coordinate transformation to flow sensor coordinates */ float x_rate = y_rate_sensor; // change x and y rates float y_rate = - x_rate_sensor; float z_rate = z_rate_sensor; // z is correct /* get sonar data */ distance_valid = sonar_read(&sonar_distance_filtered, &sonar_distance_raw); /* reset to zero for invalid distances */ if (!distance_valid) { sonar_distance_filtered = 0.0f; sonar_distance_raw = 0.0f; } bool use_klt = global_data.param[PARAM_ALGORITHM_CHOICE] != 0; uint32_t start_computations = 0; /* get recent images */ camera_image_buffer *frames[2]; camera_img_stream_get_buffers(&cam_ctx, frames, 2, true); start_computations = get_boot_time_us(); int frame_delta = ((int32_t)frames[0]->frame_number - (int32_t)last_frame_index); last_frame_index = frames[0]->frame_number; fps_skipped_counter += frame_delta - 1; flow_klt_image *klt_images[2] = {NULL, NULL}; { /* make sure that the new images get the correct treatment */ /* this algorithm will still work if both images are new */ int i; bool used_klt_image[2] = {false, false}; for (i = 0; i < 2; ++i) { if (frames[i]->frame_number != frames[i]->meta) { // the image is new. apply pre-processing: /* filter the new image */ if (global_data.param[PARAM_ALGORITHM_IMAGE_FILTER]) { filter_image(frames[i]->buffer, frames[i]->param.p.size.x); } /* update meta data to mark it as an up-to date image: */ frames[i]->meta = frames[i]->frame_number; } else { // the image has the preprocessing already applied. if (use_klt) { int j; /* find the klt image that matches: */ for (j = 0; j < 2; ++j) { if (flow_klt_images[j].meta == frames[i]->frame_number) { used_klt_image[j] = true; klt_images[i] = &flow_klt_images[j]; } } } } } if (use_klt) { /* only for KLT: */ /* preprocess the images if they are not yet preprocessed */ for (i = 0; i < 2; ++i) { if (klt_images[i] == NULL) { // need processing. find unused KLT image: int j; for (j = 0; j < 2; ++j) { if (!used_klt_image[j]) { used_klt_image[j] = true; klt_images[i] = &flow_klt_images[j]; break; } } klt_preprocess_image(frames[i]->buffer, klt_images[i]); } } } } float frame_dt = (frames[0]->timestamp - frames[1]->timestamp) * 0.000001f; /* compute gyro rate in pixels and change to image coordinates */ float x_rate_px = - y_rate * (focal_length_px * frame_dt); float y_rate_px = x_rate * (focal_length_px * frame_dt); float z_rate_fr = - z_rate * frame_dt; /* compute optical flow in pixels */ flow_raw_result flow_rslt[32]; uint16_t flow_rslt_count = 0; if (!use_klt) { flow_rslt_count = compute_flow(frames[1]->buffer, frames[0]->buffer, x_rate_px, y_rate_px, z_rate_fr, flow_rslt, 32); } else { flow_rslt_count = compute_klt(klt_images[1], klt_images[0], x_rate_px, y_rate_px, z_rate_fr, flow_rslt, 32); } /* calculate flow value from the raw results */ float pixel_flow_x; float pixel_flow_y; float outlier_threshold = global_data.param[PARAM_ALGORITHM_OUTLIER_THR_RATIO]; float min_outlier_threshold = 0; if(global_data.param[PARAM_ALGORITHM_CHOICE] == 0) { min_outlier_threshold = global_data.param[PARAM_ALGORITHM_OUTLIER_THR_BLOCK]; }else { min_outlier_threshold = global_data.param[PARAM_ALGORITHM_OUTLIER_THR_KLT]; } uint8_t qual = flow_extract_result(flow_rslt, flow_rslt_count, &pixel_flow_x, &pixel_flow_y, outlier_threshold, min_outlier_threshold); /* create flow image if needed (previous_image is not needed anymore) * -> can be used for debugging purpose */ previous_image = frames[1]; if (global_data.param[PARAM_USB_SEND_VIDEO]) { uint16_t frame_size = global_data.param[PARAM_IMAGE_WIDTH]; uint8_t *prev_img = previous_image->buffer; for (int i = 0; i < flow_rslt_count; i++) { if (flow_rslt[i].quality > 0) { prev_img[flow_rslt[i].at_y * frame_size + flow_rslt[i].at_x] = 255; int ofs = (int)floor(flow_rslt[i].at_y + flow_rslt[i].y * 2 + 0.5f) * frame_size + (int)floor(flow_rslt[i].at_x + flow_rslt[i].x * 2 + 0.5f); if (ofs >= 0 && ofs < frame_size * frame_size) { prev_img[ofs] = 200; } } } } /* return the image buffers */ camera_img_stream_return_buffers(&cam_ctx, frames, 2); /* decide which distance to use */ float ground_distance = 0.0f; if(global_data.param[PARAM_SONAR_FILTERED]) { ground_distance = sonar_distance_filtered; } else { ground_distance = sonar_distance_raw; } /* update I2C transmit buffer */ update_TX_buffer(frame_dt, x_rate, y_rate, z_rate, gyro_temp, qual, pixel_flow_x, pixel_flow_y, 1.0f / focal_length_px, distance_valid, ground_distance, get_time_delta_us(get_sonar_measure_time())); /* accumulate the results */ result_accumulator_feed(&mavlink_accumulator, frame_dt, x_rate, y_rate, z_rate, gyro_temp, qual, pixel_flow_x, pixel_flow_y, 1.0f / focal_length_px, distance_valid, ground_distance, get_time_delta_us(get_sonar_measure_time())); uint32_t computaiton_time_us = get_time_delta_us(start_computations); counter++; fps_counter++; /* serial mavlink + usb mavlink output throttled */ if (counter % (uint32_t)global_data.param[PARAM_FLOW_SERIAL_THROTTLE_FACTOR] == 0)//throttling factor { float fps = 0; float fps_skip = 0; if (fps_counter + fps_skipped_counter > 100) { uint32_t dt = get_time_delta_us(fps_timing_start); fps_timing_start += dt; fps = (float)fps_counter / ((float)dt * 1e-6f); fps_skip = (float)fps_skipped_counter / ((float)dt * 1e-6f); fps_counter = 0; fps_skipped_counter = 0; mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "TIMING", get_boot_time_us(), computaiton_time_us, fps, fps_skip); } mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "EXPOSURE", get_boot_time_us(), frames[0]->param.exposure, frames[0]->param.analog_gain, cam_ctx.last_brightness); /* calculate the output values */ result_accumulator_output_flow output_flow; result_accumulator_output_flow_rad output_flow_rad; int min_valid_ratio = global_data.param[PARAM_ALGORITHM_MIN_VALID_RATIO]; result_accumulator_calculate_output_flow(&mavlink_accumulator, min_valid_ratio, &output_flow); result_accumulator_calculate_output_flow_rad(&mavlink_accumulator, min_valid_ratio, &output_flow_rad); // send flow mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], output_flow.flow_x, output_flow.flow_y, output_flow.flow_comp_m_x, output_flow.flow_comp_m_y, output_flow.quality, output_flow.ground_distance); mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], output_flow_rad.integration_time, output_flow_rad.integrated_x, output_flow_rad.integrated_y, output_flow_rad.integrated_xgyro, output_flow_rad.integrated_ygyro, output_flow_rad.integrated_zgyro, output_flow_rad.temperature, output_flow_rad.quality, output_flow_rad.time_delta_distance_us,output_flow_rad.ground_distance); if (global_data.param[PARAM_USB_SEND_FLOW] && (output_flow.quality > 0 || global_data.param[PARAM_USB_SEND_QUAL_0])) { mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], output_flow.flow_x, output_flow.flow_y, output_flow.flow_comp_m_x, output_flow.flow_comp_m_y, output_flow.quality, output_flow.ground_distance); mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], output_flow_rad.integration_time, output_flow_rad.integrated_x, output_flow_rad.integrated_y, output_flow_rad.integrated_xgyro, output_flow_rad.integrated_ygyro, output_flow_rad.integrated_zgyro, output_flow_rad.temperature, output_flow_rad.quality, output_flow_rad.time_delta_distance_us,output_flow_rad.ground_distance); } if(global_data.param[PARAM_USB_SEND_GYRO]) { mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "GYRO", get_boot_time_us(), x_rate, y_rate, z_rate); } result_accumulator_reset(&mavlink_accumulator); } /* forward flow from other sensors */ if (counter % 2) { communication_receive_forward(); } } }
int time_tumble_receive(struct time_tumble *time_op,int type, int time, tuple_t *tup) { // operator has been scheduled if(type & TUPLE_ADDED) { if(time_op->begin == 0) { // begin to generate time signal from "begin_time" // generate one time signal every "window_size" second // generate the time signal until deregister this operator time_op->pTimer = timer_register(time_op->opr.schedopr,time_op->begin_time,time_op->window_size,0); } // invalid value else { assert(0); } } // new data tuple arrived if(type & TUPLE_INS) { // window running if(time_op->begin == 1) { // generate + stream tuple dispatch_tuple(time_op->opr.schedopr,TUPLE_INS, time, tup); } // window not begin else if(time_op->begin == 0) { // ignore tuple tuple_unlock(tup); } // invalid value else { assert(0); } } // time signal arrived if(type & TUPLE_TIME) { // window is running, dispatch update and clean signal if(time_op->begin == 1) { // dispatch clean signal dispatch_tuple(time_op->opr.schedopr,TUPLE_UPDATE, time, NULL); dispatch_tuple(time_op->opr.schedopr,TUPLE_CLEAN, time, NULL); } // receive the first time signal, window begin else if(time_op->begin == 0) { time_op->begin = 1; } // invalid value else { assert(0); } } return 0; }