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