From 04f365dab43f0f6c45076a248824ef07940e7440 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Thu, 3 Sep 2015 17:39:16 +0200 Subject: [PATCH] changed result accumulator to handle dropped frame time correctly. this only affects the legacy data in pixels / accumulation period. --- inc/i2c.h | 3 ++- inc/result_accumulator.h | 37 ++++++++++++++++++++----------------- src/i2c.c | 6 ++++-- src/main.c | 12 ++++++++---- src/result_accumulator.c | 5 ++++- 5 files changed, 38 insertions(+), 25 deletions(-) diff --git a/inc/i2c.h b/inc/i2c.h index eeb544a..3d246c3 100644 --- a/inc/i2c.h +++ b/inc/i2c.h @@ -49,7 +49,8 @@ */ void i2c_init(void); -void update_TX_buffer(float dt, float x_rate, float y_rate, float z_rate, int16_t gyro_temp, +void update_TX_buffer(float dt, float dropped_dt, + float x_rate, float y_rate, float z_rate, int16_t gyro_temp, uint8_t qual, float pixel_flow_x, float pixel_flow_y, float rad_per_pixel, bool distance_valid, float ground_distance, uint32_t distance_age); char i2c_get_ownaddress1(void); diff --git a/inc/result_accumulator.h b/inc/result_accumulator.h index 9d86283..dd285b3 100644 --- a/inc/result_accumulator.h +++ b/inc/result_accumulator.h @@ -42,6 +42,7 @@ typedef struct _result_accumulator_ctx { struct _last { uint32_t frame_count; ///< Frame counter. float dt; ///< The time delta of this sample. + float dropped_dt; ///< The time delta of samples that have been dropped before this sample. float x_rate; ///< The current x_rate of the gyro in rad / sec. (Image/Flow coordinates) float y_rate; ///< The current y_rate of the gyro in rad / sec. (Image/Flow coordinates) float z_rate; ///< The current z_rate of the gyro in rad / sec. (Image/Flow coordinates) @@ -56,22 +57,22 @@ typedef struct _result_accumulator_ctx { float ground_distance; ///< The measured distance to the ground in meter. uint32_t distance_age; ///< Age of the distance measurement in us. } last; - uint32_t frame_count; - float px_flow_x_accu; - float px_flow_y_accu; - float rad_flow_x_accu; - float rad_flow_y_accu; - float m_flow_x_accu; - float m_flow_y_accu; - uint8_t min_quality; - uint16_t data_count; - uint16_t valid_data_count; - float valid_dist_time; - float valid_time; - float full_time; - float gyro_x_accu; - float gyro_y_accu; - float gyro_z_accu; + uint32_t frame_count; ///< Frame counter which increases with each sample that has been fed to the accumulator. + float px_flow_x_accu; ///< Pixel flow accumulator (x-direction). + float px_flow_y_accu; ///< Pixel flow accumulator (y-direction). + float rad_flow_x_accu; ///< Radiant flow accumulator (x-axis). + float rad_flow_y_accu; ///< Radiant flow accumulator (y-axis). + float m_flow_x_accu; ///< Flow in meter accumulator (x-axis). + float m_flow_y_accu; ///< Flow in meter accumulator (y-axis). + uint8_t min_quality; ///< Minimum of all non-zero quality measurements in the current accumulation period. + uint16_t data_count; ///< Number of samples of the current accumulation period. + uint16_t valid_data_count; ///< Number of samples with quality > 0. + float valid_dist_time; ///< Accumulated sample time with valid distance and quality > 0. + float valid_time; ///< Accumulated sample time with quality > 0. + float full_time; ///< Accumulated sample time including the dropped delta time. + float gyro_x_accu; ///< X-Gyro accumulator. + float gyro_y_accu; ///< Y-Gyro accumulator. + float gyro_z_accu; ///< Z-Gyro accumulator. } result_accumulator_ctx; @@ -123,6 +124,7 @@ void result_accumulator_init(result_accumulator_ctx *ctx); /** Feeds the result accumulator with new data. It will take care of handling invalid data. * @param ctx The result accumulator context to use. * @param dt The time delta of this sample. + * @param dropped_dt The time delta of samples that have been dropped before this sample. * @param x_rate The current x_rate of the gyro in rad / sec. (flow sensor coordinates) * @param y_rate The current y_rate of the gyro in rad / sec. (flow sensor coordinates) * @param z_rate The current z_rate of the gyro in rad / sec. (flow sensor coordinates) @@ -135,7 +137,8 @@ void result_accumulator_init(result_accumulator_ctx *ctx); * @param ground_distance The measured distance to the ground in meter. * @param distance_age Age of the distance measurement in us. */ -void result_accumulator_feed(result_accumulator_ctx *ctx, float dt, float x_rate, float y_rate, float z_rate, int16_t gyro_temp, +void result_accumulator_feed(result_accumulator_ctx *ctx, float dt, float dropped_dt, + float x_rate, float y_rate, float z_rate, int16_t gyro_temp, uint8_t qual, float pixel_flow_x, float pixel_flow_y, float rad_per_pixel, bool distance_valid, float ground_distance, uint32_t distance_age); diff --git a/src/i2c.c b/src/i2c.c index bc21f23..dc09985 100644 --- a/src/i2c.c +++ b/src/i2c.c @@ -222,7 +222,8 @@ void I2C1_ER_IRQHandler(void) { } } -void update_TX_buffer(float dt, float x_rate, float y_rate, float z_rate, int16_t gyro_temp, +void update_TX_buffer(float dt, float dropped_dt, + float x_rate, float y_rate, float z_rate, int16_t gyro_temp, uint8_t qual, float pixel_flow_x, float pixel_flow_y, float rad_per_pixel, bool distance_valid, float ground_distance, uint32_t distance_age) { static result_accumulator_ctx accumulator; @@ -240,7 +241,8 @@ void update_TX_buffer(float dt, float x_rate, float y_rate, float z_rate, int16_ } /* feed the accumulator and recalculate */ - result_accumulator_feed(&accumulator, dt, x_rate, y_rate, z_rate, gyro_temp, + result_accumulator_feed(&accumulator, dt, dropped_dt, + x_rate, y_rate, z_rate, gyro_temp, qual, pixel_flow_x, pixel_flow_y, rad_per_pixel, distance_valid, ground_distance, distance_age); diff --git a/src/main.c b/src/main.c index 6bec320..aa301b5 100644 --- a/src/main.c +++ b/src/main.c @@ -292,6 +292,8 @@ int main(void) uint32_t last_frame_index = 0; + uint32_t last_processed_frame_timestamp = get_boot_time_us(); + /* main loop */ while (1) { @@ -393,8 +395,10 @@ int main(void) } } - float frame_dt = (frames[0]->timestamp - frames[1]->timestamp) * 0.000001f; - + float frame_dt = calculate_time_delta_us(frames[0]->timestamp, frames[1]->timestamp) * 0.000001f; + float dropped_dt = calculate_time_delta_us(frames[1]->timestamp, last_processed_frame_timestamp) * 0.000001f; + last_processed_frame_timestamp = frames[0]->timestamp; + /* compute gyro rate in pixels and change to image coordinates */ float x_rate_px = - y_rate * (focal_length_px * frame_dt); float y_rate_px = x_rate * (focal_length_px * frame_dt); @@ -459,13 +463,13 @@ int main(void) } /* update I2C transmit buffer */ - update_TX_buffer(frame_dt, + update_TX_buffer(frame_dt, dropped_dt, x_rate, y_rate, z_rate, gyro_temp, qual, pixel_flow_x, pixel_flow_y, 1.0f / focal_length_px, distance_valid, ground_distance, get_time_delta_us(get_sonar_measure_time())); /* accumulate the results */ - result_accumulator_feed(&mavlink_accumulator, frame_dt, + result_accumulator_feed(&mavlink_accumulator, frame_dt, dropped_dt, x_rate, y_rate, z_rate, gyro_temp, qual, pixel_flow_x, pixel_flow_y, 1.0f / focal_length_px, distance_valid, ground_distance, get_time_delta_us(get_sonar_measure_time())); diff --git a/src/result_accumulator.c b/src/result_accumulator.c index 36c5d6b..395f1a0 100644 --- a/src/result_accumulator.c +++ b/src/result_accumulator.c @@ -49,12 +49,14 @@ void result_accumulator_init(result_accumulator_ctx *ctx) ctx->last.ground_distance = -1; } -void result_accumulator_feed(result_accumulator_ctx *ctx, float dt, float x_rate, float y_rate, float z_rate, int16_t gyro_temp, +void result_accumulator_feed(result_accumulator_ctx *ctx, float dt, float dropped_dt, + float x_rate, float y_rate, float z_rate, int16_t gyro_temp, uint8_t qual, float pixel_flow_x, float pixel_flow_y, float rad_per_pixel, bool distance_valid, float ground_distance, uint32_t distance_age) { /* update last value in struct */ ctx->last.dt = dt; + ctx->last.dropped_dt = dropped_dt; ctx->last.x_rate = x_rate; ctx->last.y_rate = y_rate; ctx->last.z_rate = z_rate; @@ -98,6 +100,7 @@ void result_accumulator_feed(result_accumulator_ctx *ctx, float dt, float x_rate ctx->data_count++; ctx->frame_count++; ctx->full_time += dt; + ctx->full_time += ctx->last.dropped_dt; ctx->last.frame_count = ctx->frame_count; }