bool calibration_enter(void) { // If not flying if (!sys_state_is_flying()) { calibration_prev_state = sys_get_state(); calibration_prev_mode = sys_get_mode(); // Lock vehicle during calibration sys_set_mode((uint8_t)MAV_MODE_LOCKED); sys_set_state((uint8_t)MAV_STATE_CALIBRATING); debug_message_buffer("Starting calibration."); mavlink_msg_sys_status_send(MAVLINK_COMM_0, global_data.state.mav_mode, global_data.state.nav_mode, global_data.state.status, global_data.cpu_usage, global_data.battery_voltage, global_data.motor_block, communication_get_uart_drop_rate()); mavlink_msg_sys_status_send(MAVLINK_COMM_1, global_data.state.mav_mode, global_data.state.nav_mode, global_data.state.status, global_data.cpu_usage, global_data.battery_voltage, global_data.motor_block, communication_get_uart_drop_rate()); debug_message_send_one(); debug_message_send_one(); return true; } else { //Can't calibrate during flight debug_message_buffer("Can't calibrate during flight!!!"); return false; } }
static void mavlinkStateMachineTask(void* parameters) { // MAVLink protocol state machine while (1) { // Run handlers, sleep for 20 ms // Send setpoints, time out // Send one param mavlink_pm_queued_send(); vTaskDelay(5); mavlink_wpm_loop(); vTaskDelay(5); // Send one text message debug_message_send_one(); // Wait 20 ms vTaskDelay(20); } }
void calibration_exit(void) { // Go back to old state sys_set_mode(calibration_prev_mode); sys_set_state(calibration_prev_state); // Clear debug message buffers for (int i = 0; i < DEBUG_COUNT; i++) { debug_message_send_one(); } // Clear UART buffers while (uart0_char_available()) {uart0_get_char();} while (uart1_char_available()) {uart1_get_char();} debug_message_buffer("Calibration finished. UART buffers cleared."); }
/** * @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); } } }
void send_params_fn(void) { debug_message_send_one(); communication_parameter_send(); }
/** * @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 } }
/** * @brief This is the main loop * * It will be executed at maximum MCU speed (60 Mhz) */ void main_loop_ground_car(void) { last_mainloop_idle = sys_time_clock_get_time_usec(); debug_message_buffer("Starting main loop"); while (1) { // Time Measurement uint64_t loop_start_time = sys_time_clock_get_time_usec(); /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// CRITICAL 200 Hz functions /////////////////////////////////////////////////////////////////////////// if (us_run_every(5000, COUNTER2, loop_start_time)) { // Kalman Attitude filter, used on all systems gyro_read(); sensors_read_acc(); // Read out magnetometer at its default 50 Hz rate static uint8_t mag_count = 0; if (mag_count == 3) { sensors_read_mag(); attitude_observer_correct_magnet(global_data.magnet_corrected); mag_count = 0; } else { mag_count++; } // Correction step of observer filter attitude_observer_correct_accel(global_data.accel_raw); // Write in roll and pitch static float_vect3 att; //if not static we have spikes in roll and pitch on bravo !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! attitude_observer_get_angles(&att); global_data.attitude.x = att.x; global_data.attitude.y = att.y; if (global_data.param[PARAM_ATT_KAL_IYAW]) { global_data.attitude.z += 0.005 * global_data.gyros_si.z; } else { global_data.attitude.z = att.z; } // Prediction step of observer attitude_observer_predict(global_data.gyros_si); // TODO READ OUT MOUSE SENSOR } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// CRITICAL FAST 50 Hz functions /////////////////////////////////////////////////////////////////////////// else if (us_run_every(20000, COUNTER3, loop_start_time)) { // Read Analog-to-Digital converter adc_read(); // Read remote control remote_control(); // Send the raw sensor/ADC values communication_send_raw_data(loop_start_time); } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// UNCRITICAL SLOW 5 Hz functions /////////////////////////////////////////////////////////////////////////// else if (us_run_every(200000, COUNTER8, loop_start_time)) { // Send buffered data such as debug text messages communication_queued_send(); // Empty one message out of the buffer debug_message_send_one(); // Toggle status led //led_toggle(LED_YELLOW); led_toggle(LED_RED); // just for green LED on alpha at the moment // Toggle active mode led if (global_data.state.mav_mode == MAV_MODE_MANUAL || global_data.state.mav_mode == MAV_MODE_GUIDED || global_data.state.mav_mode == MAV_MODE_AUTO) { led_on(LED_GREEN); } else { led_off(LED_GREEN); } handle_eeprom_write_request(); handle_reset_request(); communication_send_remote_control(); // Pressure sensor driver works, but not tested regarding stability sensors_pressure_bmp085_read_out(); } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// NON-CRITICAL SLOW 20 Hz functions /////////////////////////////////////////////////////////////////////////// else if (us_run_every(50000, COUNTER7, loop_start_time)) { led_toggle(LED_YELLOW); communication_send_attitude_position(loop_start_time); } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// NON-CRITICAL SLOW 200 Hz functions // /////////////////////////////////////////////////////////////////////////// else if (us_run_every(5000, COUNTER5, loop_start_time)) { if (global_data.state.status == MAV_STATE_STANDBY) { //Check if parameters should be written or read param_handler(); } } /////////////////////////////////////////////////////////////////////////// else { // All Tasks are fine and we have no starvation last_mainloop_idle = loop_start_time; } // Read out comm at max rate - takes only a few microseconds in worst case communication_receive(); // MCU load measurement uint64_t loop_stop_time = sys_time_clock_get_time_usec(); global_data.cpu_usage = measure_avg_cpu_load(loop_start_time, loop_stop_time, min_mainloop_time); global_data.cpu_peak = measure_peak_cpu_load(loop_start_time, loop_stop_time, min_mainloop_time); if (loop_start_time - last_mainloop_idle >= 5000) { debug_message_buffer( "CRITICAL WARNING! CPU LOAD TO HIGH. STARVATION!"); last_mainloop_idle = loop_start_time;//reset to prevent multiple messages } if (global_data.cpu_usage > 800) { // CPU load higher than 80% debug_message_buffer("CRITICAL WARNING! CPU LOAD HIGHER THAN 80%"); } } // End while(1) }
/** * @brief This is the main loop * * It will be executed at maximum MCU speed (60 Mhz) */ void main_loop_fixed_wing(void) { last_mainloop_idle = sys_time_clock_get_time_usec(); debug_message_buffer("Starting main loop"); while (1) { // Time Measurement uint64_t loop_start_time = sys_time_clock_get_time_usec(); /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// Camera Shutter /////////////////////////////////////////////////////////////////////////// // Set camera shutter with 2.5ms resolution if (us_run_every(2500, COUNTER1, loop_start_time)) { camera_shutter_handling(loop_start_time); } if (global_data.state.mav_mode == MAV_MODE_RC_TRAINING) { /////////////////////////////////////////////////////////////////////////// /// RC INTERFACE HACK AT 50 Hz /////////////////////////////////////////////////////////////////////////// if (us_run_every(20000, COUNTER8, loop_start_time)) { // Write start byte uart1_transmit(0xFF); // Write channels 1-6 for (int i = 1; i < 7; i++) { uart1_transmit((radio_control_get_channel(1)+1)*127); } } led_toggle(LED_GREEN); led_toggle(LED_RED); // Do not execute any of the functions below continue; } /////////////////////////////////////////////////////////////////////////// /// CRITICAL 200 Hz functions /////////////////////////////////////////////////////////////////////////// if (us_run_every(5000, COUNTER2, loop_start_time)) { // Kalman Attitude filter, used on all systems gyro_read(); sensors_read_acc(); // Read out magnetometer at its default 50 Hz rate static uint8_t mag_count = 0; if (mag_count == 3) { sensors_read_mag(); attitude_observer_correct_magnet(global_data.magnet_corrected); mag_count = 0; } else { mag_count++; } // Correction step of observer filter attitude_observer_correct_accel(global_data.accel_raw); // Write in roll and pitch static float_vect3 att; //if not static we have spikes in roll and pitch on bravo !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! attitude_observer_get_angles(&att); global_data.attitude.x = att.x; global_data.attitude.y = att.y; if (global_data.param[PARAM_ATT_KAL_IYAW]) { global_data.attitude.z += 0.005 * global_data.gyros_si.z; } else { global_data.attitude.z = att.z; } // Prediction step of observer attitude_observer_predict(global_data.gyros_si); control_fixed_wing_attitude(); } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// CRITICAL FAST 50 Hz functions /////////////////////////////////////////////////////////////////////////// else if (us_run_every(20000, COUNTER3, loop_start_time)) { // Read Analog-to-Digital converter adc_read(); // Read remote control remote_control(); } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// NON-CRITICAL SLOW 100 Hz functions /////////////////////////////////////////////////////////////////////////// else if (us_run_every(10000, COUNTER6, loop_start_time)) { // Send the raw sensor/ADC values communication_send_raw_data(loop_start_time); } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// UNCRITICAL SLOW 5 Hz functions /////////////////////////////////////////////////////////////////////////// else if (us_run_every(200000, COUNTER8, loop_start_time)) { // The onboard controllers go into failsafe mode once // position data is missing handle_controller_timeouts(loop_start_time); // Send buffered data such as debug text messages communication_queued_send(); // Empty one message out of the buffer debug_message_send_one(); // Toggle status led //led_toggle(LED_YELLOW); led_toggle(LED_RED); // just for green LED on alpha at the moment // Toggle active mode led if (global_data.state.mav_mode == MAV_MODE_MANUAL || global_data.state.mav_mode == MAV_MODE_GUIDED || global_data.state.mav_mode == MAV_MODE_AUTO) { led_on(LED_GREEN); } else { led_off(LED_GREEN); } handle_eeprom_write_request(); handle_reset_request(); communication_send_controller_feedback(); communication_send_remote_control(); // Pressure sensor driver works, but not tested regarding stability sensors_pressure_bmp085_read_out(); if (global_data.param[PARAM_POSITION_YAW_TRACKING] == 1) { mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 90, global_data.param[PARAM_POSITION_SETPOINT_YAW]); mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 91, global_data.yaw_pos_setpoint); } } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// NON-CRITICAL SLOW 1 Hz functions /////////////////////////////////////////////////////////////////////////// else if (us_run_every(1000000, COUNTER9, loop_start_time)) { // Send system state, mode, battery voltage, etc. send_system_state(); if (global_data.param[PARAM_GPS_MODE] >= 10) { //Send GPS information float_vect3 gps; gps.x = gps_utm_north / 100.0f;//m gps.y = gps_utm_east / 100.0f;//m gps.z = gps_utm_zone;// gps_week; debug_vect("GPS", gps); } beep_on_low_voltage(); } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// NON-CRITICAL SLOW 20 Hz functions /////////////////////////////////////////////////////////////////////////// else if (us_run_every(50000, COUNTER7, loop_start_time)) { led_toggle(LED_YELLOW); if (global_data.param[PARAM_GPS_MODE] >= 10) { //get thru all gps messages debug_message_send_one(); } communication_send_attitude_position(loop_start_time); } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// NON-CRITICAL SLOW 200 Hz functions // /////////////////////////////////////////////////////////////////////////// else if (us_run_every(5000, COUNTER5, loop_start_time)) { if (global_data.state.status == MAV_STATE_STANDBY) { //Check if parameters should be written or read param_handler(); } } /////////////////////////////////////////////////////////////////////////// else { // All Tasks are fine and we have no starvation last_mainloop_idle = loop_start_time; } // Read out comm at max rate - takes only a few microseconds in worst case communication_receive(); // MCU load measurement uint64_t loop_stop_time = sys_time_clock_get_time_usec(); global_data.cpu_usage = measure_avg_cpu_load(loop_start_time, loop_stop_time, min_mainloop_time); global_data.cpu_peak = measure_peak_cpu_load(loop_start_time, loop_stop_time, min_mainloop_time); if (loop_start_time - last_mainloop_idle >= 5000) { debug_message_buffer( "CRITICAL WARNING! CPU LOAD TO HIGH. STARVATION!"); last_mainloop_idle = loop_start_time;//reset to prevent multiple messages } if (global_data.cpu_usage > 800) { // CPU load higher than 80% debug_message_buffer("CRITICAL WARNING! CPU LOAD HIGHER THAN 80%"); } } // End while(1) }
void main_loop_quadrotor(void) { /** * @brief Initialize the whole system * * All functions that need to be called before the first mainloop iteration * should be placed here. */ main_init_generic(); control_quadrotor_position_init(); control_quadrotor_attitude_init(); attitude_tobi_laurens_init(); // FIXME XXX Make proper mode switching // outdoor_position_kalman_init(); //vision_position_kalman_init(); // Default filters, allow Vision, Vicon and optical flow inputs vicon_position_kalman_init(); optflow_speed_kalman_init(); /** * @brief This is the main loop * * It will be executed at maximum MCU speed (60 Mhz) */ // Executiontime debugging time_debug.x = 0; time_debug.y = 0; time_debug.z = 0; last_mainloop_idle = sys_time_clock_get_time_usec(); debug_message_buffer("Starting main loop"); led_off(LED_GREEN); led_off(LED_RED); while (1) { // Time Measurement uint64_t loop_start_time = sys_time_clock_set_loop_start_time(); // loop_start_time should not be used anymore /////////////////////////////////////////////////////////////////////////// /// CRITICAL 200 Hz functions /////////////////////////////////////////////////////////////////////////// if (us_run_every(5000, COUNTER2, loop_start_time)) { // Kalman Attitude filter, used on all systems gyro_read(); sensors_read_acc(); sensors_pressure_bmp085_read_out(); // Read out magnetometer at its default 50 Hz rate static uint8_t mag_count = 0; if (mag_count == 3) { sensors_read_mag(); //attitude_observer_correct_magnet(global_data.magnet_corrected); mag_count = 0; }else if(mag_count==1){ hmc5843_start_read(); mag_count++; } else { mag_count++; } // Correction step of observer filter attitude_tobi_laurens(); if (global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_VICON_ONLY || global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_VISION_VICON_BACKUP) { vicon_position_kalman(); } else if (global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_GPS_ONLY) { outdoor_position_kalman(); } control_quadrotor_attitude(); //debug counting number of executions count++; } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// Camera Shutter - This takes 50 usecs!!! /////////////////////////////////////////////////////////////////////////// // Set camera shutter with 2.5ms resolution else if (us_run_every(5000, COUNTER1, loop_start_time)) //was 2500 !!! { camera_shutter_handling(loop_start_time); // Measure time for debugging time_debug.x = max(time_debug.x, sys_time_clock_get_time_usec() - loop_start_time); } /////////////////////////////////////////////////////////////////////////// /// CRITICAL FAST 50 Hz functions /////////////////////////////////////////////////////////////////////////// else if (us_run_every(20000, COUNTER3, loop_start_time)) { // Read infrared sensor //adc_read(); // Control the quadrotor position control_quadrotor_position(); // Read remote control remote_control(); control_camera_angle(); // //float_vect3 opt; // static float_vect3 opt_int; // uint8_t valid = optical_flow_get_dxy(80, &global_data.optflow.x, &global_data.optflow.y, &global_data.optflow.z); // if (valid) // { // opt_int.x += global_data.optflow.x; // opt_int.y += global_data.optflow.y; // // } // // uint8_t supersampling = 10; // for (int i = 0; i < supersampling; ++i) // { // global_data.sonar_distance += sonar_distance_get(ADC_5_CHANNEL); // } // // global_data.sonar_distance /= supersampling; // // opt_int.z = valid; // static unsigned int i = 0; // if (i == 10) // { // mavlink_msg_optical_flow_send(global_data.param[PARAM_SEND_DEBUGCHAN], sys_time_clock_get_unix_loop_start_time(), 0, global_data.optflow.x, global_data.optflow.y, global_data.optflow.z, global_data.sonar_distance_filtered); // // i = 0; // } // i++; //optical_flow_debug_vect_send(); //debug_vect("opt_int", opt_int); // optical_flow_start_read(80); if (global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_INTEGRATING || global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_NON_INTEGRATING || global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_ADD_VICON_AS_OFFSET || global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_ADD_VISION_AS_OFFSET || global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_ODOMETRY_ADD_VISION_AS_OFFSET || global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_VICON || global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_GPS_OPTICAL_FLOW || global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_GLOBAL_VISION || global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_VISUAL_ODOMETRY_GLOBAL_VISION) { optflow_speed_kalman(); } // Send the raw sensor/ADC values communication_send_raw_data(loop_start_time); float_vect3 yy; yy.x = global_data.yaw_lowpass; yy.y = 0.f; yy.z = 0.f; debug_vect("yaw_low", yy); } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// CRITICAL FAST 20 Hz functions /////////////////////////////////////////////////////////////////////////// else if (us_run_every(50000, COUNTER4, loop_start_time)) { //*** this happens in handle_controller_timeouts already!!!!! *** // //update global_data.state // if (global_data.param[PARAM_VICON_MODE] == 1) // { // //VICON_MODE 1 only accepts vicon position // global_data.state.position_fix = global_data.state.vicon_ok; // } // else // { // //VICON_MODEs 0, 2, 3 accepts vision additionally, so check vision // global_data.state.position_fix = global_data.state.vision_ok; // } update_system_statemachine(loop_start_time); update_controller_setpoints(); mavlink_msg_roll_pitch_yaw_thrust_setpoint_send( global_data.param[PARAM_SEND_DEBUGCHAN], sys_time_clock_get_loop_start_time_boot_ms(), global_data.attitude_setpoint.x, global_data.attitude_setpoint.y, global_data.position_yaw_control_output, global_data.thrust_control_output); //STARTING AND LANDING quadrotor_start_land_handler(loop_start_time); } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// NON-CRITICAL SLOW 100 Hz functions /////////////////////////////////////////////////////////////////////////// else if (us_run_every(5000, COUNTER6, loop_start_time)) { if (global_data.param[PARAM_SEND_SLOT_DEBUG_6]) { debug_vect("att_ctrl_o", global_data.attitude_control_output); } } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// UNCRITICAL SLOW 5 Hz functions /////////////////////////////////////////////////////////////////////////// else if (us_run_every(200000, COUNTER8, loop_start_time)) { // The onboard controllers go into failsafe mode once // position data is missing handle_controller_timeouts(loop_start_time); // Send buffered data such as debug text messages // Empty one message out of the buffer debug_message_send_one(); // Toggle status led led_toggle(LED_RED); // Toggle active mode led if (global_data.state.mav_mode & MAV_MODE_FLAG_SAFETY_ARMED) { led_on(LED_GREEN); } else { led_off(LED_GREEN); } handle_eeprom_write_request(); handle_reset_request(); update_controller_parameters(); communication_send_controller_feedback(); communication_send_remote_control(); // Pressure sensor driver works, but not tested regarding stability // sensors_pressure_bmp085_read_out(); if (global_data.param[PARAM_POSITION_YAW_TRACKING] == 1) { mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 90, global_data.param[PARAM_POSITION_SETPOINT_YAW]); mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 91, global_data.yaw_pos_setpoint); } } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// NON-CRITICAL SLOW 1 Hz functions /////////////////////////////////////////////////////////////////////////// else if (us_run_every(1000000, COUNTER9, loop_start_time)) { // Send system state, mode, battery voltage, etc. send_system_state(); // Send position setpoint offset //debug_vect("pos offs", global_data.position_setpoint_offset); // Send current onboard time mavlink_msg_system_time_send(MAVLINK_COMM_1, sys_time_clock_get_unix_loop_start_time(),sys_time_clock_get_loop_start_time_boot_ms()); mavlink_msg_system_time_send(MAVLINK_COMM_0, sys_time_clock_get_unix_loop_start_time(),sys_time_clock_get_loop_start_time_boot_ms()); //update state from received parameters sync_state_parameters(); //debug number of execution count = 0; if (global_data.param[PARAM_GPS_MODE] >= 10) { //Send GPS information float_vect3 gps; gps.x = gps_utm_north / 100.0f;//m gps.y = gps_utm_east / 100.0f;//m gps.z = gps_utm_zone;// gps_week; debug_vect("GPS", gps); } else if (global_data.param[PARAM_GPS_MODE] == 9 || global_data.param[PARAM_GPS_MODE] == 8) { if (global_data.param[PARAM_GPS_MODE] == 8) { gps_set_local_origin(); // gps_local_home_init = false; } if (gps_lat == 0) { debug_message_buffer("GPS Signal Lost"); } else { float_vect3 gps_local, gps_local_velocity; gps_get_local_position(&gps_local); debug_vect("GPS local", gps_local); gps_get_local_velocity(&gps_local_velocity); debug_vect("GPS loc velocity", gps_local_velocity); } } if (global_data.state.gps_mode) { gps_send_local_origin(); } beep_on_low_voltage(); } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// NON-CRITICAL SLOW 20 Hz functions /////////////////////////////////////////////////////////////////////////// else if (us_run_every(50000, COUNTER7, loop_start_time)) { //led_toggle(LED_YELLOW); if (global_data.param[PARAM_GPS_MODE] >= 10) { //get thru all gps messages debug_message_send_one(); } communication_send_attitude_position(loop_start_time); // Send parameter communication_queued_send(); // //infrared distance // float_vect3 infra; // infra.x = global_data.ground_distance; // infra.y = global_data.ground_distance_unfiltered; // infra.z = global_data.state.ground_distance_ok; // debug_vect("infrared", infra); } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// NON-CRITICAL SLOW 200 Hz functions // /////////////////////////////////////////////////////////////////////////// else if (us_run_every(5000, COUNTER5, loop_start_time)) { if (global_data.state.status == MAV_STATE_STANDBY) { //Check if parameters should be written or read param_handler(); } } /////////////////////////////////////////////////////////////////////////// else { // All Tasks are fine and we have no starvation last_mainloop_idle = loop_start_time; } // Read out comm at max rate - takes only a few microseconds in worst case communication_receive(); // MCU load measurement uint64_t loop_stop_time = sys_time_clock_get_time_usec(); global_data.cpu_usage = measure_avg_cpu_load(loop_start_time, loop_stop_time, min_mainloop_time); global_data.cpu_peak = measure_peak_cpu_load(loop_start_time, loop_stop_time, min_mainloop_time); time_debug.y = max(time_debug.y, global_data.cpu_usage); time_debug.z = max(time_debug.z, global_data.cpu_peak); if (loop_start_time - last_mainloop_idle >= 20000) { debug_message_buffer( "CRITICAL WARNING! CPU LOAD TO HIGH. STARVATION!"); last_mainloop_idle = loop_start_time;//reset to prevent multiple messages } if (global_data.cpu_usage > 800) { // CPU load higher than 80% debug_message_buffer("CRITICAL WARNING! CPU LOAD HIGHER THAN 80%"); } } // End while(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); } } }