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