__EXPORT void timer_init(void) { /* For system timing probing see bord.h and * CONFIG_BOARD_USE_PROBES */ PROBE_INIT(7); PROBE(1, true); PROBE(2, true); PROBE(3, true); PROBE(1, false); PROBE(2, false); PROBE(3, false); /* This is the lowlevel IO if needed to instrument timing * with the smallest impact * *((uint32_t *)0x40011010) = 0x100; // PROBE(3,true); * *((uint32_t *)0x40011014) = 0x100; // PROBE(3,false); */ /* Initialize timer data */ sys_tic = 0; memset(timers, 0, sizeof(timers)); }
/** * @brief Main function. */ int main(void) { __enable_irq(); /* load settings and parameters */ global_data_reset_param_defaults(); global_data_reset(); PROBE_INIT(); /* init led */ LEDInit(LED_ACT); LEDInit(LED_COM); LEDInit(LED_ERR); LEDOff(LED_ACT); LEDOff(LED_COM); LEDOff(LED_ERR); board_led_rgb(255,255,255, 1); board_led_rgb( 0, 0,255, 0); board_led_rgb( 0, 0, 0, 0); board_led_rgb(255, 0, 0, 1); board_led_rgb(255, 0, 0, 2); board_led_rgb(255, 0, 0, 3); board_led_rgb( 0,255, 0, 3); board_led_rgb( 0, 0,255, 4); /* enable FPU on Cortex-M4F core */ SCB_CPACR |= ((3UL << 10 * 2) | (3UL << 11 * 2)); /* set CP10 Full Access and set CP11 Full Access */ /* init clock */ if (SysTick_Config(SystemCoreClock / 100000))/*set timer to trigger interrupt every 10 microsecond */ { /* capture clock error */ LEDOn(LED_ERR); while (1); } /* init usb */ USBD_Init( &USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_CDC_cb, &USR_cb); /* init mavlink */ communication_init(); /* enable image capturing */ enable_image_capture(); /* gyro config */ gyro_config(); /* init and clear fast image buffers */ for (int i = 0; i < global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; i++) { image_buffer_8bit_1[i] = 0; image_buffer_8bit_2[i] = 0; } uint8_t * current_image = image_buffer_8bit_1; uint8_t * previous_image = image_buffer_8bit_2; /* 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[TIMER_SONAR] = SONAR_TIMER_COUNT; timer[TIMER_SYSTEM_STATE] = SYSTEM_STATE_COUNT; timer[TIMER_RECEIVE] = SYSTEM_STATE_COUNT / 2; timer[TIMER_PARAMS] = PARAMS_COUNT; timer[TIMER_IMAGE] = global_data.param[PARAM_VIDEO_RATE]; /* variables */ uint32_t counter = 0; uint8_t qual = 0; /* bottom flow variables */ float pixel_flow_x = 0.0f; float pixel_flow_y = 0.0f; float pixel_flow_x_sum = 0.0f; float pixel_flow_y_sum = 0.0f; float velocity_x_sum = 0.0f; float velocity_y_sum = 0.0f; float velocity_x_lp = 0.0f; float velocity_y_lp = 0.0f; int valid_frame_count = 0; int pixel_flow_count = 0; static float accumulated_flow_x = 0; static float accumulated_flow_y = 0; static float accumulated_gyro_x = 0; static float accumulated_gyro_y = 0; static float accumulated_gyro_z = 0; static uint16_t accumulated_framecount = 0; static uint16_t accumulated_quality = 0; static uint32_t integration_timespan = 0; static uint32_t lasttime = 0; uint32_t time_since_last_sonar_update= 0; uint32_t time_last_pub= 0; uavcan_start(); /* main loop */ while (1) { PROBE_1(false); uavcan_run(); PROBE_1(true); /* reset flow buffers if needed */ if(buffer_reset_needed) { buffer_reset_needed = 0; for (int i = 0; i < global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; i++) { image_buffer_8bit_1[i] = 0; image_buffer_8bit_2[i] = 0; } delay(500); continue; } /* calibration routine */ if(FLOAT_AS_BOOL(global_data.param[PARAM_VIDEO_ONLY])) { while(FLOAT_AS_BOOL(global_data.param[PARAM_VIDEO_ONLY])) { dcmi_restart_calibration_routine(); /* waiting for first quarter of image */ while(get_frame_counter() < 2){} dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1); /* waiting for second quarter of image */ while(get_frame_counter() < 3){} dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1); /* waiting for all image parts */ while(get_frame_counter() < 4){} send_calibration_image(&previous_image, ¤t_image); if (FLOAT_AS_BOOL(global_data.param[PARAM_SYSTEM_SEND_STATE])) communication_system_state_send(); communication_receive_usb(); debug_message_send_one(); communication_parameter_send(); LEDToggle(LED_COM); } dcmi_restart_calibration_routine(); LEDOff(LED_COM); } uint16_t image_size = global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; /* 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 */ 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 /* calculate focal_length in pixel */ const float focal_length_px = (global_data.param[PARAM_FOCAL_LENGTH_MM]) / (4.0f * 6.0f) * 1000.0f; //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled /* 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; } /* compute optical flow */ if (FLOAT_EQ_INT(global_data.param[PARAM_SENSOR_POSITION], BOTTOM)) { /* copy recent image to faster ram */ dma_copy_image_buffers(¤t_image, &previous_image, image_size, 1); /* compute optical flow */ qual = compute_flow(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y); /* * real point P (X,Y,Z), image plane projection p (x,y,z), focal-length f, distance-to-scene Z * x / f = X / Z * y / f = Y / Z */ float flow_compx = pixel_flow_x / focal_length_px / (get_time_between_images() / 1000000.0f); float flow_compy = pixel_flow_y / focal_length_px / (get_time_between_images() / 1000000.0f); if (qual > 0) { valid_frame_count++; uint32_t deltatime = (get_boot_time_us() - lasttime); integration_timespan += deltatime; accumulated_flow_x += pixel_flow_y / focal_length_px * 1.0f; //rad axis swapped to align x flow around y axis accumulated_flow_y += pixel_flow_x / focal_length_px * -1.0f;//rad accumulated_gyro_x += x_rate * deltatime / 1000000.0f; //rad accumulated_gyro_y += y_rate * deltatime / 1000000.0f; //rad accumulated_gyro_z += z_rate * deltatime / 1000000.0f; //rad accumulated_framecount++; accumulated_quality += qual; } /* integrate velocity and output values only if distance is valid */ if (distance_valid) { /* calc velocity (negative of flow values scaled with distance) */ float new_velocity_x = - flow_compx * sonar_distance_filtered; float new_velocity_y = - flow_compy * sonar_distance_filtered; time_since_last_sonar_update = (get_boot_time_us()- get_sonar_measure_time()); if (qual > 0) { velocity_x_sum += new_velocity_x; velocity_y_sum += new_velocity_y; /* lowpass velocity output */ velocity_x_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * new_velocity_x + (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp; velocity_y_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * new_velocity_y + (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp; } else { /* taking flow as zero */ velocity_x_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp; velocity_y_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp; } } else { /* taking flow as zero */ velocity_x_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp; velocity_y_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp; } //update lasttime lasttime = get_boot_time_us(); pixel_flow_x_sum += pixel_flow_x; pixel_flow_y_sum += pixel_flow_y; pixel_flow_count++; } counter++; if (FLOAT_EQ_INT(global_data.param[PARAM_SENSOR_POSITION], BOTTOM)) { /* send bottom flow if activated */ float ground_distance = 0.0f; if(FLOAT_AS_BOOL(global_data.param[PARAM_SONAR_FILTERED])) { ground_distance = sonar_distance_filtered; } else { ground_distance = sonar_distance_raw; } uavcan_define_export(i2c_data, legacy_12c_data_t, ccm); uavcan_define_export(range_data, range_data_t, ccm); uavcan_timestamp_export(i2c_data); uavcan_assign(range_data.time_stamp_utc, i2c_data.time_stamp_utc); //update I2C transmitbuffer if(valid_frame_count>0) { update_TX_buffer(pixel_flow_x, pixel_flow_y, velocity_x_sum/valid_frame_count, velocity_y_sum/valid_frame_count, qual, ground_distance, x_rate, y_rate, z_rate, gyro_temp, uavcan_use_export(i2c_data)); } else { update_TX_buffer(pixel_flow_x, pixel_flow_y, 0.0f, 0.0f, qual, ground_distance, x_rate, y_rate, z_rate, gyro_temp, uavcan_use_export(i2c_data)); } PROBE_2(false); uavcan_publish(range, 40, range_data); PROBE_2(true); PROBE_3(false); uavcan_publish(flow, 40, i2c_data); PROBE_3(true); //serial mavlink + usb mavlink output throttled uint32_t now = get_boot_time_us(); uint32_t time_since_last_pub = now - time_last_pub; if (time_since_last_pub > (1.0e6f/global_data.param[PARAM_BOTTOM_FLOW_PUB_RATE])) { time_last_pub = now; float flow_comp_m_x = 0.0f; float flow_comp_m_y = 0.0f; if(FLOAT_AS_BOOL(global_data.param[PARAM_BOTTOM_FLOW_LP_FILTERED])) { flow_comp_m_x = velocity_x_lp; flow_comp_m_y = velocity_y_lp; } else { if(valid_frame_count>0) { flow_comp_m_x = velocity_x_sum/valid_frame_count; flow_comp_m_y = velocity_y_sum/valid_frame_count; } else { flow_comp_m_x = 0.0f; flow_comp_m_y = 0.0f; } } // send flow mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f, flow_comp_m_x, flow_comp_m_y, qual, ground_distance); mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], integration_timespan, accumulated_flow_x, accumulated_flow_y, accumulated_gyro_x, accumulated_gyro_y, accumulated_gyro_z, gyro_temp, accumulated_quality/accumulated_framecount, time_since_last_sonar_update,ground_distance); /* send approximate local position estimate without heading */ if (FLOAT_AS_BOOL(global_data.param[PARAM_SYSTEM_SEND_LPOS])) { /* rough local position estimate for unit testing */ lpos.x += ground_distance*accumulated_flow_x; lpos.y += ground_distance*accumulated_flow_y; lpos.z = -ground_distance; lpos.vx = ground_distance*accumulated_flow_x/integration_timespan; lpos.vy = ground_distance*accumulated_flow_y/integration_timespan; lpos.vz = 0; // no direct measurement, just ignore } else { /* toggling param allows user reset */ lpos.x = 0; lpos.y = 0; lpos.z = 0; lpos.vx = 0; lpos.vy = 0; lpos.vz = 0; } if (FLOAT_AS_BOOL(global_data.param[PARAM_USB_SEND_FLOW])) { mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f, flow_comp_m_x, flow_comp_m_y, qual, ground_distance); mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], integration_timespan, accumulated_flow_x, accumulated_flow_y, accumulated_gyro_x, accumulated_gyro_y, accumulated_gyro_z, gyro_temp, accumulated_quality/accumulated_framecount, time_since_last_sonar_update,ground_distance); } if(FLOAT_AS_BOOL(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); } integration_timespan = 0; accumulated_flow_x = 0; accumulated_flow_y = 0; accumulated_framecount = 0; accumulated_quality = 0; accumulated_gyro_x = 0; accumulated_gyro_y = 0; accumulated_gyro_z = 0; velocity_x_sum = 0.0f; velocity_y_sum = 0.0f; pixel_flow_x_sum = 0.0f; pixel_flow_y_sum = 0.0f; valid_frame_count = 0; pixel_flow_count = 0; } } /* forward flow from other sensors */ if (counter % 2) { communication_receive_forward(); } /* send system state, receive commands */ if (send_system_state_now) { /* every second */ if (FLOAT_AS_BOOL(global_data.param[PARAM_SYSTEM_SEND_STATE])) { communication_system_state_send(); } send_system_state_now = false; } /* receive commands */ if (receive_now) { /* test every second */ communication_receive(); communication_receive_usb(); receive_now = false; } /* sending debug msgs and requested parameters */ if (send_params_now) { debug_message_send_one(); communication_parameter_send(); send_params_now = false; } /* send local position estimate, for testing only, doesn't account for heading */ if (send_lpos_now) { if (FLOAT_AS_BOOL(global_data.param[PARAM_SYSTEM_SEND_LPOS])) { mavlink_msg_local_position_ned_send(MAVLINK_COMM_2, timer_ms, lpos.x, lpos.y, lpos.z, lpos.vx, lpos.vy, lpos.vz); } send_lpos_now = false; } /* transmit raw 8-bit image */ if (FLOAT_AS_BOOL(global_data.param[PARAM_USB_SEND_VIDEO])&& send_image_now) { /* get size of image to send */ uint16_t image_size_send; uint16_t image_width_send; uint16_t image_height_send; image_size_send = image_size; image_width_send = global_data.param[PARAM_IMAGE_WIDTH]; image_height_send = global_data.param[PARAM_IMAGE_HEIGHT]; mavlink_msg_data_transmission_handshake_send( MAVLINK_COMM_2, MAVLINK_DATA_STREAM_IMG_RAW8U, image_size_send, image_width_send, image_height_send, image_size_send / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1, MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN, 100); LEDToggle(LED_COM); uint16_t frame = 0; for (frame = 0; frame < image_size_send / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1; frame++) { mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, &((uint8_t *) previous_image)[frame * MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN]); } send_image_now = false; } else if (!FLOAT_AS_BOOL(global_data.param[PARAM_USB_SEND_VIDEO])) { LEDOff(LED_COM); } } }