void ins_update_sonar() { static float last_offset = 0.; // new value filtered with median_filter ins_sonar_alt = update_median_filter(&sonar_median, sonar_meas); float sonar = (ins_sonar_alt - ins_sonar_offset) * INS_SONAR_SENS; #ifdef INS_SONAR_VARIANCE_THRESHOLD /* compute variance of error between sonar and baro alt */ int32_t err = POS_BFP_OF_REAL(sonar) + ins_baro_alt; // sonar positive up, baro positive down !!!! var_err[var_idx] = POS_FLOAT_OF_BFP(err); var_idx = (var_idx + 1) % VAR_ERR_MAX; float var = variance_float(var_err, VAR_ERR_MAX); DOWNLINK_SEND_INS_SONAR(DefaultChannel,DefaultDevice,&err, &sonar, &var); //DOWNLINK_SEND_INS_SONAR(DefaultChannel,DefaultDevice,&ins_sonar_alt, &sonar, &var); #endif /* update filter assuming a flat ground */ if (sonar < INS_SONAR_MAX_RANGE #ifdef INS_SONAR_MIN_RANGE && sonar > INS_SONAR_MIN_RANGE #endif #ifdef INS_SONAR_THROTTLE_THRESHOLD && stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD #endif #ifdef INS_SONAR_STAB_THRESHOLD && stabilization_cmd[COMMAND_ROLL] < INS_SONAR_STAB_THRESHOLD && stabilization_cmd[COMMAND_ROLL] > -INS_SONAR_STAB_THRESHOLD && stabilization_cmd[COMMAND_PITCH] < INS_SONAR_STAB_THRESHOLD && stabilization_cmd[COMMAND_PITCH] > -INS_SONAR_STAB_THRESHOLD && stabilization_cmd[COMMAND_YAW] < INS_SONAR_STAB_THRESHOLD && stabilization_cmd[COMMAND_YAW] > -INS_SONAR_STAB_THRESHOLD #endif #ifdef INS_SONAR_BARO_THRESHOLD && ins_baro_alt > -POS_BFP_OF_REAL(INS_SONAR_BARO_THRESHOLD) /* z down */ #endif #ifdef INS_SONAR_VARIANCE_THRESHOLD && var < INS_SONAR_VARIANCE_THRESHOLD #endif && ins_update_on_agl && baro.status == BS_RUNNING) { vff_update_alt_conf(-sonar, VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabs(sonar)); last_offset = vff_offset; } else { /* update offset with last value to avoid divergence */ vff_update_offset(last_offset); } }
/** * Poll lidar for data * run at max 50Hz */ void lidar_lite_periodic(void) { switch (lidar.status) { case LIDAR_INIT_RANGING: // ask for i2c frame lidar.trans.buf[0] = LIDAR_LITE_REG_ADDR; // sets register pointer to (0x00) lidar.trans.buf[1] = LIDAR_LITE_REG_VAL; // take acquisition & correlation processing with DC correction if (i2c_transmit(&LIDAR_LITE_I2C_DEV, &lidar.trans, lidar.addr, 2)) { // transaction OK, increment status lidar.status = LIDAR_REQ_READ; } break; case LIDAR_REQ_READ: lidar.trans.buf[0] = LIDAR_LITE_READ_ADDR; // sets register pointer to results register lidar.trans.buf[1] = 0; if (i2c_transceive(&LIDAR_LITE_I2C_DEV, &lidar.trans, lidar.addr, 1,2)) { // transaction OK, increment status lidar.status = LIDAR_READ_DISTANCE; } break; case LIDAR_READ_DISTANCE: // filter data /* lidar.distance_raw = (uint16_t)((lidar.trans.buf[0] << 8) + lidar.trans.buf[1]); lidar.distance = update_median_filter(&lidar_filter, (int32_t)lidar.distance_raw); */ //lidar.distance_raw = (uint32_t)((lidar.trans.buf[0] << 8) | lidar.trans.buf[1]); lidar.distance_raw = update_median_filter(&lidar_filter, (uint32_t)((lidar.trans.buf[0] << 8) | lidar.trans.buf[1])); lidar.distance = ((float)lidar.distance_raw)/100.0; // send message (if requested) if (lidar.update_agl) { AbiSendMsgAGL(AGL_LIDAR_LITE_ID, lidar.distance); } // increment status lidar.status = LIDAR_INIT_RANGING; break; default: break; } }
void ins_update_baro() { int32_t baro_pressure = update_median_filter(&baro_median, baro.absolute); if (baro.status == BS_RUNNING) { if (!ins_baro_initialised) { ins_qfe = baro_pressure; ins_baro_initialised = TRUE; } if (ins.vf_realign) { ins.vf_realign = FALSE; ins_qfe = baro_pressure; vff_realign(0.); ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z); } else { /* not realigning, so normal update with baro measurement */ ins_baro_alt = ((baro_pressure - ins_qfe) * INS_BARO_SENS_NUM)/INS_BARO_SENS_DEN; float alt_float = POS_FLOAT_OF_BFP(ins_baro_alt); vff_update_baro(alt_float); } } }
/** * Poll lidar for data */ void lidar_sf11_periodic(void) { switch (lidar_sf11.status) { case LIDAR_SF11_REQ_READ: // read two bytes lidar_sf11.trans.buf[0] = 0; // set tx to zero i2c_transceive(&LIDAR_SF11_I2C_DEV, &lidar_sf11.trans, lidar_sf11.addr, 1, 2); break; case LIDAR_SF11_READ_OK: // process results // filter data lidar_sf11.distance_raw = update_median_filter( &lidar_sf11_filter, (uint32_t)((lidar_sf11.trans.buf[0] << 8) | lidar_sf11.trans.buf[1])); lidar_sf11.distance = ((float)lidar_sf11.distance_raw)/100.0; // compensate AGL measurement for body rotation if (lidar_sf11.compensate_rotation) { float phi = stateGetNedToBodyEulers_f()->phi; float theta = stateGetNedToBodyEulers_f()->theta; float gain = (float)fabs( (double) (cosf(phi) * cosf(theta))); lidar_sf11.distance = lidar_sf11.distance / gain; } // send message (if requested) if (lidar_sf11.update_agl) { AbiSendMsgAGL(AGL_LIDAR_SF11_ID, lidar_sf11.distance); } // reset status lidar_sf11.status = LIDAR_SF11_REQ_READ; break; default: break; } }
/** * Run the optical flow with EDGEFLOW on a new image frame * @param[in] *opticflow The opticalflow structure that keeps track of previous images * @param[in] *state The state of the drone * @param[in] *img The image frame to calculate the optical flow from * @param[out] *result The optical flow result */ void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img, struct opticflow_result_t *result) { // Define Static Variables static struct edge_hist_t edge_hist[MAX_HORIZON]; static uint8_t current_frame_nr = 0; struct edge_flow_t edgeflow; static uint8_t previous_frame_offset[2] = {1, 1}; // Define Normal variables struct edgeflow_displacement_t displacement; displacement.x = malloc(sizeof(int32_t) * img->w); displacement.y = malloc(sizeof(int32_t) * img->h); // If the methods just switched to this one, reintialize the // array of edge_hist structure. if (opticflow->just_switched_method == 1) { int i; for (i = 0; i < MAX_HORIZON; i++) { edge_hist[i].x = malloc(sizeof(int32_t) * img->w); edge_hist[i].y = malloc(sizeof(int32_t) * img->h); FLOAT_RATES_ZERO(edge_hist[i].rates); } } uint16_t disp_range; if (opticflow->search_distance < DISP_RANGE_MAX) { disp_range = opticflow->search_distance; } else { disp_range = DISP_RANGE_MAX; } uint16_t window_size; if (opticflow->window_size < MAX_WINDOW_SIZE) { window_size = opticflow->window_size; } else { window_size = MAX_WINDOW_SIZE; } uint16_t RES = opticflow->subpixel_factor; //......................Calculating EdgeFlow..................... // // Calculate current frame's edge histogram int32_t *edge_hist_x = edge_hist[current_frame_nr].x; int32_t *edge_hist_y = edge_hist[current_frame_nr].y; calculate_edge_histogram(img, edge_hist_x, 'x', 0); calculate_edge_histogram(img, edge_hist_y, 'y', 0); // Copy frame time and angles of image to calculated edge histogram edge_hist[current_frame_nr].frame_time = img->ts; edge_hist[current_frame_nr].rates = state->rates; // Calculate which previous edge_hist to compare with the current uint8_t previous_frame_nr[2]; calc_previous_frame_nr(result, opticflow, current_frame_nr, previous_frame_offset, previous_frame_nr); //Select edge histogram from the previous frame nr int32_t *prev_edge_histogram_x = edge_hist[previous_frame_nr[0]].x; int32_t *prev_edge_histogram_y = edge_hist[previous_frame_nr[1]].y; //Calculate the corresponding derotation of the two frames int16_t der_shift_x = 0; int16_t der_shift_y = 0; if (opticflow->derotation) { der_shift_x = (int16_t)(edge_hist[current_frame_nr].rates.p / result->fps * (float)img->w / (OPTICFLOW_FOV_W) * opticflow->derotation_correction_factor_x); der_shift_y = (int16_t)(edge_hist[current_frame_nr].rates.q / result->fps * (float)img->h / (OPTICFLOW_FOV_H) * opticflow->derotation_correction_factor_y); } // Estimate pixel wise displacement of the edge histograms for x and y direction calculate_edge_displacement(edge_hist_x, prev_edge_histogram_x, displacement.x, img->w, window_size, disp_range, der_shift_x); calculate_edge_displacement(edge_hist_y, prev_edge_histogram_y, displacement.y, img->h, window_size, disp_range, der_shift_y); // Fit a line on the pixel displacement to estimate // the global pixel flow and divergence (RES is resolution) line_fit(displacement.x, &edgeflow.div_x, &edgeflow.flow_x, img->w, window_size + disp_range, RES); line_fit(displacement.y, &edgeflow.div_y, &edgeflow.flow_y, img->h, window_size + disp_range, RES); /* Save Resulting flow in results * Warning: The flow detected here is different in sign * and size, therefore this will be multiplied with * the same subpixel factor and -1 to make it on par with * the LK algorithm of t opticalflow_calculator.c * */ edgeflow.flow_x = -1 * edgeflow.flow_x; edgeflow.flow_y = -1 * edgeflow.flow_y; result->flow_x = (int16_t)edgeflow.flow_x / previous_frame_offset[0]; result->flow_y = (int16_t)edgeflow.flow_y / previous_frame_offset[1]; //Fill up the results optic flow to be on par with LK_fast9 result->flow_der_x = result->flow_x; result->flow_der_y = result->flow_y; result->corner_cnt = getAmountPeaks(edge_hist_x, 500 , img->w); result->tracked_cnt = getAmountPeaks(edge_hist_x, 500 , img->w); result->divergence = (float)edgeflow.flow_x / RES; result->div_size = 0.0f; result->noise_measurement = 0.0f; result->surface_roughness = 0.0f; //......................Calculating VELOCITY ..................... // /*Estimate fps per direction * This is the fps with adaptive horizon for subpixel flow, which is not similar * to the loop speed of the algorithm. The faster the quadcopter flies * the higher it becomes */ float fps_x = 0; float fps_y = 0; float time_diff_x = (float)(timeval_diff(&edge_hist[previous_frame_nr[0]].frame_time, &img->ts)) / 1000.; float time_diff_y = (float)(timeval_diff(&edge_hist[previous_frame_nr[1]].frame_time, &img->ts)) / 1000.; fps_x = 1 / (time_diff_x); fps_y = 1 / (time_diff_y); result->fps = fps_x; // Calculate velocity float vel_x = edgeflow.flow_x * fps_x * state->agl * OPTICFLOW_FOV_W / (img->w * RES); float vel_y = edgeflow.flow_y * fps_y * state->agl * OPTICFLOW_FOV_H / (img->h * RES); //Apply a median filter to the velocity if wanted if (opticflow->median_filter == true) { result->vel_x = (float)update_median_filter(&vel_x_filt, (int32_t)(vel_x * 1000)) / 1000; result->vel_y = (float)update_median_filter(&vel_y_filt, (int32_t)(vel_y * 1000)) / 1000; } else { result->vel_x = vel_x; result->vel_y = vel_y; } result->noise_measurement = 0.2; #if OPTICFLOW_SHOW_FLOW draw_edgeflow_img(img, edgeflow, prev_edge_histogram_x, edge_hist_x); #endif // Increment and wrap current time frame current_frame_nr = (current_frame_nr + 1) % MAX_HORIZON; }
/** * Run the optical flow with fast9 and lukaskanade on a new image frame * @param[in] *opticflow The opticalflow structure that keeps track of previous images * @param[in] *state The state of the drone * @param[in] *img The image frame to calculate the optical flow from * @param[out] *result The optical flow result */ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img, struct opticflow_result_t *result) { if (opticflow->just_switched_method) { opticflow_calc_init(opticflow, img->w, img->h); } // variables for size_divergence: float size_divergence; int n_samples; // variables for linear flow fit: float error_threshold; int n_iterations_RANSAC, n_samples_RANSAC, success_fit; struct linear_flow_fit_info fit_info; // Update FPS for information result->fps = 1 / (timeval_diff(&opticflow->prev_timestamp, &img->ts) / 1000.); opticflow->prev_timestamp = img->ts; // Convert image to grayscale image_to_grayscale(img, &opticflow->img_gray); // Copy to previous image if not set if (!opticflow->got_first_img) { image_copy(&opticflow->img_gray, &opticflow->prev_img_gray); opticflow->got_first_img = true; } // ************************************************************************************* // Corner detection // ************************************************************************************* // FAST corner detection // TODO: There is something wrong with fast9_detect destabilizing FPS. This problem is reduced with putting min_distance // to 0 (see defines), however a more permanent solution should be considered fast9_detect(img, opticflow->fast9_threshold, opticflow->fast9_min_distance, opticflow->fast9_padding, opticflow->fast9_padding, &result->corner_cnt, &opticflow->fast9_rsize, opticflow->fast9_ret_corners); // Adaptive threshold if (opticflow->fast9_adaptive) { // Decrease and increase the threshold based on previous values if (result->corner_cnt < 40 && opticflow->fast9_threshold > FAST9_LOW_THRESHOLD) { // TODO: Replace 40 with OPTICFLOW_MAX_TRACK_CORNERS / 2 opticflow->fast9_threshold--; } else if (result->corner_cnt > OPTICFLOW_MAX_TRACK_CORNERS * 2 && opticflow->fast9_threshold < FAST9_HIGH_THRESHOLD) { opticflow->fast9_threshold++; } } #if OPTICFLOW_SHOW_CORNERS image_show_points(img, opticflow->fast9_ret_corners, result->corner_cnt); #endif // Check if we found some corners to track if (result->corner_cnt < 1) { image_copy(&opticflow->img_gray, &opticflow->prev_img_gray); return; } // ************************************************************************************* // Corner Tracking // ************************************************************************************* // Execute a Lucas Kanade optical flow result->tracked_cnt = result->corner_cnt; struct flow_t *vectors = opticFlowLK(&opticflow->img_gray, &opticflow->prev_img_gray, opticflow->fast9_ret_corners, &result->tracked_cnt, opticflow->window_size / 2, opticflow->subpixel_factor, opticflow->max_iterations, opticflow->threshold_vec, opticflow->max_track_corners, opticflow->pyramid_level); #if OPTICFLOW_SHOW_FLOW printf("show: n tracked = %d\n", result->tracked_cnt); image_show_flow(img, vectors, result->tracked_cnt, opticflow->subpixel_factor); #endif // Estimate size divergence: if (SIZE_DIV) { n_samples = 100; size_divergence = get_size_divergence(vectors, result->tracked_cnt, n_samples); result->div_size = size_divergence; } else { result->div_size = 0.0f; } if (LINEAR_FIT) { // Linear flow fit (normally derotation should be performed before): error_threshold = 10.0f; n_iterations_RANSAC = 20; n_samples_RANSAC = 5; success_fit = analyze_linear_flow_field(vectors, result->tracked_cnt, error_threshold, n_iterations_RANSAC, n_samples_RANSAC, img->w, img->h, &fit_info); if (!success_fit) { fit_info.divergence = 0.0f; fit_info.surface_roughness = 0.0f; } result->divergence = fit_info.divergence; result->surface_roughness = fit_info.surface_roughness; } else { result->divergence = 0.0f; result->surface_roughness = 0.0f; } // Get the median flow qsort(vectors, result->tracked_cnt, sizeof(struct flow_t), cmp_flow); if (result->tracked_cnt == 0) { // We got no flow result->flow_x = 0; result->flow_y = 0; } else if (result->tracked_cnt > 3) { // Take the average of the 3 median points result->flow_x = vectors[result->tracked_cnt / 2 - 1].flow_x; result->flow_y = vectors[result->tracked_cnt / 2 - 1].flow_y; result->flow_x += vectors[result->tracked_cnt / 2].flow_x; result->flow_y += vectors[result->tracked_cnt / 2].flow_y; result->flow_x += vectors[result->tracked_cnt / 2 + 1].flow_x; result->flow_y += vectors[result->tracked_cnt / 2 + 1].flow_y; result->flow_x /= 3; result->flow_y /= 3; } else { // Take the median point result->flow_x = vectors[result->tracked_cnt / 2].flow_x; result->flow_y = vectors[result->tracked_cnt / 2].flow_y; } // Flow Derotation float diff_flow_x = 0; float diff_flow_y = 0; /*// Flow Derotation TODO: float diff_flow_x = (state->phi - opticflow->prev_phi) * img->w / OPTICFLOW_FOV_W; float diff_flow_y = (state->theta - opticflow->prev_theta) * img->h / OPTICFLOW_FOV_H;*/ if (opticflow->derotation && result->tracked_cnt > 5) { diff_flow_x = (state->rates.p) / result->fps * img->w / OPTICFLOW_FOV_W;// * img->w / OPTICFLOW_FOV_W; diff_flow_y = (state->rates.q) / result->fps * img->h / OPTICFLOW_FOV_H;// * img->h / OPTICFLOW_FOV_H; } result->flow_der_x = result->flow_x - diff_flow_x * opticflow->subpixel_factor * opticflow->derotation_correction_factor_x; result->flow_der_y = result->flow_y - diff_flow_y * opticflow->subpixel_factor * opticflow->derotation_correction_factor_y; opticflow->prev_rates = state->rates; // Velocity calculation // Right now this formula is under assumption that the flow only exist in the center axis of the camera. // TODO Calculate the velocity more sophisticated, taking into account the drone's angle and the slope of the ground plane. float vel_x = result->flow_der_x * result->fps * state->agl / opticflow->subpixel_factor / OPTICFLOW_FX; float vel_y = result->flow_der_y * result->fps * state->agl / opticflow->subpixel_factor / OPTICFLOW_FY; //Apply a median filter to the velocity if wanted if (opticflow->median_filter == true) { result->vel_x = (float)update_median_filter(&vel_x_filt, (int32_t)(vel_x * 1000)) / 1000; result->vel_y = (float)update_median_filter(&vel_y_filt, (int32_t)(vel_y * 1000)) / 1000; } else { result->vel_x = vel_x; result->vel_y = vel_y; } // Velocity calculation: uncomment if focal length of the camera is not known or incorrect. // result->vel_x = - result->flow_der_x * result->fps * state->agl / opticflow->subpixel_factor * OPTICFLOW_FOV_W / img->w // result->vel_y = result->flow_der_y * result->fps * state->agl / opticflow->subpixel_factor * OPTICFLOW_FOV_H / img->h // Determine quality of noise measurement for state filter //TODO develop a noise model based on groundtruth float noise_measurement_temp = (1 - ((float)result->tracked_cnt / ((float)opticflow->max_track_corners * 1.25))); result->noise_measurement = noise_measurement_temp; // ************************************************************************************* // Next Loop Preparation // ************************************************************************************* free(vectors); image_switch(&opticflow->img_gray, &opticflow->prev_img_gray); }