/** * 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); }