/** * Update the optical flow state for the calculation thread * and update the stabilization loops with the newest result */ void opticflow_module_run(void) { // Send Updated data to thread pthread_mutex_lock(&opticflow_mutex); opticflow_state.phi = stateGetNedToBodyEulers_f()->phi; opticflow_state.theta = stateGetNedToBodyEulers_f()->theta; // Update the stabilization loops on the current calculation if (opticflow_got_result) { uint32_t now_ts = get_sys_time_usec(); uint8_t quality = opticflow_result.divergence; // FIXME, scale to some quality measure 0-255 AbiSendMsgOPTICAL_FLOW(OPTICFLOW_SENDER_ID, now_ts, opticflow_result.flow_x, opticflow_result.flow_y, opticflow_result.flow_der_x, opticflow_result.flow_der_y, quality, opticflow_result.div_size, opticflow_state.agl); //TODO Find an appropiate quality measure for the noise model in the state filter, for now it is tracked_cnt if (opticflow_result.tracked_cnt > 0) { AbiSendMsgVELOCITY_ESTIMATE(OPTICFLOW_SENDER_ID, now_ts, opticflow_result.vel_body_x, opticflow_result.vel_body_y, 0.0f, opticflow_result.noise_measurement ); } opticflow_got_result = false; } pthread_mutex_unlock(&opticflow_mutex); }
/** * Update the optical flow state for the calculation thread * and update the stabilization loops with the newest result */ void opticflow_module_run(void) { pthread_mutex_lock(&opticflow_mutex); // Update the stabilization loops on the current calculation if (opticflow_got_result) { uint32_t now_ts = get_sys_time_usec(); AbiSendMsgOPTICAL_FLOW(OPTICFLOW_SENDER_ID, now_ts, opticflow_result.flow_x, opticflow_result.flow_y, opticflow_result.flow_der_x, opticflow_result.flow_der_y, opticflow_result.noise_measurement,// FIXME, scale to some quality measure 0-255 opticflow_result.div_size, opticflow_state.agl); //TODO Find an appropiate quality measure for the noise model in the state filter, for now it is tracked_cnt if (opticflow_result.noise_measurement < 0.8) { AbiSendMsgVELOCITY_ESTIMATE(OPTICFLOW_SENDER_ID, now_ts, opticflow_result.vel_body_x, opticflow_result.vel_body_y, 0.0f, opticflow_result.noise_measurement ); } opticflow_got_result = false; } pthread_mutex_unlock(&opticflow_mutex); }
/** * Propagate optical flow information */ static inline void px4flow_i2c_frame_cb(void) { static float quality = 0; static float noise = 0; uint32_t now_ts = get_sys_time_usec(); quality = ((float)px4flow.i2c_frame.qual) / 255.0; noise = px4flow.stddev + (1 - quality) * px4flow.stddev * 10; noise = noise * noise; // square the noise to get variance of the measurement static float timestamp = 0; static uint32_t time_usec = 0; static float flow_comp_m_x = 0.0; static float flow_comp_m_y = 0.0; if (quality > PX4FLOW_QUALITY_THRESHOLD) { timestamp = get_sys_time_float(); time_usec = (uint32_t)(timestamp * 1e6); flow_comp_m_x = ((float)px4flow.i2c_frame.flow_comp_m_x) / 1000.0; flow_comp_m_y = ((float)px4flow.i2c_frame.flow_comp_m_y) / 1000.0; // flip the axis (if the PX4FLOW is mounted as shown in // https://pixhawk.org/modules/px4flow AbiSendMsgVELOCITY_ESTIMATE(VEL_PX4FLOW_ID, time_usec, flow_comp_m_y, flow_comp_m_x, 0.0f, noise, noise, -1.f); } // distance is always positive - use median filter to remove outliers static int32_t ground_distance = 0; static float ground_distance_float = 0.0; // update filter ground_distance = update_median_filter_i(&sonar_filter, (int32_t)px4flow.i2c_frame.ground_distance); ground_distance_float = ((float)ground_distance) / 1000.0; // compensate AGL measurement for body rotation if (px4flow.compensate_rotation) { float phi = stateGetNedToBodyEulers_f()->phi; float theta = stateGetNedToBodyEulers_f()->theta; float gain = (float)fabs( (double) (cosf(phi) * cosf(theta))); ground_distance_float = ground_distance_float / gain; } if (px4flow.update_agl) { AbiSendMsgAGL(AGL_SONAR_PX4FLOW_ID, now_ts, ground_distance_float); } }