Skip to content

Commit

Permalink
Changes for EKF integration testing
Browse files Browse the repository at this point in the history 
1) Motion compensation is not applied
2) Output rate is changed to 10Hz
3) Output data has been repurposed so that what is output on the raw flow message is the gyro angular rate in mrad/sec about the X and Y sensor axis
4) Output data has been repurposed so that what is output on the velocity message is the optical flow angular rate in rad/sec which is equivalent to velocity / range
5) Gyro angle rate, optical flow angle rate and quality measurements are all averaged using measurements summed when valid flow readings were available.
  • Loading branch information
@priseborough
priseborough committed Aug 16, 2014
1 parent 398e4e3 commit 8d4e185
Show file tree
Hide file tree
Showing 2 changed files with 46 additions and 71 deletions.
2 changes: 1 addition & 1 deletion src/flow.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -671,7 +671,7 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat
* -y_rate gives x flow
* x_rates gives y_flow
*/
if (global_data.param[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION])
if (false)
{
if(fabsf(y_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD])
{
Expand Down
115 changes: 45 additions & 70 deletions src/main.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -243,18 +243,22 @@ int main(void)
/* variables */
uint32_t counter = 0;
uint8_t qual = 0;
uint16_t qualSum = 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 angVel_x_sum = 0.0f;
float angVel_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;
// variables used to average angular rate
float x_rate_sum = 0.0f;
float y_rate_sum = 0.0f;

/* main loop */
while (1)
Expand Down Expand Up @@ -317,17 +321,9 @@ int main(void)
float y_rate = - x_rate_sensor;
float z_rate = z_rate_sensor; // z is correct

/* calculate focal_length in pixel */
/* 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);

Expand Down Expand Up @@ -356,45 +352,25 @@ int main(void)
* 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++;
/* calc angular velocity (negative of flow values scaled with distance) */
float new_angVel_x = - pixel_flow_x / focal_length_px / (get_time_between_images() / 1000.0f);
float new_angVel_y = - pixel_flow_y / focal_length_px / (get_time_between_images() / 1000.0f);

if (qual > 0)
{
angVel_x_sum += new_angVel_x;
angVel_y_sum += new_angVel_y;
qualSum += qual;
valid_frame_count++;

x_rate_sum += x_rate;
y_rate_sum += y_rate;
/* lowpass velocity output */
velocity_x_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * new_angVel_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_angVel_y +
(1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp;
}

}

Expand All @@ -406,21 +382,13 @@ int main(void)
if(global_data.param[PARAM_SENSOR_POSITION] == BOTTOM)
{
/* send bottom flow if activated */
if (counter % 2 == 0)
if (counter % 40 == 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;
}
flow_comp_m_x = angVel_x_sum/valid_frame_count;
flow_comp_m_y = angVel_y_sum/valid_frame_count;

if(global_data.param[PARAM_SONAR_FILTERED])
ground_distance = sonar_distance_filtered;
Expand All @@ -429,33 +397,37 @@ int main(void)

if (valid_frame_count > 0)
{
float x_rate_avg = 1000.0f * x_rate_sum / valid_frame_count; // mrad/sec
float y_rate_avg = 1000.0f * y_rate_sum / valid_frame_count; // mrad/sec
qual = qualSum / valid_frame_count;

// 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,
x_rate_avg, y_rate_avg,
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,
x_rate_avg, y_rate_avg,
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,
update_TX_buffer(x_rate_avg, y_rate_avg, 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, 0,
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, 0,
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,
update_TX_buffer(0, 0, 0.0f, 0.0f, 0, ground_distance, x_rate, y_rate,
z_rate);
}

Expand All @@ -464,12 +436,15 @@ int main(void)
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;
angVel_x_sum = 0.0f;
angVel_y_sum = 0.0f;
pixel_flow_x_sum = 0.0f;
pixel_flow_y_sum = 0.0f;
valid_frame_count = 0;
x_rate_sum = 0.0f;
y_rate_sum = 0.0f;
valid_frame_count = 0;
pixel_flow_count = 0;
qualSum = 0;
}
}

Expand Down

0 comments on commit 8d4e185

Please sign in to comment.