void task_poll_sonar(void* p){
while(1){
/*float distance1,distance2,distance3,distance4,distance5;
distance1 = sonar_read(TRIGGER_PIN,ECHO_PIN);
distance2= sonar_read(TRIGGER_PIN2,ECHO_PIN2);
distance3 = sonar_read(TRIGGER_PIN3,ECHO_PIN3);
distance4 = sonar_read(TRIGGER_PIN4,ECHO_PIN4);
distance5 = sonar_read(TRIGGER_PIN5,ECHO_PIN5);
dprintf("%d %d %d %d %d",(int)distance1,(int)distance2,(int)distance3,(int)distance4,(int)distance5);*/
data[ID_DATA_SONAR1] = (int) sonar_read(TRIGGER_PIN,ECHO_PIN);
dprintf("%d 1 ",(int) sonar_read(TRIGGER_PIN,ECHO_PIN));
//data[ID_DATA_SONAR2]= (int) sonar_read(TRIGGER_PIN2,ECHO_PIN2);
//data[ID_DATA_SONAR3] = (int) sonar_read(TRIGGER_PIN3,ECHO_PIN3);
//data[ID_DATA_SONAR4] = (int) sonar_read(TRIGGER_PIN4,ECHO_PIN4);
//data[ID_DATA_SONAR5] = (int) sonar_read(TRIGGER_PIN5,ECHO_PIN5);
//dprintf("%d %d %d %d %d",data[ID_DATA_SONAR1],data[ID_DATA_SONAR2],data[ID_DATA_SONAR3],data[ID_DATA_SONAR4],data[ID_DATA_SONAR5]);
vTaskDelay(taskDelay);
}
}
/**
* @brief Main function.
*/
int main(void)
{
/* 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 clock */
if (SysTick_Config(SystemCoreClock / 1000))
{
/* 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;
/* main loop */
while (1)
{
/* 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(global_data.param[PARAM_CALIBRATION_ON])
{
while(global_data.param[PARAM_CALIBRATION_ON])
{
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 (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;
gyro_read(&x_rate_sensor, &y_rate_sensor, &z_rate_sensor);
/* 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
/* debug */
float x_rate_pixel = x_rate * (get_time_between_images() / 1000.0f) * focal_length_px;
float y_rate_pixel = y_rate * (get_time_between_images() / 1000.0f) * focal_length_px;
//FIXME for the old sensor PX4FLOW v1.2 uncomment this!!!!
// x_rate = x_rate_raw_sensor; // change x and y rates
// y_rate = y_rate_raw_sensor;
/* get sonar data */
sonar_read(&sonar_distance_filtered, &sonar_distance_raw);
/* compute optical flow */
if(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);
if (sonar_distance_filtered > 5.0f || sonar_distance_filtered == 0.0f)
{
/* distances above 5m are considered invalid */
sonar_distance_filtered = 0.0f;
distance_valid = false;
}
else
{
distance_valid = true;
}
/*
* 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() / 1000.0f);
float flow_compy = pixel_flow_y / focal_length_px / (get_time_between_images() / 1000.0f);
/* 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;
if (qual > 0)
{
velocity_x_sum += new_velocity_x;
velocity_y_sum += new_velocity_y;
valid_frame_count++;
/* 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;
}
pixel_flow_x_sum += pixel_flow_x;
pixel_flow_y_sum += pixel_flow_y;
pixel_flow_count++;
}
counter++;
/* TODO for debugging */
//mavlink_msg_named_value_float_send(MAVLINK_COMM_2, boot_time_ms, "blabla", blabla);
if(global_data.param[PARAM_SENSOR_POSITION] == BOTTOM)
{
/* send bottom flow if activated */
if (counter % 2 == 0)
{
float flow_comp_m_x = 0.0f;
float flow_comp_m_y = 0.0f;
float ground_distance = 0.0f;
if(global_data.param[PARAM_BOTTOM_FLOW_LP_FILTERED])
{
flow_comp_m_x = velocity_x_lp;
flow_comp_m_y = velocity_y_lp;
}
else
{
flow_comp_m_x = velocity_x_sum/valid_frame_count;
flow_comp_m_y = velocity_y_sum/valid_frame_count;
}
if(global_data.param[PARAM_SONAR_FILTERED])
ground_distance = sonar_distance_filtered;
else
ground_distance = sonar_distance_raw;
if (valid_frame_count > 0)
{
// send flow
mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_ms() * 1000, 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);
if (global_data.param[PARAM_USB_SEND_FLOW])
mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_ms() * 1000, 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);
update_TX_buffer(pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f, flow_comp_m_x, flow_comp_m_y, qual,
ground_distance, x_rate, y_rate, z_rate);
}
else
{
// send distance
mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_ms() * 1000, global_data.param[PARAM_SENSOR_ID],
pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f,
0.0f, 0.0f, 0, ground_distance);
if (global_data.param[PARAM_USB_SEND_FLOW])
mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_ms() * 1000, global_data.param[PARAM_SENSOR_ID],
pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f,
0.0f, 0.0f, 0, ground_distance);
update_TX_buffer(pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f, 0.0f, 0.0f, 0, ground_distance, x_rate, y_rate,
z_rate);
}
if(global_data.param[PARAM_USB_SEND_GYRO])
{
mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "GYRO", get_boot_time_ms() * 1000, x_rate, y_rate, z_rate);
}
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 (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;
}
/* transmit raw 8-bit image */
if (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];
if (global_data.param[PARAM_VIDEO_USB_MODE] == CAM_VIDEO)
{
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 / 253 + 1,
253,
100);
LEDToggle(LED_COM);
uint16_t frame;
for (frame = 0; frame < image_size_send / 253 + 1; frame++)
{
mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, &((uint8_t *) current_image)[frame * 253]);
}
}
else if (global_data.param[PARAM_VIDEO_USB_MODE] == FLOW_VIDEO)
{
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 / 253 + 1,
253,
100);
LEDToggle(LED_COM);
uint16_t frame;
for (frame = 0; frame < image_size / 253 + 1; frame++)
{
mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, &((uint8_t *) previous_image)[frame * 253]);
}
}
send_image_now = false;
}
else if (!global_data.param[PARAM_USB_SEND_VIDEO])
{
LEDOff(LED_COM);
}
}
}
/**
* @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();
}
}
}
void task_poll_sensor(void* p){
while(1){
//unsigned int uS = sonar.ping(); // Send ping, get ping time in microseconds (uS).
//unsigned int uS2 = sonar2.ping();
/* Serial.print("Sonar 1: ");
Serial.print(sonar.convert_cm(uS)); // Convert ping time to distance and print result (0 = outside set distance range, no ping echo)
Serial.println("cm");
Serial.print("Sonar 2: ");
Serial.print(sonar2.convert_cm(uS2));
Serial.println("cm");*/
// dprintf("%d",(int)sonar.convert_cm(uS));
// vTaskDelay(1000);
// dprintf("%d",(int)sonar.convert_cm(uS2));
/* if(sonar.convert_cm(uS)<50){
digitalWrite(MOTOR, HIGH); // sets the LED on
delay(100); // waits for a second
//digitalWrite(MOTOR, LOW); // sets the LED off
//delay(1000); // waits for a second
}else{
digitalWrite(MOTOR, LOW);
delay(100);
}
*/
/* digitalWrite(TRIGGER_PIN, LOW);
delayMicroseconds(2);
digitalWrite(TRIGGER_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIGGER_PIN, LOW);
pinMode(ECHO_PIN,INPUT);
duration = pulseIn(ECHO_PIN, HIGH,100000);
//Calculate the distance (in cm) based on the speed of sound.
distance = duration/58.2;
// dprintf("%d 1", (int)distance);
digitalWrite(TRIGGER_PIN2, LOW);
delayMicroseconds(2);
digitalWrite(TRIGGER_PIN2, HIGH);
delayMicroseconds(10);
digitalWrite(TRIGGER_PIN2, LOW);
pinMode(ECHO_PIN2,INPUT);
duration = pulseIn(ECHO_PIN2, HIGH,100000);
//Calculate the distance (in cm) based on the speed of sound.
distance = duration/58.2;
// dprintf("%d 2", (int)distance);
digitalWrite(TRIGGER_PIN3, LOW);
delayMicroseconds(2);
digitalWrite(TRIGGER_PIN3, HIGH);
delayMicroseconds(10);
digitalWrite(TRIGGER_PIN3, LOW);
pinMode(ECHO_PIN3,INPUT);
duration = pulseIn(ECHO_PIN3, HIGH,100000);
//Calculate the distance (in cm) based on the speed of sound.
distance = duration/58.2;
// dprintf("%d 3", (int)distance);
digitalWrite(TRIGGER_PIN4, LOW);
delayMicroseconds(2);
digitalWrite(TRIGGER_PIN4, HIGH);
delayMicroseconds(10);
digitalWrite(TRIGGER_PIN4, LOW);
pinMode(ECHO_PIN4,INPUT);
duration = pulseIn(ECHO_PIN4, HIGH,100000);
//Calculate the distance (in cm) based on the speed of sound.
distance = duration/58.2;
// dprintf("%d 4", (int)distance);
digitalWrite(TRIGGER_PIN5, LOW);
delayMicroseconds(2);
digitalWrite(TRIGGER_PIN5, HIGH);
delayMicroseconds(10);
digitalWrite(TRIGGER_PIN5, LOW);
pinMode(ECHO_PIN5,INPUT);
duration = pulseIn(ECHO_PIN5, HIGH,100000);
//Calculate the distance (in cm) based on the speed of sound.
distance = duration/58.2;
// dprintf("%d 5", (int)distance);
//Calculate the distance (in cm) based on the speed of sound.
/*distance = duration/58.2;*/
float distance1,distance2,distance3,distance4,distance5;
distance1 = sonar_read(TRIGGER_PIN,ECHO_PIN);
distance2= sonar_read(TRIGGER_PIN2,ECHO_PIN2);
distance3 = sonar_read(TRIGGER_PIN3,ECHO_PIN3);
distance4 = sonar_read(TRIGGER_PIN4,ECHO_PIN4);
distance5 = sonar_read(TRIGGER_PIN5,ECHO_PIN5);
dprintf("%d %d %d %d %d",(int)distance1,(int)distance2,(int)distance3,(int)distance4,(int)distance5);
/*dprintf("%d", (int) sonar_read(TRIGGER_PIN,ECHO_PIN));
dprintf("%d", (int) sonar_read(TRIGGER_PIN2,ECHO_PIN2));
dprintf("%d", (int) sonar_read(TRIGGER_PIN3,ECHO_PIN3));
dprintf("%d", (int) sonar_read(TRIGGER_PIN4,ECHO_PIN4));
dprintf("%d", (int) sonar_read(TRIGGER_PIN5,ECHO_PIN5));*/
/*sonar final code
digitalWrite(TRIGGER_PIN, LOW);
delayMicroseconds(2);
digitalWrite(TRIGGER_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIGGER_PIN, LOW);
pinMode(ECHO_PIN,INPUT);
duration = pulseIn(ECHO_PIN, HIGH,100000);
//Calculate the distance (in cm) based on the speed of sound.
distance = duration/58.2;
*/
/*
pinMode(ECHO_PIN,INPUT);
digitalWrite(TRIGGER_PIN,HIGH);
delayMicroseconds(1000);
digitalWrite(TRIGGER_PIN,LOW);
duration = pulseIn(ECHO_PIN,HIGH);
distance = (duration/2)/29.1;*/
/* if(distance>10 && distance < 60){
digitalWrite(MOTOR, HIGH); // sets the LED on
// delay(100); // waits for a second
//digitalWrite(MOTOR, LOW); // sets the LED off
//delay(1000); // waits for a second
}else{
digitalWrite(MOTOR, LOW);
// delay(100);
}*/
// dprintf("%d",(int)distance);
/***********************************
** reading sensors
************************************/
compass.read();
dprintf("%d", int(compass.heading()));
//dprintf("%d z",(int)(compass.a.z/16.0));
/*if(compass.a.z/16.0<-1000){
distFromStart += 33;
step++;
dprintf("%d step",step);
}*/
/* float heading = compass.heading();
float XaVal, YaVal, ZaVal, fXa, fYa,fZa, pitch, roll,pitch_print, roll_print;
const float alpha = 0.15;
XaVal = compass.a.x/16.0; //Acceleration data registers contain a left-aligned 12-bit number, so values should be shifted right by 4 bits (divided by 16)
YaVal = compass.a.y/16.0; //unit is in cm/s2
ZaVal = compass.a.z/16.0;
/***********************************
** keypad
************************************/
char key = keypad.getKey();
//print out the key that is pressed
if (key != NO_KEY){
// Serial.print("You have pressed ");
Serial.println(key);
}
/***********************************
** altitude
************************************/
float pressure = ps.readPressureMillibars() + 248.5;
float altitude = ps.pressureToAltitudeMeters(pressure);
//dprintf("alt %d , pres %d",(int)altitude,(int)pressure);
// Serial.print("Pressure is ");
// Serial.print(pressure);
// Serial.println(" mbar");
// Serial.print("Altitude is ");
// Serial.print(altitude);// causes error
// Serial.println(" m.");
//dprintf("%d",(int)pressure);
//dprintf("%d",(int)altitude);
/******************************************************
** gyro meter reading
******************************************************/
gyro.read();
/*Serial.println("Gyro meter ");
Serial.print("X: ");
Serial.print((int)gyro.g.x * 8.75 /1000);
Serial.println(" degree/second");
Serial.print("Y: ");
Serial.print((int)gyro.g.y * 8.75 /1000);
Serial.println(" degree/second");
Serial.print("Z: ");
Serial.print((int)gyro.g.z * 8.75 /1000);
Serial.println(" degree/second");
Serial.println("");*/
//dprintf("x: %d",(int)(gyro.g.x * 8.75 /1000));
//dprintf("y: %d",(int)(gyro.g.y * 8.75 /1000));
//dprintf("z: %d",(int)(gyro.g.z * 8.75 /1000));
/*******************************************************************
get Headings
When given no arguments, the heading() function returns the angular
difference in the horizontal plane between a default vector and
north, in degrees.
/*
When given no arguments, the heading() function returns the angular
difference in the horizontal plane between a default vector and
north, in degrees.
The default vector is chosen by the library to point along the
surface of the PCB, in the direction of the top of the text on the
silkscreen. This is the +X axis on the Pololu LSM303D carrier and
the -Y axis on the Pololu LSM303DLHC, LSM303DLM, and LSM303DLH
carriers.
To use a different vector as a reference, use the version of heading()
that takes a vector argument; for example, use
compass.heading((LSM303::vector<int>){0, 0, 1});
to use the +Z axis as a reference.
*******************************************************************/
// String direction = "";
/*if(heading>=340 || heading <= 20)
dprintf("North"); // direction = "North";
else if (heading>=70 && heading <= 110)
dprintf("East"); // direction = "East";
else if (heading>=160 && heading <= 200)
dprintf("South"); //direction = "South";
else if (heading>=250 && heading <= 290)
dprintf("West"); // direction = "West";
else if (heading>20 && heading < 70)
dprintf("North East"); // direction = "North East";
else if (heading>110 && heading < 160)
dprintf("South East"); // direction = "South East";
else if (heading>200 && heading < 250)
dprintf("South West");// direction = "South West";
else if (heading>290 && heading < 340)
dprintf("North West"); // direction = "North West";
// Serial.print("Heading is ");
//Serial.println(direction);
//Serial.println("degree.");
/******************************************************
** Method 1 to calculate distance: using steps
******************************************************/
// a step and distance using Z-ACCELERATION
/* if(ZaVal<-950){
distFromStart+=33; //1 step is 33 cm
step++;
}
/* Serial.print("X accel is ");Serial.print(XaVal); Serial.print(" cm/s2"); Serial.println(" ");
Serial.print("Y accel is ");Serial.print(YaVal); Serial.print(" cm/s2"); Serial.println(" ");
Serial.print("Z accel is ");Serial.print(ZaVal);Serial.print(" cm/s2"); Serial.println(" ");
Serial.print("1. You have walked ");
Serial.print(step);
Serial.print(" steps and distance is ");
Serial.print(distFromStart);
Serial.println(" cm from start");*/
/*dprintf("x accel %d", (int)XaVal);
dprintf("y accel %d",(int) YaVal);
dprintf("z accel %d", (int)ZaVal); */
/******************************************************
** pitch and roll
******************************************************/
// Low-Pass filter accelerometer
/* fXa = XaVal * alpha + (fXa * (1.0 - alpha));
fYa = YaVal * alpha + (fYa * (1.0 - alpha));
fZa = ZaVal * alpha + (fZa * (1.0 - alpha));
/* Serial.print("Low pass X accel is ");Serial.print(fXa); Serifal.print(" cm/s2"); Serial.println(" ");
Serial.print("Low pass Y accel is ");Serial.print(fYa); Serial.print(" cm/s2"); Serial.println(" ");
Serial.print("Low pass Z accel is ");Serial.print(fZa);Serial.print(" cm/s2"); Serial.println(" "); */
/* roll = atan2(fYa, sqrt(fXa*fXa + fZa*fZa));
pitch = atan2(fXa, sqrt(fYa*fYa + fZa*fZa));
roll_print = roll*180.0/M_PI;
pitch_print = pitch*180.0/M_PI;
/* Serial.print("pitch(Y) is ");
Serial.print(pitch_print);
Serial.println("degree ");
Serial.print("roll(X) is ");
Serial.print(roll_print);
Serial.println("degree ");*/
/******************************************************
** Method 2 to calculate distance: using accelerations
******************************************************/
/* newTime = millis();
deltaTime = newTime - oldTime;
XaVal = XaVal - (1000 * (sin(pitch)));//offsetting pitch
// estimate the average acceleration since the previous sample, by averaging the two samples
long avgAccel = (oldXaVal + XaVal) / 2;
//if ((XaVal < 50 && XaVal > -50) && (oldXaVal < 50 && oldXaVal > -50))
// avgAccel = 0;
/* working
Serial.print("the avgAccel is ");
Serial.print(avgAccel);
Serial.println(" cm/s2");*/
// integrate the average accel and add it to the previous speed to calculate the new speed
// long newVelocity = oldVelocity + (avgAccel * deltaTime/1000);
//estimate the average speed since the previous sample, by averaging the two speeds
//long avgVelocity = (oldVelocity + newVelocity) / 2;
// if ((XaVal < 50 && XaVal > -50) && (oldXaVal < 50 && oldXaVal > -50))
// avgVelocity = 0;
// integrate the average speed and add it to the previous displacement to get the new displacement
/* long newDisplacement = oldDis + (avgVelocity * deltaTime/1000);
oldTime = newTime;
oldVelocity = newVelocity ;
oldDis = newDisplacement;
oldXaVal = XaVal;*/
/*working
Serial.print("2. You have walked ");
Serial.print(newDisplacement);
Serial.println("cm from start"); */
/******************************************************
** IR sensor meter reading
******************************************************/
sensorValue = analogRead(sensorIR);
cm = 10650.08 * pow(sensorValue,-0.935) - 10;
/* Serial.print("IR sensor reads ");
Serial.print(cm);
Serial.println(" Cm");*/
//delay(100);
vTaskDelay(100);
}
}
/**
* @brief Main function.
*/
int main(void)
{
/* 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 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;
uint8_t current_image1[64][64];
uint8_t current_image2[64][64];
uint8_t current_image3[64][64];
uint8_t current_image4[64][64];
uint8_t previous_image1[64][64];
uint8_t previous_image2[64][64];
uint8_t previous_image3[64][64];
uint8_t previous_image4[64][64];
for(int i=0;i<64;++i)
{
for(int j=0;j<64;++j)
{
current_image1[i][j]=0;
previous_image1[i][j]=0;
current_image2[i][j]=0;
previous_image2[i][j]=0;
current_image3[i][j]=0;
previous_image3[i][j]=0;
current_image4[i][j]=0;
previous_image4[i][j]=0;
}
}
uint8_t ourImage[OurSize];
for(int i=0;i<OurSize;++i)
{
ourImage[i]=0;
}
/* 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;
float pixel_flow_x_lp = 0.0f;
float pixel_flow_y_lp = 0.0f;
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;
static float last_vel_x = 0;
static float last_vel_y = 0;
float vel_x = 0, vel_y = 0;
//Change
int count=1;
//int gyroCount=1;
uint8_t * tmp_image1 = *previous_image;
uint8_t * tmp_image2 = *current_image;
float x_temprate=0;
float y_temprate=0;
float z_temprate=0;
int defCount=3;
for (uint16_t pixel = 0; pixel < global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; pixel++)
{
*(tmp_image1+pixel)=0;
*(tmp_image2+pixel)=0;
}
uint16_t image_sum1=0;
uint16_t image_sum2=0;
uint16_t image_sum3=0;
uint16_t image_sum4=0;
//EndChange
/* main loop */
while (1)
{
uint16_t image_size = global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT];
/* 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(global_data.param[PARAM_VIDEO_ONLY])
{
while(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 (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);
}
//StartChange
if(count<=defCount)
{
if(count==1)
{
for (uint16_t pixel = 0; pixel < image_size; pixel++)
{
image_sum1+=*(previous_image+pixel);
}
for (uint16_t pixel = 0; pixel < image_size; pixel++)
{
*(tmp_image1+pixel)=(uint8_t)(*(previous_image+pixel));
}
}
else
{
for (uint16_t pixel = 0; pixel < image_size; pixel++)
{
image_sum2+=*(previous_image+pixel);
}
if(image_sum1<image_sum2)
{
image_sum1=image_sum2;
for (uint16_t pixel = 0; pixel < image_size; pixel++)
{
*(tmp_image1+pixel)=(uint8_t)(*(previous_image+pixel));
}
}
image_sum2=0;
}
}
if(count>defCount&&count<=2*defCount)
{
// uint16_t image_sum1=0;
// uint16_t image_sum2=0;
if(count==defCount+1)
{
for (uint16_t pixel = 0; pixel < image_size; pixel++)
{
image_sum3+=*(current_image+pixel);
}
for (uint16_t pixel = 0; pixel < image_size; pixel++)
{
*(tmp_image2+pixel)=(uint8_t)(*(previous_image+pixel));
}
}
else
{
for (uint16_t pixel = 0; pixel < image_size; pixel++)
{
image_sum4+=*(current_image+pixel);
}
if(image_sum3<image_sum4)
{
image_sum3=image_sum4;
for (uint16_t pixel = 0; pixel < image_size; pixel++)
{
*(tmp_image2+pixel)=(uint8_t)(*(previous_image+pixel));
}
}
image_sum4=0;
}
}
// if(count<=defCount)
// {
// for (uint16_t pixel = 0; pixel < image_size; pixel++)
// {
// *(tmp_image1+pixel)+=(uint8_t)(*(previous_image+pixel)/defCount);
// }
// }
// if(count>defCount&&count<=2*defCount)
// {
// for (uint16_t pixel = 0; pixel < image_size; pixel++)
// {
// *(tmp_image2+pixel)+=(uint8_t)(*(current_image+pixel)/defCount);
// }
//
// }
count++;
if(count==2*defCount+1)
{
/* 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 */
x_temprate += y_rate_sensor/(2*defCount); // change x and y rates
y_temprate += - x_rate_sensor/(2*defCount);
z_temprate += z_rate_sensor/(2*defCount); // 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;
}
float x_rate = x_temprate; // change x and y rates
float y_rate = - y_temprate;
float z_rate = z_temprate; // z is correct
x_temprate =0.0f;
y_temprate =0.0f;
z_temprate =0.0f;
// /* 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);
*previous_image=tmp_image1;
*current_image=tmp_image2;
count=1;
for (uint16_t pixel = 0; pixel < image_size; pixel++)
{
*(tmp_image1+pixel)=0;
*(tmp_image2+pixel)=0;
}
image_sum1=0;
image_sum2=0;
image_sum3=0;
image_sum4=0;
/* compute optical flow */
if(global_data.param[PARAM_SENSOR_POSITION] == BOTTOM)
{
/* copy recent image to faster ram */
dma_copy_image_buffers(¤t_image, &previous_image, image_size, 1);
for(int i=0;i<64;++i)
{
for(int j=0;j<64;++j)
{
previous_image1[i][j]=*(previous_image+64*i+j);
current_image1[i][j]=*(current_image+64*i+j);
}
}
/* 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);
/* 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;
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;
/* 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(global_data.param[PARAM_SENSOR_POSITION] == BOTTOM)
{
/* send bottom flow if activated */
float ground_distance = 0.0f;
if(global_data.param[PARAM_SONAR_FILTERED])
{
ground_distance = sonar_distance_filtered;
}
else
{
ground_distance = sonar_distance_raw;
}
if(global_data.param[PARAM_BOTTOM_FLOW_LP_FILTERED])
{
/* lowpass velocity output */
pixel_flow_x_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * pixel_flow_x +
(1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * pixel_flow_x_lp;
pixel_flow_y_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * pixel_flow_y +
(1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * pixel_flow_y_lp;
}
else
{
pixel_flow_x_lp = pixel_flow_x;
pixel_flow_y_lp = pixel_flow_y;
}
//update I2C transmitbuffer
if(valid_frame_count>0)
{
update_TX_buffer(pixel_flow_x_lp, pixel_flow_y_lp, velocity_x_sum/valid_frame_count, velocity_y_sum/valid_frame_count, qual,
ground_distance, x_rate, y_rate, z_rate, gyro_temp);
last_vel_x = velocity_x_sum/valid_frame_count;
last_vel_y = velocity_y_sum/valid_frame_count;
vel_x = last_vel_x;
vel_y = last_vel_y;
}
else
{
/*
update_TX_buffer(pixel_flow_x_lp, pixel_flow_y_lp, last_vel_x, last_vel_y, qual,
ground_distance, x_rate, y_rate, z_rate, gyro_temp);
vel_x = 0;
vel_y = 0;
*/
}
//serial mavlink + usb mavlink output throttled
if (counter % (uint32_t)global_data.param[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR] == 0)//throttling factor
{
float flow_comp_m_x = 0.0f;
float flow_comp_m_y = 0.0f;
if(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);
/*
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,
vel_x, vel_y, accumulated_gyro_z,
gyro_temp, accumulated_quality/accumulated_framecount,
time_since_last_sonar_update,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,
last_vel_x, last_vel_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 (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;
/* velocity not directly measured and not important for testing */
lpos.vx = 0;
lpos.vy = 0;
lpos.vz = 0;
} 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 (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(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 (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 (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 (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 *) current_image)[frame * MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN]);
}
send_image_now = false;
}
else if (!global_data.param[PARAM_USB_SEND_VIDEO])
{
LEDOff(LED_COM);
}
}
//EndChange
}
}
void autonomous_routine1(void)
{
unsigned int sonar_distance = 0;
static unsigned long elapsed_time,
old_time = 0,
start_time;
controller_begin_autonomous_mode();
elapsed_time = start_time = SYSTEM_TIMER_SECONDS();
/*
* An autonomous loop with sensor input. Loop terminates when
* a button sensor on port 5 is pressed, or the 20 second time
* period runs out.
*/
forward1(MOTOR_POWER1);
while ( (elapsed_time - start_time) < ROUTINE1_DURATION )
{
sonar_distance = sonar_read(SONAR_INTERRUPT_PORT);
if ( (sonar_distance < ROUTINE1_SONAR_DISTANCE) ||
(io_read_digital(BUMPER_LEFT_PORT) == 0) ||
(io_read_digital(BUMPER_RIGHT_PORT) == 0) )
{
reverse1(MOTOR_POWER1);
delay_msec(ROUTINE1_BACKUP_MS);
sonar_distance = sonar_read(SONAR_INTERRUPT_PORT);
spin1(MOTOR_POWER1, ROUTINE1_SPIN_MS);
forward1(MOTOR_POWER1);
sonar_distance = sonar_read(SONAR_INTERRUPT_PORT);
}
elapsed_time = SYSTEM_TIMER_MS();
/* Adjust sonar direction every 40 ms */
if ( elapsed_time % ROUTINE1_SONAR_SERVO_DELAY == 0 )
sonar_scan(SONAR_SERVO_PORT);
/* Produce debug output once per second */
elapsed_time /= MS_PER_SEC;
if ( elapsed_time > old_time )
{
old_time = elapsed_time;
/*
if ( rc_new_data_available() )
{
DPRINTF("ET: %ld RC: %x A[1,2]: %x %x "
"D[5,6]: %d %d Shaft I[%d,%d]: %d %d Sonar[%d]: %d\n",
elapsed_time - start_time, rc_read_status(),
io_read_analog(1), io_read_analog(2),
io_read_digital(5), io_read_digital(6),
LEFT_ENCODER_INTERRUPT_PORT, RIGHT_ENCODER_INTERRUPT_PORT,
shaft_encoder_read_std(LEFT_ENCODER_INTERRUPT_PORT),
shaft_encoder_read_std(RIGHT_ENCODER_INTERRUPT_PORT),
SONAR_INTERRUPT_PORT, sonar_distance);
}
*/
}
}
pwm_write(RIGHT_DRIVE_PORT, MOTOR_STOP);
pwm_write(LEFT_DRIVE_PORT, MOTOR_STOP);
controller_submit_data(NO_WAIT);
controller_end_autonomous_mode();
}
void rc_routine(void)
{
/*
* Keep a count of calls to this function. This will overflow
* at 2^16 (65, 536), but we don't really care since we're only
* interested in whether or not calls is a multiple of some number.
*/
static unsigned int sonar_distance = 0;
static unsigned long elapsed_time,
old_time = 0;
static line_sensor_t line_sensor;
sonar_distance = sonar_read(SONAR_INTERRUPT_PORT);
#if DRIVE_ROUTINE == DEFAULT
/*
* VEX default code behavior. The default code assumes a squarebot
* or similar configuration.
* Replace this with your own function to implement different behavior.
*/
vex_default_routine(User_rxdata,txdata);
#elif DRIVE_ROUTINE == TANK
/*
* Alternative to default code behavior for robots with tank
* drive (wheels driven directly by motors, no gearing).
*/
tank_drive_routine();
#elif DRIVE_ROUTINE == ARCADE
arcade_drive_routine();
#endif
controller_submit_data(NO_WAIT);
/*
* Since serial output is slow, we only display once per second
* so that the program won't be constantly waiting for terminal
* output to finish.
*/
elapsed_time = SYSTEM_TIMER_SECONDS();
if ( elapsed_time > old_time )
{
old_time = elapsed_time;
#if DEBUG_STACK
stack_report();
#endif
line_sensor_read(LINE_SENSOR_LEFT_PORT, LINE_SENSOR_CENTER_PORT,
LINE_SENSOR_RIGHT_PORT, &line_sensor);
/*
* Display status of various sensors and rc inputs.
*/
DPRINTF("ET: %ld RC: %d %d %d %d %d %d %d Line: %x %x %x Light %x ",
elapsed_time, rc_read_status(),
rc_read_data(1), rc_read_data(2),
rc_read_data(3), rc_read_data(4),
rc_read_data(5), rc_read_data(6),
line_sensor.left, line_sensor.center, line_sensor.right,
io_read_analog(LIGHT_SENSOR_PORT));
DPRINTF("B: %d %d Shaft I[%d,%d]: %d %d Sonar[%d]: %d T1: %ld\n",
io_read_digital(BUMPER_LEFT_PORT),
io_read_digital(BUMPER_RIGHT_PORT),
LEFT_ENCODER_INTERRUPT_PORT, RIGHT_ENCODER_INTERRUPT_PORT,
shaft_encoder_read_std(LEFT_ENCODER_INTERRUPT_PORT),
shaft_encoder_read_std(RIGHT_ENCODER_INTERRUPT_PORT),
SONAR_INTERRUPT_PORT, sonar_distance, timer_read_ms(1));
}
}
/**
* @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);
}
}
}
