From b7faf41c7d9de8e9ab28a8f69ea87b13f6a62bc1 Mon Sep 17 00:00:00 2001 From: pt Date: Fri, 25 Jul 2014 18:11:24 +0200 Subject: [PATCH 001/120] implementation of klt based flow on one pyramid level (+/- 1 pixel flow), this implementation needs to be extended with image pyramid with ~3 levels and reimplemented with special instructions and fixed point for bilinear filtering --- inc/flow.h | 6 ++ src/flow.c | 193 +++++++++++++++++++++++++++++++++++++++++++++++++++-- src/main.c | 1 + 3 files changed, 196 insertions(+), 4 deletions(-) diff --git a/inc/flow.h b/inc/flow.h index af5989b..d182fdd 100644 --- a/inc/flow.h +++ b/inc/flow.h @@ -42,4 +42,10 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate, float *histflowx, float *histflowy); +/** + * @brief Computes pixel flow from image1 to image2 with KLT method + */ +uint8_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate, + float *pixel_flow_x, float *pixel_flow_y); + #endif /* FLOW_H_ */ diff --git a/src/flow.c b/src/flow.c index 78c0611..2f56a2c 100644 --- a/src/flow.c +++ b/src/flow.c @@ -48,11 +48,16 @@ #define __ASM asm #include "core_cm4_simd.h" -#define FRAME_SIZE global_data.param[PARAM_IMAGE_WIDTH] -#define SEARCH_SIZE global_data.param[PARAM_MAX_FLOW_PIXEL] // maximum offset to search: 4 + 1/2 pixels +#define FRAME_SIZE BOTTOM_FLOW_IMAGE_WIDTH +#define SEARCH_SIZE BOTTOM_FLOW_SEARCH_WINDOW_SIZE // maximum offset to search: 4 + 1/2 pixels #define TILE_SIZE 8 // x & y tile size #define NUM_BLOCKS 5 // x & y number of tiles to check +#define HALF_PATCH_SIZE (SEARCH_SIZE+1) //this is half the wanted patch size minus 1 +#define PATCH_SIZE (HALF_PATCH_SIZE*2+1) +float Jx[PATCH_SIZE*PATCH_SIZE]; +float Jy[PATCH_SIZE*PATCH_SIZE]; + #define sign(x) (( x > 0 ) - ( x < 0 )) // compliments of Adam Williams @@ -402,8 +407,8 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat /* variables */ uint16_t pixLo = SEARCH_SIZE + 1; - uint16_t pixHi = FRAME_SIZE - (SEARCH_SIZE + 1) - TILE_SIZE; - uint16_t pixStep = (pixHi - pixLo) / NUM_BLOCKS + 1; + uint16_t pixHi = FRAME_SIZE - (SEARCH_SIZE + 1); + uint16_t pixStep = (pixHi - pixLo) / (NUM_BLOCKS-1); uint16_t i, j; uint32_t acc[8]; // subpixels uint16_t histx[hist_size]; // counter for x shift @@ -417,6 +422,8 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat float histflowx = 0.0f; float histflowy = 0.0f; + int xxx = 0; + /* initialize with 0 */ for (j = 0; j < hist_size; j++) { histx[j] = 0; histy[j] = 0; } @@ -426,6 +433,7 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat { for (i = pixLo; i < pixHi; i += pixStep) { + xxx++; /* test pixel if it is suitable for flow tracking */ uint32_t diff = compute_diff(image1, i, j, (uint16_t) global_data.param[PARAM_IMAGE_WIDTH]); if (diff < global_data.param[PARAM_BOTTOM_FLOW_FEATURE_THRESHOLD]) @@ -745,3 +753,180 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat return qual; } + + +/** + * @brief Computes pixel flow from image1 to image2 + * + * Searches the corresponding position in the new image (image2) of max. 64 pixels from the old image (image1) + * with the KLT method (one level = 1 pixel search range!) and outputs the average value of all flow vectors + * + * @param image1 previous image buffer + * @param image2 current image buffer (new) + * @param x_rate gyro x rate + * @param y_rate gyro y rate + * @param z_rate gyro z rate + * + * @return quality of flow calculation + */ +uint8_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate, float *pixel_flow_x, float *pixel_flow_y) +{ + /* variables */ + uint16_t i, j; + + //better distribution of sampling points: + uint16_t pixLo = FRAME_SIZE / (NUM_BLOCKS + 1); + if (pixLo < PATCH_SIZE) pixLo = PATCH_SIZE; + uint16_t pixHi = (uint16_t)FRAME_SIZE * NUM_BLOCKS / (NUM_BLOCKS + 1); + if (pixHi > (FRAME_SIZE - PATCH_SIZE)) pixHi = FRAME_SIZE - PATCH_SIZE; + uint16_t pixStep = (pixHi - pixLo) / (NUM_BLOCKS-1); + + float meanflowx = 0.0f; + float meanflowy = 0.0f; + uint16_t meancount = 0; + + /* iterate over all patterns + */ + for (j = pixLo; j < pixHi; j += pixStep) + { + for (i = pixLo; i < pixHi; i += pixStep) + { + uint16_t iwidth = global_data.param[PARAM_IMAGE_WIDTH]; + uint8_t *base1 = image1 + j * iwidth + i; + + float JTJ[4]; //the 2x2 Hessian + JTJ[0] = 0; + JTJ[1] = 0; + JTJ[2] = 0; + JTJ[3] = 0; + int c = 0; + + //compute jacobians and the hessian for the patch at the current location + for (int8_t jj = -HALF_PATCH_SIZE; jj <= HALF_PATCH_SIZE; jj++) + { + uint8_t *left = base1 + jj*iwidth; + for (int8_t ii = -HALF_PATCH_SIZE; ii <= HALF_PATCH_SIZE; ii++) + { + const float jx = ((uint16_t)left[ii+1] - (uint16_t)left[ii-1]) * 0.5f; + const float jy = ((uint16_t)left[ii+iwidth] - (uint16_t)left[ii-iwidth]) * 0.5f; + Jx[c] = jx; + Jy[c] = jy; + JTJ[0] += jx*jx; + JTJ[1] += jx*jy; + JTJ[2] += jx*jy; + JTJ[3] += jy*jy; + c++; + } + } + + //assume that we did not move + float u = i; + float v = j; + + float chi_sq_previous = 0.f; + + //Now do some Gauss-Newton iterations for flow + for (int iters = 0; iters < 5; iters++) + { + float JTe_x = 0; //accumulators for Jac transposed times error + float JTe_y = 0; + + uint8_t *base2 = image2 + (uint16_t)v * iwidth + (uint16_t)u; + + //extract bilinearly filtered pixel values for the current location in image2 + float dX = u - floorf(u); + float dY = v - floorf(v); + float fMixTL = (1.f - dX) * (1.f - dY); + float fMixTR = (dX) * (1.f - dY); + float fMixBL = (1.f - dX) * (dY); + float fMixBR = (dX) * (dY); + + float chi_sq = 0.f; + int c = 0; + for (int8_t jj = -HALF_PATCH_SIZE; jj <= HALF_PATCH_SIZE; jj++) + { + uint8_t *left1 = base1 + jj*iwidth; + uint8_t *left2 = base2 + jj*iwidth; + + for (int8_t ii = -HALF_PATCH_SIZE; ii <= HALF_PATCH_SIZE; ii++) + { + float fPixel = fMixTL * left2[ii] + fMixTR * left2[ii+1] + fMixBL * left2[ii+iwidth] + fMixBR * left2[ii+iwidth+1]; + float fDiff = fPixel - left1[ii]; + JTe_x += fDiff * Jx[c]; + JTe_y += fDiff * Jy[c]; + chi_sq += fDiff*fDiff; + c++; + } + } + + //only update if the error got smaller + if (iters == 0 || chi_sq_previous > chi_sq) + { + //compute inverse of hessian + float det = (JTJ[0]*JTJ[3]-JTJ[1]*JTJ[2]); + if (det != 0.f) + { + float detinv = 1.f / det; + float JTJinv[4]; + JTJinv[0] = detinv * JTJ[3]; + JTJinv[1] = detinv * -JTJ[1]; + JTJinv[2] = detinv * -JTJ[2]; + JTJinv[3] = detinv * JTJ[0]; + + //compute update and shift current position accordingly + float updx = JTJinv[0]*JTe_x + JTJinv[1]*JTe_y; + float updy = JTJinv[2]*JTe_x + JTJinv[3]*JTe_y; + float new_u = u-updx; + float new_v = v-updy; + + //check if we drifted outside the image + if (((int16_t)new_u < HALF_PATCH_SIZE+1) || (int16_t)new_u > (iwidth-HALF_PATCH_SIZE-1) || ((int16_t)new_v < HALF_PATCH_SIZE+1) || (int16_t)new_v > (iwidth-HALF_PATCH_SIZE-1)) + { + break; + } + else + { + u = new_u; + v = new_v; + } + } + else + break; + } + else + break; + + chi_sq_previous = chi_sq; + } + + float nx = i-u; + float ny = j-v; + //only accept this flow measurement if it is inside the patch + if (fabs(nx) < HALF_PATCH_SIZE && fabs(ny) < HALF_PATCH_SIZE) + { + meanflowx += nx; + meanflowy += ny; + meancount++; + } + } + } + + /* evaluate flow calculation */ + if (meancount > 0) + { + meanflowx /= meancount; + meanflowy /= meancount; + + *pixel_flow_x = meanflowx; + *pixel_flow_y = meanflowy; +} +else +{ + *pixel_flow_x = 0.0f; + *pixel_flow_y = 0.0f; + return 0; +} + +/* return quality */ +return (uint8_t)(meancount * 255 / (NUM_BLOCKS*NUM_BLOCKS)); +} diff --git a/src/main.c b/src/main.c index e0e2a44..0ce94c0 100644 --- a/src/main.c +++ b/src/main.c @@ -339,6 +339,7 @@ int main(void) /* compute optical flow */ qual = compute_flow(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y); +// qual = compute_klt(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y); if (sonar_distance_filtered > 5.0f || sonar_distance_filtered == 0.0f) { From 75cedc75d3bbba6da66e51233c2d3a0c3185df12 Mon Sep 17 00:00:00 2001 From: pt Date: Tue, 29 Jul 2014 16:54:56 +0200 Subject: [PATCH 002/120] added image pyramid, now the maximum flow is around 4 pixels between frames but depending on the amount of motion blur and lens defocus it can be also much higher. Important: Gyro compensation not implemented currently, the corresponding parameter has no influence. To maintain 400 Hz refresh rate the flow is computed on 9 points in the image in a 3x3 grid, the size of the patch is 5x5 pixels. The code is not yet optimized, feel free to do so. --- Makefile | 2 +- src/flow.c | 275 +++++++++++++++++++++++++++++++++-------------------- src/main.c | 8 +- 3 files changed, 178 insertions(+), 107 deletions(-) diff --git a/Makefile b/Makefile index 00d3445..7b8b9fa 100644 --- a/Makefile +++ b/Makefile @@ -57,7 +57,7 @@ SRCS += lib/STM32F4xx_StdPeriph_Driver/src/misc.c \ .PHONY: clean upload-usb CFLAGS = -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 \ - -O1 -g \ + -O3 \ -std=gnu99 \ -Wall \ -MD \ diff --git a/src/flow.c b/src/flow.c index 2f56a2c..71bc102 100644 --- a/src/flow.c +++ b/src/flow.c @@ -51,10 +51,13 @@ #define FRAME_SIZE BOTTOM_FLOW_IMAGE_WIDTH #define SEARCH_SIZE BOTTOM_FLOW_SEARCH_WINDOW_SIZE // maximum offset to search: 4 + 1/2 pixels #define TILE_SIZE 8 // x & y tile size -#define NUM_BLOCKS 5 // x & y number of tiles to check +#define NUM_BLOCKS 3 // x & y number of tiles to check -#define HALF_PATCH_SIZE (SEARCH_SIZE+1) //this is half the wanted patch size minus 1 +//this are the settings for KLT based flow +#define PYR_LVLS 2 +#define HALF_PATCH_SIZE 4 //this is half the wanted patch size minus 1 #define PATCH_SIZE (HALF_PATCH_SIZE*2+1) + float Jx[PATCH_SIZE*PATCH_SIZE]; float Jy[PATCH_SIZE*PATCH_SIZE]; @@ -759,7 +762,7 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat * @brief Computes pixel flow from image1 to image2 * * Searches the corresponding position in the new image (image2) of max. 64 pixels from the old image (image1) - * with the KLT method (one level = 1 pixel search range!) and outputs the average value of all flow vectors + * with the KLT method and outputs the average value of all flow vectors * * @param image1 previous image buffer * @param image2 current image buffer (new) @@ -774,139 +777,207 @@ uint8_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate /* variables */ uint16_t i, j; - //better distribution of sampling points: - uint16_t pixLo = FRAME_SIZE / (NUM_BLOCKS + 1); - if (pixLo < PATCH_SIZE) pixLo = PATCH_SIZE; - uint16_t pixHi = (uint16_t)FRAME_SIZE * NUM_BLOCKS / (NUM_BLOCKS + 1); - if (pixHi > (FRAME_SIZE - PATCH_SIZE)) pixHi = FRAME_SIZE - PATCH_SIZE; - uint16_t pixStep = (pixHi - pixLo) / (NUM_BLOCKS-1); - float meanflowx = 0.0f; float meanflowy = 0.0f; uint16_t meancount = 0; - /* iterate over all patterns + /* + * compute image pyramid for current frame + * there is 188*120 bytes per buffer, we are only using 64*64 per buffer, + * so just add the pyramid levels after the image */ - for (j = pixLo; j < pixHi; j += pixStep) + //first compute the offsets in the memory for the pyramid levels + uint16_t lvl_off[PYR_LVLS]; + uint16_t s = FRAME_SIZE; + uint16_t off = 0; + for (int l = 0; l < PYR_LVLS; l++) { - for (i = pixLo; i < pixHi; i += pixStep) - { - uint16_t iwidth = global_data.param[PARAM_IMAGE_WIDTH]; - uint8_t *base1 = image1 + j * iwidth + i; - - float JTJ[4]; //the 2x2 Hessian - JTJ[0] = 0; - JTJ[1] = 0; - JTJ[2] = 0; - JTJ[3] = 0; - int c = 0; - - //compute jacobians and the hessian for the patch at the current location - for (int8_t jj = -HALF_PATCH_SIZE; jj <= HALF_PATCH_SIZE; jj++) + lvl_off[l] = off; + off += s*s; + s /= 2; + } + + //then subsample the images consecutively, no blurring is done before the subsampling (if someone volunteers, please go ahead...) + for (int l = 1; l < PYR_LVLS; l++) + { + uint16_t src_size = FRAME_SIZE >> (l-1); + uint16_t tar_size = FRAME_SIZE >> l; + uint8_t *source = &image2[lvl_off[l-1]]; //pointer to the beginning of the previous level + uint8_t *target = &image2[lvl_off[l]]; //pointer to the beginning of the current level + for (j = 0; j < tar_size; j++) + for (i = 0; i < tar_size; i+=2) { - uint8_t *left = base1 + jj*iwidth; - for (int8_t ii = -HALF_PATCH_SIZE; ii <= HALF_PATCH_SIZE; ii++) - { - const float jx = ((uint16_t)left[ii+1] - (uint16_t)left[ii-1]) * 0.5f; - const float jy = ((uint16_t)left[ii+iwidth] - (uint16_t)left[ii-iwidth]) * 0.5f; - Jx[c] = jx; - Jy[c] = jy; - JTJ[0] += jx*jx; - JTJ[1] += jx*jy; - JTJ[2] += jx*jy; - JTJ[3] += jy*jy; - c++; - } + //subsample the image by 2, use the halving-add instruction to do so + uint32_t l1 = (__UHADD8(*((uint32_t*) &source[(j*2+0)*src_size + i*2]), *((uint32_t*) &source[(j*2+0)*src_size + i*2+1]))); + uint32_t l2 = (__UHADD8(*((uint32_t*) &source[(j*2+1)*src_size + i*2]), *((uint32_t*) &source[(j*2+1)*src_size + i*2+1]))); + uint32_t r = __UHADD8(l1, l2); + + //the first and the third byte are the values we want to have + target[j*tar_size + i+0] = (uint8_t) r; + target[j*tar_size + i+1] = (uint8_t) (r>>16); } + } - //assume that we did not move - float u = i; - float v = j; + //need to store the flow values between pyramid level changes + float us[NUM_BLOCKS*NUM_BLOCKS]; + float vs[NUM_BLOCKS*NUM_BLOCKS]; + uint16_t is[NUM_BLOCKS*NUM_BLOCKS]; + uint16_t js[NUM_BLOCKS*NUM_BLOCKS]; - float chi_sq_previous = 0.f; - //Now do some Gauss-Newton iterations for flow - for (int iters = 0; iters < 5; iters++) - { - float JTe_x = 0; //accumulators for Jac transposed times error - float JTe_y = 0; + //initialize flow values with the pixel value of the previous image + uint16_t pixLo = FRAME_SIZE / (NUM_BLOCKS + 1); + if (pixLo < PATCH_SIZE) pixLo = PYR_LVLS * PATCH_SIZE; + uint16_t pixHi = (uint16_t)FRAME_SIZE * NUM_BLOCKS / (NUM_BLOCKS + 1); + if (pixHi > (FRAME_SIZE - PATCH_SIZE)) pixHi = FRAME_SIZE - (PYR_LVLS * PATCH_SIZE); + uint16_t pixStep = (pixHi - pixLo) / (NUM_BLOCKS-1); - uint8_t *base2 = image2 + (uint16_t)v * iwidth + (uint16_t)u; + j = pixLo; + for (int y = 0; y < NUM_BLOCKS; y++, j += pixStep) + { + i = pixLo; + for (int x = 0; x < NUM_BLOCKS; x++, i += pixStep) + { + //TODO: for proper rotation compensation, insert gyro values here and then substract at the end + us[y*NUM_BLOCKS+x] = i; //position in new image at level 0 + vs[y*NUM_BLOCKS+x] = j; + is[y*NUM_BLOCKS+x] = i; //position in previous image at level 0 + js[y*NUM_BLOCKS+x] = j; + } + } + + //for all pyramid levels, start from the smallest level + for (int l = PYR_LVLS-1; l >= 0; l--) + { + //iterate over all patterns + for (int k = 0; k < NUM_BLOCKS*NUM_BLOCKS; k++) + { + uint16_t i = is[k] >> l; //reference pixel for the current level + uint16_t j = js[k] >> l; - //extract bilinearly filtered pixel values for the current location in image2 - float dX = u - floorf(u); - float dY = v - floorf(v); - float fMixTL = (1.f - dX) * (1.f - dY); - float fMixTR = (dX) * (1.f - dY); - float fMixBL = (1.f - dX) * (dY); - float fMixBR = (dX) * (dY); + uint16_t iwidth = FRAME_SIZE >> l; + uint8_t *base1 = image1 + lvl_off[l] + j * iwidth + i; - float chi_sq = 0.f; + float JTJ[4]; //the 2x2 Hessian + JTJ[0] = 0; + JTJ[1] = 0; + JTJ[2] = 0; + JTJ[3] = 0; int c = 0; + + //compute jacobians and the hessian for the patch at the current location for (int8_t jj = -HALF_PATCH_SIZE; jj <= HALF_PATCH_SIZE; jj++) { - uint8_t *left1 = base1 + jj*iwidth; - uint8_t *left2 = base2 + jj*iwidth; - + uint8_t *left = base1 + jj*iwidth; for (int8_t ii = -HALF_PATCH_SIZE; ii <= HALF_PATCH_SIZE; ii++) { - float fPixel = fMixTL * left2[ii] + fMixTR * left2[ii+1] + fMixBL * left2[ii+iwidth] + fMixBR * left2[ii+iwidth+1]; - float fDiff = fPixel - left1[ii]; - JTe_x += fDiff * Jx[c]; - JTe_y += fDiff * Jy[c]; - chi_sq += fDiff*fDiff; + const float jx = ((uint16_t)left[ii+1] - (uint16_t)left[ii-1]) * 0.5f; + const float jy = ((uint16_t)left[ii+iwidth] - (uint16_t)left[ii-iwidth]) * 0.5f; + Jx[c] = jx; + Jy[c] = jy; + JTJ[0] += jx*jx; + JTJ[1] += jx*jy; + JTJ[2] += jx*jy; + JTJ[3] += jy*jy; c++; } } - //only update if the error got smaller - if (iters == 0 || chi_sq_previous > chi_sq) + // us and vs store the sample position in level 0 pixel coordinates + float u = (us[k] / (1< (iwidth-HALF_PATCH_SIZE-1) || ((int16_t)new_v < HALF_PATCH_SIZE+1) || (int16_t)new_v > (iwidth-HALF_PATCH_SIZE-1)) + uint8_t *left1 = base1 + jj*iwidth; + uint8_t *left2 = base2 + jj*iwidth; + + for (int8_t ii = -HALF_PATCH_SIZE; ii <= HALF_PATCH_SIZE; ii++) { - break; + float fPixel = fMixTL * left2[ii] + fMixTR * left2[ii+1] + fMixBL * left2[ii+iwidth] + fMixBR * left2[ii+iwidth+1]; + float fDiff = fPixel - left1[ii]; + JTe_x += fDiff * Jx[c]; + JTe_y += fDiff * Jy[c]; + chi_sq += fDiff*fDiff; + c++; } - else + } + + //only update if the error got smaller + if (iters == 0 || chi_sq_previous > chi_sq) + { + //compute inverse of hessian + float det = (JTJ[0]*JTJ[3]-JTJ[1]*JTJ[2]); + if (det != 0.f) { - u = new_u; - v = new_v; + float detinv = 1.f / det; + float JTJinv[4]; + JTJinv[0] = detinv * JTJ[3]; + JTJinv[1] = detinv * -JTJ[1]; + JTJinv[2] = detinv * -JTJ[2]; + JTJinv[3] = detinv * JTJ[0]; + + //compute update and shift current position accordingly + float updx = JTJinv[0]*JTe_x + JTJinv[1]*JTe_y; + float updy = JTJinv[2]*JTe_x + JTJinv[3]*JTe_y; + float new_u = u-updx; + float new_v = v-updy; + + //check if we drifted outside the image + if (((int16_t)new_u < HALF_PATCH_SIZE) || (int16_t)new_u > (iwidth-HALF_PATCH_SIZE-1) || ((int16_t)new_v < HALF_PATCH_SIZE) || (int16_t)new_v > (iwidth-HALF_PATCH_SIZE-1)) + { + break; + } + else + { + u = new_u; + v = new_v; + } } + else + break; } else break; - } - else - break; - chi_sq_previous = chi_sq; - } + chi_sq_previous = chi_sq; + } - float nx = i-u; - float ny = j-v; - //only accept this flow measurement if it is inside the patch - if (fabs(nx) < HALF_PATCH_SIZE && fabs(ny) < HALF_PATCH_SIZE) - { - meanflowx += nx; - meanflowy += ny; - meancount++; + if (l > 0) + { + us[k] = u * (1< 5.0f || sonar_distance_filtered == 0.0f) { @@ -516,9 +516,9 @@ int main(void) uint16_t image_width_send; uint16_t image_height_send; - image_size_send = image_size; image_width_send = global_data.param[PARAM_IMAGE_WIDTH]; - image_height_send = global_data.param[PARAM_IMAGE_HEIGHT]; + image_height_send = global_data.param[PARAM_IMAGE_HEIGHT];//+global_data.param[PARAM_IMAGE_HEIGHT]/2; + image_size_send = image_width_send*image_height_send; mavlink_msg_data_transmission_handshake_send( MAVLINK_COMM_2, From de9a6e1f97d52a6ec6c484a96bcbb675119fef42 Mon Sep 17 00:00:00 2001 From: pt Date: Wed, 30 Jul 2014 13:22:43 +0200 Subject: [PATCH 003/120] moved gyro compensation from flow.c to main.c, removed clamping to max flow value, the clamping should happen on the gyro rates not on the final flow output --- src/flow.c | 70 +++--------------------------------------------------- src/main.c | 23 ++++++++++++++++++ 2 files changed, 26 insertions(+), 67 deletions(-) diff --git a/src/flow.c b/src/flow.c index 78c0611..4d66a6e 100644 --- a/src/flow.c +++ b/src/flow.c @@ -658,73 +658,9 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat } - /* compensate rotation */ - /* 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 - - /* - * gyro compensation - * the compensated value is clamped to - * the maximum measurable flow value (param BFLOW_MAX_PIX) +0.5 - * (sub pixel flow can add half pixel to the value) - * - * -y_rate gives x flow - * x_rates gives y_flow - */ - if (global_data.param[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION]) - { - if(fabsf(y_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD]) - { - /* calc pixel of gyro */ - float y_rate_pixel = y_rate * (get_time_between_images() / 1000.0f) * focal_length_px; - float comp_x = histflowx + y_rate_pixel; - - /* clamp value to maximum search window size plus half pixel from subpixel search */ - if (comp_x < (-SEARCH_SIZE - 0.5f)) - *pixel_flow_x = (-SEARCH_SIZE - 0.5f); - else if (comp_x > (SEARCH_SIZE + 0.5f)) - *pixel_flow_x = (SEARCH_SIZE + 0.5f); - else - *pixel_flow_x = comp_x; - } - else - { - *pixel_flow_x = histflowx; - } - - if(fabsf(x_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD]) - { - /* calc pixel of gyro */ - float x_rate_pixel = x_rate * (get_time_between_images() / 1000.0f) * focal_length_px; - float comp_y = histflowy - x_rate_pixel; - - /* clamp value to maximum search window size plus/minus half pixel from subpixel search */ - if (comp_y < (-SEARCH_SIZE - 0.5f)) - *pixel_flow_y = (-SEARCH_SIZE - 0.5f); - else if (comp_y > (SEARCH_SIZE + 0.5f)) - *pixel_flow_y = (SEARCH_SIZE + 0.5f); - else - *pixel_flow_y = comp_y; - } - else - { - *pixel_flow_y = histflowy; - } - - /* alternative compensation */ -// /* compensate y rotation */ -// *pixel_flow_x = histflowx + y_rate_pixel; -// -// /* compensate x rotation */ -// *pixel_flow_y = histflowy - x_rate_pixel; - - } else - { - /* without gyro compensation */ - *pixel_flow_x = histflowx; - *pixel_flow_y = histflowy; - } - + /* write results */ + *pixel_flow_x = histflowx; + *pixel_flow_y = histflowy; } else { diff --git a/src/main.c b/src/main.c index e0e2a44..cc4922c 100644 --- a/src/main.c +++ b/src/main.c @@ -359,6 +359,29 @@ int main(void) 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); + + /* + * gyro compensation + * + * TODO: do not compensate more than the valid flow value (+/- 4.5 pixels) + * + * -y_rate gives x flow + * x_rates gives y_flow + */ + if (global_data.param[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION]) + { + if(fabsf(y_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD]) + { + flow_compx = flow_compx + y_rate; + } + + if(fabsf(x_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD]) + { + flow_compy = flow_compy - x_rate; + } + } + + /* integrate velocity and output values only if distance is valid */ if (distance_valid) { From 3e85ec555362e5c69b693dc7434f5c4317b5aebf Mon Sep 17 00:00:00 2001 From: pt Date: Wed, 30 Jul 2014 14:22:11 +0200 Subject: [PATCH 004/120] sign mistake --- src/main.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main.c b/src/main.c index cc4922c..03da15e 100644 --- a/src/main.c +++ b/src/main.c @@ -372,12 +372,12 @@ int main(void) { if(fabsf(y_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD]) { - flow_compx = flow_compx + y_rate; + flow_compx = flow_compx - y_rate; } if(fabsf(x_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD]) { - flow_compy = flow_compy - x_rate; + flow_compy = flow_compy + x_rate; } } From e797078fc506270842b04bcfafcfb6a6deb6fecc Mon Sep 17 00:00:00 2001 From: pt Date: Tue, 23 Jun 2015 14:49:54 +0200 Subject: [PATCH 005/120] clean up --- src/flow.c | 315 +++++++++++++++++++---------------------------------- src/main.c | 6 +- 2 files changed, 115 insertions(+), 206 deletions(-) diff --git a/src/flow.c b/src/flow.c index d974258..3b1d96b 100644 --- a/src/flow.c +++ b/src/flow.c @@ -134,49 +134,6 @@ float Jy[PATCH_SIZE*PATCH_SIZE]; result; \ }) -/** - * @brief Computes the Hessian at a pixel location - * - * The hessian (second order partial derivatives of the image) is - * a measure of the salience of the image at the appropriate - * box filter scale. It allows to judge wether a pixel - * location is suitable for optical flow calculation. - * - * @param image the array holding pixel data - * @param x location of the pixel in x - * @param y location of the pixel in y - * - * @return gradient magnitude - */ -static inline uint32_t compute_hessian_4x6(uint8_t *image, uint16_t x, uint16_t y, uint16_t row_size) -{ - // candidate for hessian calculation: - uint16_t off1 = y*row_size + x; // First row of ones - uint16_t off2 = (y+1)*row_size + x; // Second row of ones - uint16_t off3 = (y+2)*row_size + x; // Third row of minus twos - uint16_t off4 = (y+3)*row_size + x; // Third row of minus twos - uint16_t off5 = (y+4)*row_size + x; // Third row of minus twos - uint16_t off6 = (y+5)*row_size + x; // Third row of minus twos - uint32_t magnitude; - - // Uncentered for max. performance: - // center pixel is in brackets () - - // 1 1 1 1 - // 1 1 1 1 - // -2 (-2) -2 -2 - // -2 -2 -2 -2 - // 1 1 1 1 - // 1 1 1 1 - - magnitude = __UADD8(*((uint32_t*) &image[off1 - 1]), *((uint32_t*) &image[off2 - 1])); - magnitude -= 2*__UADD8(*((uint32_t*) &image[off3 - 1]), *((uint32_t*) &image[off4 - 1])); - magnitude += __UADD8(*((uint32_t*) &image[off5 - 1]), *((uint32_t*) &image[off6 - 1])); - - return magnitude; -} - - /** * @brief Compute the average pixel gradient of all horizontal and vertical steps * @@ -343,50 +300,6 @@ static inline uint32_t compute_subpixel(uint8_t *image1, uint8_t *image2, uint16 return 0; } -/** - * @brief Compute SAD of two 8x8 pixel windows. - * - * @param image1 ... - * @param image2 ... - * @param off1X x coordinate of upper left corner of pattern in image1 - * @param off1Y y coordinate of upper left corner of pattern in image1 - * @param off2X x coordinate of upper left corner of pattern in image2 - * @param off2Y y coordinate of upper left corner of pattern in image2 - */ -static inline uint32_t compute_sad_8x8(uint8_t *image1, uint8_t *image2, uint16_t off1X, uint16_t off1Y, uint16_t off2X, uint16_t off2Y, uint16_t row_size) -{ - /* calculate position in image buffer */ - uint16_t off1 = off1Y * row_size + off1X; // image1 - uint16_t off2 = off2Y * row_size + off2X; // image2 - - uint32_t acc; - acc = __USAD8 (*((uint32_t*) &image1[off1 + 0 + 0 * row_size]), *((uint32_t*) &image2[off2 + 0 + 0 * row_size])); - acc = __USADA8(*((uint32_t*) &image1[off1 + 4 + 0 * row_size]), *((uint32_t*) &image2[off2 + 4 + 0 * row_size]), acc); - - acc = __USADA8(*((uint32_t*) &image1[off1 + 0 + 1 * row_size]), *((uint32_t*) &image2[off2 + 0 + 1 * row_size]), acc); - acc = __USADA8(*((uint32_t*) &image1[off1 + 4 + 1 * row_size]), *((uint32_t*) &image2[off2 + 4 + 1 * row_size]), acc); - - acc = __USADA8(*((uint32_t*) &image1[off1 + 0 + 2 * row_size]), *((uint32_t*) &image2[off2 + 0 + 2 * row_size]), acc); - acc = __USADA8(*((uint32_t*) &image1[off1 + 4 + 2 * row_size]), *((uint32_t*) &image2[off2 + 4 + 2 * row_size]), acc); - - acc = __USADA8(*((uint32_t*) &image1[off1 + 0 + 3 * row_size]), *((uint32_t*) &image2[off2 + 0 + 3 * row_size]), acc); - acc = __USADA8(*((uint32_t*) &image1[off1 + 4 + 3 * row_size]), *((uint32_t*) &image2[off2 + 4 + 3 * row_size]), acc); - - acc = __USADA8(*((uint32_t*) &image1[off1 + 0 + 4 * row_size]), *((uint32_t*) &image2[off2 + 0 + 4 * row_size]), acc); - acc = __USADA8(*((uint32_t*) &image1[off1 + 4 + 4 * row_size]), *((uint32_t*) &image2[off2 + 4 + 4 * row_size]), acc); - - acc = __USADA8(*((uint32_t*) &image1[off1 + 0 + 5 * row_size]), *((uint32_t*) &image2[off2 + 0 + 5 * row_size]), acc); - acc = __USADA8(*((uint32_t*) &image1[off1 + 4 + 5 * row_size]), *((uint32_t*) &image2[off2 + 4 + 5 * row_size]), acc); - - acc = __USADA8(*((uint32_t*) &image1[off1 + 0 + 6 * row_size]), *((uint32_t*) &image2[off2 + 0 + 6 * row_size]), acc); - acc = __USADA8(*((uint32_t*) &image1[off1 + 4 + 6 * row_size]), *((uint32_t*) &image2[off2 + 4 + 6 * row_size]), acc); - - acc = __USADA8(*((uint32_t*) &image1[off1 + 0 + 7 * row_size]), *((uint32_t*) &image2[off2 + 0 + 7 * row_size]), acc); - acc = __USADA8(*((uint32_t*) &image1[off1 + 4 + 7 * row_size]), *((uint32_t*) &image2[off2 + 4 + 7 * row_size]), acc); - - return acc; -} - /** * @brief Computes pixel flow from image1 to image2 * @@ -788,152 +701,152 @@ uint8_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate //iterate over all patterns for (int k = 0; k < NUM_BLOCKS*NUM_BLOCKS; k++) { - uint16_t i = is[k] >> l; //reference pixel for the current level - uint16_t j = js[k] >> l; + uint16_t i = is[k] >> l; //reference pixel for the current level + uint16_t j = js[k] >> l; - uint16_t iwidth = FRAME_SIZE >> l; - uint8_t *base1 = image1 + lvl_off[l] + j * iwidth + i; + uint16_t iwidth = FRAME_SIZE >> l; + uint8_t *base1 = image1 + lvl_off[l] + j * iwidth + i; - float JTJ[4]; //the 2x2 Hessian - JTJ[0] = 0; - JTJ[1] = 0; - JTJ[2] = 0; - JTJ[3] = 0; - int c = 0; + float JTJ[4]; //the 2x2 Hessian + JTJ[0] = 0; + JTJ[1] = 0; + JTJ[2] = 0; + JTJ[3] = 0; + int c = 0; + + //compute jacobians and the hessian for the patch at the current location + for (int8_t jj = -HALF_PATCH_SIZE; jj <= HALF_PATCH_SIZE; jj++) + { + uint8_t *left = base1 + jj*iwidth; + for (int8_t ii = -HALF_PATCH_SIZE; ii <= HALF_PATCH_SIZE; ii++) + { + const float jx = ((uint16_t)left[ii+1] - (uint16_t)left[ii-1]) * 0.5f; + const float jy = ((uint16_t)left[ii+iwidth] - (uint16_t)left[ii-iwidth]) * 0.5f; + Jx[c] = jx; + Jy[c] = jy; + JTJ[0] += jx*jx; + JTJ[1] += jx*jy; + JTJ[2] += jx*jy; + JTJ[3] += jy*jy; + c++; + } + } + + // us and vs store the sample position in level 0 pixel coordinates + float u = (us[k] / (1< chi_sq) { - float JTe_x = 0; //accumulators for Jac transposed times error - float JTe_y = 0; - - uint8_t *base2 = image2 + lvl_off[l] + (uint16_t)v * iwidth + (uint16_t)u; - - //extract bilinearly filtered pixel values for the current location in image2 - float dX = u - floorf(u); - float dY = v - floorf(v); - float fMixTL = (1.f - dX) * (1.f - dY); - float fMixTR = (dX) * (1.f - dY); - float fMixBL = (1.f - dX) * (dY); - float fMixBR = (dX) * (dY); - - float chi_sq = 0.f; - int c = 0; - for (int8_t jj = -HALF_PATCH_SIZE; jj <= HALF_PATCH_SIZE; jj++) + //compute inverse of hessian + float det = (JTJ[0]*JTJ[3]-JTJ[1]*JTJ[2]); + if (det != 0.f) { - uint8_t *left1 = base1 + jj*iwidth; - uint8_t *left2 = base2 + jj*iwidth; - - for (int8_t ii = -HALF_PATCH_SIZE; ii <= HALF_PATCH_SIZE; ii++) + float detinv = 1.f / det; + float JTJinv[4]; + JTJinv[0] = detinv * JTJ[3]; + JTJinv[1] = detinv * -JTJ[1]; + JTJinv[2] = detinv * -JTJ[2]; + JTJinv[3] = detinv * JTJ[0]; + + //compute update and shift current position accordingly + float updx = JTJinv[0]*JTe_x + JTJinv[1]*JTe_y; + float updy = JTJinv[2]*JTe_x + JTJinv[3]*JTe_y; + float new_u = u-updx; + float new_v = v-updy; + + //check if we drifted outside the image + if (((int16_t)new_u < HALF_PATCH_SIZE) || (int16_t)new_u > (iwidth-HALF_PATCH_SIZE-1) || ((int16_t)new_v < HALF_PATCH_SIZE) || (int16_t)new_v > (iwidth-HALF_PATCH_SIZE-1)) { - float fPixel = fMixTL * left2[ii] + fMixTR * left2[ii+1] + fMixBL * left2[ii+iwidth] + fMixBR * left2[ii+iwidth+1]; - float fDiff = fPixel - left1[ii]; - JTe_x += fDiff * Jx[c]; - JTe_y += fDiff * Jy[c]; - chi_sq += fDiff*fDiff; - c++; + break; } - } - - //only update if the error got smaller - if (iters == 0 || chi_sq_previous > chi_sq) - { - //compute inverse of hessian - float det = (JTJ[0]*JTJ[3]-JTJ[1]*JTJ[2]); - if (det != 0.f) + else { - float detinv = 1.f / det; - float JTJinv[4]; - JTJinv[0] = detinv * JTJ[3]; - JTJinv[1] = detinv * -JTJ[1]; - JTJinv[2] = detinv * -JTJ[2]; - JTJinv[3] = detinv * JTJ[0]; - - //compute update and shift current position accordingly - float updx = JTJinv[0]*JTe_x + JTJinv[1]*JTe_y; - float updy = JTJinv[2]*JTe_x + JTJinv[3]*JTe_y; - float new_u = u-updx; - float new_v = v-updy; - - //check if we drifted outside the image - if (((int16_t)new_u < HALF_PATCH_SIZE) || (int16_t)new_u > (iwidth-HALF_PATCH_SIZE-1) || ((int16_t)new_v < HALF_PATCH_SIZE) || (int16_t)new_v > (iwidth-HALF_PATCH_SIZE-1)) - { - break; - } - else - { - u = new_u; - v = new_v; - } + u = new_u; + v = new_v; } - else - break; } else break; - - chi_sq_previous = chi_sq; } + else + break; - if (l > 0) + chi_sq_previous = chi_sq; + } + + if (l > 0) + { + us[k] = u * (1< 0) { meanflowx /= meancount; meanflowy /= meancount; - *pixel_flow_x = meanflowx; - *pixel_flow_y = meanflowy; -} -else -{ - *pixel_flow_x = 0.0f; - *pixel_flow_y = 0.0f; - return 0; -} + *pixel_flow_x = meanflowx; + *pixel_flow_y = meanflowy; + } + else + { + *pixel_flow_x = 0.0f; + *pixel_flow_y = 0.0f; + return 0; + } -/* return quality */ -return (uint8_t)(meancount * 255 / (NUM_BLOCKS*NUM_BLOCKS)); + /* return quality */ + return (uint8_t)(meancount * 255 / (NUM_BLOCKS*NUM_BLOCKS)); } diff --git a/src/main.c b/src/main.c index 78c4412..606b713 100644 --- a/src/main.c +++ b/src/main.c @@ -320,10 +320,6 @@ int main(void) /* 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; @@ -460,7 +456,7 @@ int main(void) 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, + pixel_flow_x_sum * 100.0f, pixel_flow_y_sum * 100.0f, 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, From 589075b8d23b39c72121a541aba09c1f8f8548b5 Mon Sep 17 00:00:00 2001 From: Severin Latkovic Date: Wed, 1 Jul 2015 18:46:13 +0200 Subject: [PATCH 006/120] Added gyro compensation to KLT algorithm --- src/flow.c | 158 +++++++++++++++++++++++++++++++++++++++++------------ src/main.c | 16 ++++-- 2 files changed, 134 insertions(+), 40 deletions(-) diff --git a/src/flow.c b/src/flow.c index 54df5d9..ba0af7b 100644 --- a/src/flow.c +++ b/src/flow.c @@ -457,8 +457,8 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat for (ii = winmin; ii <= winmax; ii++) { -// uint32_t temp_dist = compute_sad_8x8(image1, image2, i, j, i + ii, j + jj, (uint16_t) global_data.param[PARAM_IMAGE_WIDTH]); - uint32_t temp_dist = ABSDIFF(base1, base2 + ii); + uint32_t temp_dist = compute_sad_8x8(image1, image2, i, j, i + ii, j + jj, (uint16_t) global_data.param[PARAM_IMAGE_WIDTH]); +// uint32_t temp_dist = ABSDIFF(base1, base2 + ii); if (temp_dist < dist) { sumx = ii; @@ -509,23 +509,23 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat /* create flow image if needed (image1 is not needed anymore) * -> can be used for debugging purpose */ -// if (global_data.param[PARAM_USB_SEND_VIDEO] )//&& global_data.param[PARAM_VIDEO_USB_MODE] == FLOW_VIDEO) -// { -// -// for (j = pixLo; j < pixHi; j += pixStep) -// { -// for (i = pixLo; i < pixHi; i += pixStep) -// { -// -// uint32_t diff = compute_diff(image1, i, j, (uint16_t) global_data.param[PARAM_IMAGE_WIDTH]); -// if (diff > global_data.param[PARAM_BOTTOM_FLOW_FEATURE_THRESHOLD]) -// { -// image1[j * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i] = 255; -// } -// -// } -// } -// } + if (global_data.param[PARAM_USB_SEND_VIDEO] )//&& global_data.param[PARAM_VIDEO_USB_MODE] == FLOW_VIDEO) + { + + for (j = pixLo; j < pixHi; j += pixStep) + { + for (i = pixLo; i < pixHi; i += pixStep) + { + + uint32_t diff = compute_diff(image1, i, j, (uint16_t) global_data.param[PARAM_IMAGE_WIDTH]); + if (diff > global_data.param[PARAM_BOTTOM_FLOW_FEATURE_THRESHOLD]) + { + image1[j * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i] = 255; + } + + } + } + } /* evaluate flow calculation */ if (meancount > 10) @@ -982,22 +982,110 @@ uint8_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate } } - /* compute mean flow */ - if (meancount > 0) - { - meanflowx /= meancount; - meanflowy /= meancount; +/* create flow image if needed (image1 is not needed anymore) + * -> can be used for debugging purpose + */ +if (global_data.param[PARAM_USB_SEND_VIDEO] )//&& global_data.param[PARAM_VIDEO_USB_MODE] == FLOW_VIDEO) +{ - *pixel_flow_x = meanflowx; - *pixel_flow_y = meanflowy; - } - else - { - *pixel_flow_x = 0.0f; - *pixel_flow_y = 0.0f; - return 0; - } + for (j = pixLo; j < pixHi; j += pixStep) + { + for (i = pixLo; i < pixHi; i += pixStep) + { + + uint32_t diff = compute_diff(image1, i, j, (uint16_t) global_data.param[PARAM_IMAGE_WIDTH]); + if (diff > global_data.param[PARAM_BOTTOM_FLOW_FEATURE_THRESHOLD]) + { + image1[j * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i] = 255; + } + + } + } +} + +/* compute mean flow */ +if (meancount > 0) +{ + meanflowx /= meancount; + meanflowy /= meancount; + + + + /* compensate rotation */ + /* 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 + + /* + * gyro compensation + * the compensated value is clamped to + * the maximum measurable flow value (param BFLOW_MAX_PIX) +0.5 + * (sub pixel flow can add half pixel to the value) + * + * -y_rate gives x flow + * x_rates gives y_flow + */ + if (global_data.param[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION]) + { + if(fabsf(y_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD]) + { + /* calc pixel of gyro */ + float y_rate_pixel = y_rate * (get_time_between_images() / 1000000.0f) * focal_length_px; + float comp_x = meanflowx + y_rate_pixel; + + /* clamp value to maximum search window size plus half pixel from subpixel search */ + if (comp_x < (-SEARCH_SIZE - 0.5f)) + *pixel_flow_x = (-SEARCH_SIZE - 0.5f); + else if (comp_x > (SEARCH_SIZE + 0.5f)) + *pixel_flow_x = (SEARCH_SIZE + 0.5f); + else + *pixel_flow_x = comp_x; + } + else + { + *pixel_flow_x = meanflowx; + } + + if(fabsf(x_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD]) + { + /* calc pixel of gyro */ + float x_rate_pixel = x_rate * (get_time_between_images() / 1000000.0f) * focal_length_px; + float comp_y = meanflowy - x_rate_pixel; + /* clamp value to maximum search window size plus/minus half pixel from subpixel search */ + if (comp_y < (-SEARCH_SIZE - 0.5f)) + *pixel_flow_y = (-SEARCH_SIZE - 0.5f); + else if (comp_y > (SEARCH_SIZE + 0.5f)) + *pixel_flow_y = (SEARCH_SIZE + 0.5f); + else + *pixel_flow_y = comp_y; + } + else + { + *pixel_flow_y = meanflowy; + } + + /* alternative compensation */ + // /* compensate y rotation */ + // *pixel_flow_x = meanflowx + y_rate_pixel; + // + // /* compensate x rotation */ + // *pixel_flow_y = meanflowy - x_rate_pixel; + + } + else + { + /* without gyro compensation */ + *pixel_flow_x = meanflowx; + *pixel_flow_y = meanflowy; + } + +} +else +{ + *pixel_flow_x = 0.0f; + *pixel_flow_y = 0.0f; + return 0; +} - /* return quality */ - return (uint8_t)(meancount * 255 / (NUM_BLOCK_KLT*NUM_BLOCK_KLT)); +/* return quality */ +return (uint8_t)(meancount * 255 / (NUM_BLOCK_KLT*NUM_BLOCK_KLT)); } \ No newline at end of file diff --git a/src/main.c b/src/main.c index b6c95c9..8b59976 100644 --- a/src/main.c +++ b/src/main.c @@ -299,10 +299,15 @@ int main(void) /* variables */ uint32_t counter = 0; uint8_t qual = 0; + uint8_t qual1 = 0; + uint8_t qual2 = 0; /* bottom flow variables */ float pixel_flow_x = 0.0f; float pixel_flow_y = 0.0f; + float pixel_flow_klt_x = 0.0f; + float pixel_flow_klt_y = 0.0f; + float pixel_flow_x_sum = 0.0f; float pixel_flow_y_sum = 0.0f; float velocity_x_sum = 0.0f; @@ -405,16 +410,17 @@ int main(void) dma_copy_image_buffers(¤t_image, &previous_image, image_size, 1); /* compute optical flow */ - qual = compute_flow(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y); -// qual = compute_klt(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y); +// qual = compute_flow(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y); + qual = compute_klt(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y); + /* * real point P (X,Y,Z), image plane projection p (x,y,z), focal-length f, distance-to-scene Z * x / f = X / Z * y / f = Y / Z */ - float flow_compx = pixel_flow_x / focal_length_px / (get_time_between_images() / 1000000.0f); - float flow_compy = pixel_flow_y / focal_length_px / (get_time_between_images() / 1000000.0f); + float flow_compx = pixel_flow_x / focal_length_px / (get_time_between_images() / 1000000.0f); + float flow_compy = pixel_flow_y / focal_length_px / (get_time_between_images() / 1000000.0f); /* integrate velocity and output values only if distance is valid */ if (distance_valid) @@ -666,7 +672,7 @@ int main(void) uint16_t frame = 0; for (frame = 0; frame < image_size_send / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1; frame++) { - mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, &((uint8_t *) current_image)[frame * MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN]); + mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, &((uint8_t *) previous_image)[frame * MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN]); } send_image_now = false; From 9b510d8c30ee2e47b9443bb665357180a2199dfe Mon Sep 17 00:00:00 2001 From: Severin Latkovic Date: Thu, 2 Jul 2015 17:02:36 +0200 Subject: [PATCH 007/120] - Fixed Gyro Compensation - Added visual representation of flow in QGroundcontrol when activating flow image (for debugging) --- src/flow.c | 160 ++++++++++++++++++++++--------------------------- src/main.c | 7 ++- src/settings.c | 2 +- 3 files changed, 77 insertions(+), 92 deletions(-) diff --git a/src/flow.c b/src/flow.c index ba0af7b..70d6218 100644 --- a/src/flow.c +++ b/src/flow.c @@ -511,7 +511,7 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat */ if (global_data.param[PARAM_USB_SEND_VIDEO] )//&& global_data.param[PARAM_VIDEO_USB_MODE] == FLOW_VIDEO) { - + int pixel_ind = 0; for (j = pixLo; j < pixHi; j += pixStep) { for (i = pixLo; i < pixHi; i += pixStep) @@ -521,6 +521,23 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat if (diff > global_data.param[PARAM_BOTTOM_FLOW_FEATURE_THRESHOLD]) { image1[j * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i] = 255; + if(subdirs[pixel_ind] == 0) + image1[j * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i+1] = 200; + if(subdirs[pixel_ind] == 1) + image1[(j+1) * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i+1] = 200; + if(subdirs[pixel_ind] == 2) + image1[(j+1) * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i] = 200; + if (subdirs[pixel_ind] == 3) + image1[(j+1) * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i-1] = 200; + if(subdirs[pixel_ind] == 4) + image1[j * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i-1] = 200; + if(subdirs[pixel_ind] == 5) + image1[(j-1) * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i-1] = 200; + if(subdirs[pixel_ind] == 6) + image1[(j-1) * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i] = 200; + if(subdirs[pixel_ind] == 7) + image1[(j-1) * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i + 1] = 200; + pixel_ind++; } } @@ -528,7 +545,7 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat } /* evaluate flow calculation */ - if (meancount > 10) + if (meancount > 5) { meanflowx /= meancount; meanflowy /= meancount; @@ -689,13 +706,13 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat float y_rate_pixel = y_rate * (get_time_between_images() / 1000000.0f) * focal_length_px; float comp_x = histflowx + y_rate_pixel; - /* clamp value to maximum search window size plus half pixel from subpixel search */ - if (comp_x < (-SEARCH_SIZE - 0.5f)) - *pixel_flow_x = (-SEARCH_SIZE - 0.5f); - else if (comp_x > (SEARCH_SIZE + 0.5f)) - *pixel_flow_x = (SEARCH_SIZE + 0.5f); - else - *pixel_flow_x = comp_x; + /* clamp value to maximum search window size plus half pixel from subpixel search */ + if (comp_x < (-SEARCH_SIZE - 0.5f)) + *pixel_flow_x = (-SEARCH_SIZE - 0.5f); + else if (comp_x > (SEARCH_SIZE + 0.5f)) + *pixel_flow_x = (SEARCH_SIZE + 0.5f); + else + *pixel_flow_x = comp_x; } else { @@ -773,12 +790,23 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat */ uint8_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate, float *pixel_flow_x, float *pixel_flow_y) { - /* variables */ - uint16_t i, j; +/* variables */ +uint16_t i, j; + +float meanflowx = 0.0f; +float meanflowy = 0.0f; +uint16_t meancount = 0; - float meanflowx = 0.0f; - float meanflowy = 0.0f; - uint16_t meancount = 0; +const float focal_length_px = (global_data.param[PARAM_FOCAL_LENGTH_MM]) / (4.0f * 6.0f) * 1000.0f; +float x_rate_pixel = x_rate * (get_time_between_images() / 1000000.0f) * focal_length_px; +float y_rate_pixel = y_rate * (get_time_between_images() / 1000000.0f) * focal_length_px; +if(fabs(x_rate_pixel)>3) + x_rate_pixel = 0; +if(fabs(y_rate_pixel)>3) + y_rate_pixel = 0; + +int x_rate_px_round = round(x_rate_pixel); +int y_rate_px_round = round(y_rate_pixel); /* * compute image pyramid for current frame @@ -839,8 +867,9 @@ uint8_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate for (int x = 0; x < NUM_BLOCK_KLT; x++, i += pixStep) { //TODO: for proper rotation compensation, insert gyro values here and then substract at the end - us[y*NUM_BLOCK_KLT+x] = i; //position in new image at level 0 - vs[y*NUM_BLOCK_KLT+x] = j; + + us[y*NUM_BLOCK_KLT+x] = i - y_rate_pixel; //position in new image at level 0 + vs[y*NUM_BLOCK_KLT+x] = j + x_rate_pixel; is[y*NUM_BLOCK_KLT+x] = i; //position in previous image at level 0 js[y*NUM_BLOCK_KLT+x] = j; } @@ -982,93 +1011,24 @@ uint8_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate } } -/* create flow image if needed (image1 is not needed anymore) - * -> can be used for debugging purpose - */ -if (global_data.param[PARAM_USB_SEND_VIDEO] )//&& global_data.param[PARAM_VIDEO_USB_MODE] == FLOW_VIDEO) -{ - - for (j = pixLo; j < pixHi; j += pixStep) - { - for (i = pixLo; i < pixHi; i += pixStep) - { - - uint32_t diff = compute_diff(image1, i, j, (uint16_t) global_data.param[PARAM_IMAGE_WIDTH]); - if (diff > global_data.param[PARAM_BOTTOM_FLOW_FEATURE_THRESHOLD]) - { - image1[j * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i] = 255; - } - - } - } -} - /* compute mean flow */ if (meancount > 0) { meanflowx /= meancount; meanflowy /= meancount; - - - /* compensate rotation */ - /* 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 - - /* - * gyro compensation - * the compensated value is clamped to - * the maximum measurable flow value (param BFLOW_MAX_PIX) +0.5 - * (sub pixel flow can add half pixel to the value) - * - * -y_rate gives x flow - * x_rates gives y_flow - */ if (global_data.param[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION]) { - if(fabsf(y_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD]) - { - /* calc pixel of gyro */ - float y_rate_pixel = y_rate * (get_time_between_images() / 1000000.0f) * focal_length_px; - float comp_x = meanflowx + y_rate_pixel; - - /* clamp value to maximum search window size plus half pixel from subpixel search */ - if (comp_x < (-SEARCH_SIZE - 0.5f)) - *pixel_flow_x = (-SEARCH_SIZE - 0.5f); - else if (comp_x > (SEARCH_SIZE + 0.5f)) - *pixel_flow_x = (SEARCH_SIZE + 0.5f); - else - *pixel_flow_x = comp_x; - } + if(fabsf(x_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD]) + *pixel_flow_x = meanflowx - y_rate_pixel; // + (x_rate_px_round - x_rate_pixel); else - { *pixel_flow_x = meanflowx; - } - if(fabsf(x_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD]) - { - /* calc pixel of gyro */ - float x_rate_pixel = x_rate * (get_time_between_images() / 1000000.0f) * focal_length_px; - float comp_y = meanflowy - x_rate_pixel; - /* clamp value to maximum search window size plus/minus half pixel from subpixel search */ - if (comp_y < (-SEARCH_SIZE - 0.5f)) - *pixel_flow_y = (-SEARCH_SIZE - 0.5f); - else if (comp_y > (SEARCH_SIZE + 0.5f)) - *pixel_flow_y = (SEARCH_SIZE + 0.5f); - else - *pixel_flow_y = comp_y; - } + if(fabsf(y_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD]) + *pixel_flow_y = meanflowy + x_rate_pixel; // + (y_rate_px_round - y_rate_pixel); else - { *pixel_flow_y = meanflowy; - } - /* alternative compensation */ - // /* compensate y rotation */ - // *pixel_flow_x = meanflowx + y_rate_pixel; - // - // /* compensate x rotation */ - // *pixel_flow_y = meanflowy - x_rate_pixel; } else @@ -1086,6 +1046,28 @@ else return 0; } +/* create flow image if needed (image1 is not needed anymore) + * -> can be used for debugging purpose + */ +if (global_data.param[PARAM_USB_SEND_VIDEO] )//&& global_data.param[PARAM_VIDEO_USB_MODE] == FLOW_VIDEO) +{ + + for (j = pixLo; j < pixHi; j += pixStep) + { + for (i = pixLo; i < pixHi; i += pixStep) + { + + + image1[j * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i] = 255; + int pixel = (j - (int)(meanflowx*4+0.5f)) * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + (i - (int)(meanflowy*4+0.5f)); + if(pixel >= 0 && pixel < global_data.param[PARAM_IMAGE_WIDTH]*global_data.param[PARAM_IMAGE_WIDTH]) + image1[pixel] = 200; + + + } + } +} + /* return quality */ return (uint8_t)(meancount * 255 / (NUM_BLOCK_KLT*NUM_BLOCK_KLT)); } \ No newline at end of file diff --git a/src/main.c b/src/main.c index 8b59976..6b287de 100644 --- a/src/main.c +++ b/src/main.c @@ -391,9 +391,11 @@ int main(void) float y_rate = - x_rate_sensor; float z_rate = z_rate_sensor; // z is correct + + /* 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 - + float x_rate_pixel = x_rate * (get_time_between_images() / 1000000.0f) * focal_length_px; /* get sonar data */ distance_valid = sonar_read(&sonar_distance_filtered, &sonar_distance_raw); @@ -439,6 +441,7 @@ int main(void) uint32_t deltatime = (get_boot_time_us() - lasttime); integration_timespan += deltatime; + //TODO -> right to swap? accumulated_flow_x += pixel_flow_y / focal_length_px * 1.0f; //rad axis swapped to align x flow around y axis accumulated_flow_y += pixel_flow_x / focal_length_px * -1.0f;//rad accumulated_gyro_x += x_rate * deltatime / 1000000.0f; //rad @@ -582,7 +585,7 @@ int main(void) if(global_data.param[PARAM_USB_SEND_GYRO]) { - mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "GYRO", get_boot_time_us(), x_rate, y_rate, z_rate); + mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "GYRO", get_boot_time_us(), pixel_flow_x, y_rate, z_rate); } integration_timespan = 0; diff --git a/src/settings.c b/src/settings.c index d367d5b..620ad40 100644 --- a/src/settings.c +++ b/src/settings.c @@ -183,7 +183,7 @@ void global_data_reset_param_defaults(void){ global_data.param_access[PARAM_BOTTOM_FLOW_HIST_FILTER] = READ_WRITE; // global_data.param[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION] = 0; - global_data.param[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION] = 0; + global_data.param[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION] = 1; strcpy(global_data.param_name[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION], "BFLOW_GYRO_COM"); global_data.param_access[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION] = READ_WRITE; From 078ef6d2a0ec276f6d24f51dd8f164a8be4e36fa Mon Sep 17 00:00:00 2001 From: Severin Latkovic Date: Fri, 3 Jul 2015 16:15:54 +0200 Subject: [PATCH 008/120] -cleanup --- src/flow.c | 18 ++++++++++-------- src/main.c | 5 ++--- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/src/flow.c b/src/flow.c index 70d6218..34663e5 100644 --- a/src/flow.c +++ b/src/flow.c @@ -457,8 +457,8 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat for (ii = winmin; ii <= winmax; ii++) { - uint32_t temp_dist = compute_sad_8x8(image1, image2, i, j, i + ii, j + jj, (uint16_t) global_data.param[PARAM_IMAGE_WIDTH]); -// uint32_t temp_dist = ABSDIFF(base1, base2 + ii); +// uint32_t temp_dist = compute_sad_8x8(image1, image2, i, j, i + ii, j + jj, (uint16_t) global_data.param[PARAM_IMAGE_WIDTH]); + uint32_t temp_dist = ABSDIFF(base1, base2 + ii); if (temp_dist < dist) { sumx = ii; @@ -797,13 +797,13 @@ float meanflowx = 0.0f; float meanflowy = 0.0f; uint16_t meancount = 0; +float chi_sum = 0.0f; +uint8_t chicount = 0; + const float focal_length_px = (global_data.param[PARAM_FOCAL_LENGTH_MM]) / (4.0f * 6.0f) * 1000.0f; float x_rate_pixel = x_rate * (get_time_between_images() / 1000000.0f) * focal_length_px; float y_rate_pixel = y_rate * (get_time_between_images() / 1000000.0f) * focal_length_px; -if(fabs(x_rate_pixel)>3) - x_rate_pixel = 0; -if(fabs(y_rate_pixel)>3) - y_rate_pixel = 0; + int x_rate_px_round = round(x_rate_pixel); int y_rate_px_round = round(y_rate_pixel); @@ -988,7 +988,8 @@ int y_rate_px_round = round(y_rate_pixel); } else break; - + chi_sum += chi_sq; + chicount++; chi_sq_previous = chi_sq; } @@ -1069,5 +1070,6 @@ if (global_data.param[PARAM_USB_SEND_VIDEO] )//&& global_data.param[PARAM_VIDEO_ } /* return quality */ -return (uint8_t)(meancount * 255 / (NUM_BLOCK_KLT*NUM_BLOCK_KLT)); +//return (uint8_t)(meancount * 255 / (NUM_BLOCK_KLT*NUM_BLOCK_KLT)); +return chi_sum/chicount; } \ No newline at end of file diff --git a/src/main.c b/src/main.c index 6b287de..cae5009 100644 --- a/src/main.c +++ b/src/main.c @@ -412,8 +412,8 @@ int main(void) dma_copy_image_buffers(¤t_image, &previous_image, image_size, 1); /* compute optical flow */ -// qual = compute_flow(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y); - qual = compute_klt(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y); + qual = compute_flow(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y); +// qual = compute_klt(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y); /* @@ -441,7 +441,6 @@ int main(void) uint32_t deltatime = (get_boot_time_us() - lasttime); integration_timespan += deltatime; - //TODO -> right to swap? accumulated_flow_x += pixel_flow_y / focal_length_px * 1.0f; //rad axis swapped to align x flow around y axis accumulated_flow_y += pixel_flow_x / focal_length_px * -1.0f;//rad accumulated_gyro_x += x_rate * deltatime / 1000000.0f; //rad From 2501b875af11f5c15448081e7531c54b13e687d0 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Fri, 3 Jul 2015 18:01:33 +0200 Subject: [PATCH 009/120] implemented simple high pass filter to pre-process the images --- Makefile | 1 + inc/filter.h | 45 +++++++++++++++++++++++++++ src/filter.c | 86 ++++++++++++++++++++++++++++++++++++++++++++++++++++ src/main.c | 3 ++ 4 files changed, 135 insertions(+) create mode 100644 inc/filter.h create mode 100644 src/filter.c diff --git a/Makefile b/Makefile index 1d6c15b..77c165d 100644 --- a/Makefile +++ b/Makefile @@ -22,6 +22,7 @@ SRCS = src/main.c \ src/led.c \ src/settings.c \ src/communication.c \ + src/filter.c \ src/flow.c \ src/dcmi.c \ src/mt9v034.c \ diff --git a/inc/filter.h b/inc/filter.h new file mode 100644 index 0000000..a11187e --- /dev/null +++ b/inc/filter.h @@ -0,0 +1,45 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef FILTER_H_ +#define FILTER_H_ + +#include + +/** + * @brief Filters the image to improve the flow processing + * @note It uses the space after the image in the image buffer to work on the image. + */ +void filter_image(uint8_t *image, uint16_t width); + +#endif /* FILTER_H_ */ diff --git a/src/filter.c b/src/filter.c new file mode 100644 index 0000000..82980af --- /dev/null +++ b/src/filter.c @@ -0,0 +1,86 @@ + +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Petri Tanskanen + * Lorenz Meier + * Samuel Zihlmann + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include +#include + +#define __INLINE inline +#define __ASM asm +#include "core_cm4_simd.h" + +#include "filter.h" + +void filter_image(uint8_t *image, uint16_t width) { + uint16_t ext_width = width + 2; + /* first we make a copy of the image and extend it beyond the edges by 1 pixel */ + uint8_t *img_ext = image + width * width; + #define IMG_PX(x, y) (*(image + width * (y) + (x))) + #define EXT_PX(x, y) (*(img_ext + ext_width * (y) + (x))) + int y, x; + /* copy the lines from the middle: */ + for (y = 0; y < width; y++) { + memcpy(&EXT_PX(1, y + 1), &IMG_PX(0, y), width); + } + /* top and bottom: */ + memcpy(&EXT_PX(1, 0), &IMG_PX(0, 0), width); + memcpy(&EXT_PX(1, width + 1), &IMG_PX(0, width - 1), width); + /* left and right: */ + for (y = 0; y < ext_width; y++) { + EXT_PX(0, y) = EXT_PX(1, y); + EXT_PX(ext_width - 1, y) = EXT_PX(ext_width - 2, y); + } + /* now compute with the following kernel: + * -1 -1 -1 + * -1 8 -1 + * -1 -1 -1 */ + for (y = 0; y < width; y++) { + for (x = 0; x < width; x++) { + int16_t sum = (uint16_t)EXT_PX(x, y + 0) + (uint16_t)EXT_PX(x + 1, y + 0) + (uint16_t)EXT_PX(x + 2, y + 0) + + (uint16_t)EXT_PX(x, y + 1) + + (uint16_t)EXT_PX(x + 2, y + 1) + + (uint16_t)EXT_PX(x, y + 2) + (uint16_t)EXT_PX(x + 1, y + 2) + (uint16_t)EXT_PX(x + 2, y + 2); + sum -= (((uint16_t)EXT_PX(x + 1, y + 1)) << 3); + /* the result will be between -255 * 8 to 255 * 8: */ + /* amplification could be done here. */ + /* clip the result to -128 to 127: */ + if (sum < -128) sum = -128; + else if (sum > 127) sum = 127; + IMG_PX(x, y) = sum + 128; + } + } +} diff --git a/src/main.c b/src/main.c index cae5009..f655850 100644 --- a/src/main.c +++ b/src/main.c @@ -48,6 +48,7 @@ #include "settings.h" #include "utils.h" #include "led.h" +#include "filter.h" #include "flow.h" #include "dcmi.h" #include "mt9v034.h" @@ -411,6 +412,8 @@ int main(void) /* copy recent image to faster ram */ dma_copy_image_buffers(¤t_image, &previous_image, image_size, 1); + /* filter the new image */ + filter_image(current_image, global_data.param[PARAM_IMAGE_WIDTH]); /* compute optical flow */ qual = compute_flow(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y); // qual = compute_klt(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y); From bf1a87ebe307e2e40efcf7097a378d19ef097b61 Mon Sep 17 00:00:00 2001 From: Severin Latkovic Date: Wed, 8 Jul 2015 17:01:10 +0200 Subject: [PATCH 010/120] Added median flow moved quality measurement to new file quality_measurement.c added iqr (interquartile range) quality fixed klt flow image --- Makefile | 1 + inc/flow.h | 3 +- inc/quality_measurement.h | 55 ++++++++++++ inc/settings.h | 4 + src/flow.c | 174 +++++++++++++++++++++++++++++++------ src/main.c | 41 +++++++-- src/quality_measurement.c | 176 ++++++++++++++++++++++++++++++++++++++ src/settings.c | 14 +++ 8 files changed, 432 insertions(+), 36 deletions(-) create mode 100644 inc/quality_measurement.h create mode 100644 src/quality_measurement.c diff --git a/Makefile b/Makefile index 77c165d..37c5f32 100644 --- a/Makefile +++ b/Makefile @@ -23,6 +23,7 @@ SRCS = src/main.c \ src/settings.c \ src/communication.c \ src/filter.c \ + src/quality_measurement.c \ src/flow.c \ src/dcmi.c \ src/mt9v034.c \ diff --git a/inc/flow.h b/inc/flow.h index d182fdd..2b5d055 100644 --- a/inc/flow.h +++ b/inc/flow.h @@ -45,7 +45,6 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat /** * @brief Computes pixel flow from image1 to image2 with KLT method */ -uint8_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate, - float *pixel_flow_x, float *pixel_flow_y); +uint8_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate, float *pixel_flow_x, float *pixel_flow_y); #endif /* FLOW_H_ */ diff --git a/inc/quality_measurement.h b/inc/quality_measurement.h new file mode 100644 index 0000000..b2c74d6 --- /dev/null +++ b/inc/quality_measurement.h @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef QUALITY_H_ +#define QUALITY_H_ + +#include +typedef struct _qual_output { + uint8_t qual; + float mean_x; + float mean_y; + float median_x; + float median_y; + float var; + float covar; + float qual_iqr; +} qual_output; + +/** + * @brief Computes the quality of the flow algorithm using the calculated pixel flow + * @note It uses the space after the image in the image buffer to work on the image. + */ +qual_output quality_new_measurement(float pixel_flow_x, float pixel_flow_y, float dt, float qual_iqr); + +#endif /* QUALITY_H_ */ diff --git a/inc/settings.h b/inc/settings.h index 0139530..67de5f2 100644 --- a/inc/settings.h +++ b/inc/settings.h @@ -130,6 +130,10 @@ enum global_param_id_t PARAM_USB_SEND_FORWARD, PARAM_USB_SEND_DEBUG, + PARAM_QUALITY_FILTER, + PARAM_ALGO_CHOICE, + PARAM_USE_MEDIAN, + PARAM_VIDEO_ONLY, PARAM_VIDEO_RATE, diff --git a/src/flow.c b/src/flow.c index 34663e5..b916712 100644 --- a/src/flow.c +++ b/src/flow.c @@ -768,11 +768,60 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat } /* calc quality */ - uint8_t qual = (uint8_t)(meancount * 255 / (NUM_BLOCKS*NUM_BLOCKS)); + int direct_count[8]; + for(int i = 0; i < 64; i++){ + if(subdirs[i] == 0) + { + direct_count[0]++; + } + if(subdirs[i] == 1) + { + direct_count[1]++; + } + if(subdirs[i] == 2) + { + direct_count[2]++; + } + if(subdirs[i] == 3) + { + direct_count[3]++; + } + if(subdirs[i] == 4) + { + direct_count[4]++; + } + if(subdirs[i] == 5) + { + direct_count[5]++; + } + if(subdirs[i] == 6) + { + direct_count[6]++; + } + if(subdirs[i] == 7) + { + direct_count[7]++; + } + } + int direct_max = 0; + for(int i = 0; i < 8; i++){ + if(direct_count[i]>direct_max) + { + direct_max = direct_count[i]; + } + } + uint8_t qual = (int)direct_max/64*255; + //uint8_t qual = (uint8_t)(meancount * 255 / (NUM_BLOCKS*NUM_BLOCKS)); return qual; } +int floatcomp(const void* elem1, const void* elem2) +{ + if(*(const float*)elem1 < *(const float*)elem2) + return -1; + return *(const float*)elem1 > *(const float*)elem2; +} /** * @brief Computes pixel flow from image1 to image2 @@ -795,6 +844,10 @@ uint16_t i, j; float meanflowx = 0.0f; float meanflowy = 0.0f; +float medianflowx = 0.0f; +float medianflowy = 0.0f; +float flow_x[NUM_BLOCK_KLT*NUM_BLOCK_KLT]; +float flow_y[NUM_BLOCK_KLT*NUM_BLOCK_KLT]; uint16_t meancount = 0; float chi_sum = 0.0f; @@ -852,6 +905,10 @@ int y_rate_px_round = round(y_rate_pixel); uint16_t is[NUM_BLOCK_KLT*NUM_BLOCK_KLT]; uint16_t js[NUM_BLOCK_KLT*NUM_BLOCK_KLT]; + //new reference pixel values after flow for debug + float ref_pixel_new_x[NUM_BLOCK_KLT*NUM_BLOCK_KLT]; + float ref_pixel_new_y[NUM_BLOCK_KLT*NUM_BLOCK_KLT]; + //initialize flow values with the pixel value of the previous image uint16_t pixLo = frame_size / (NUM_BLOCK_KLT + 1); @@ -872,6 +929,8 @@ int y_rate_px_round = round(y_rate_pixel); vs[y*NUM_BLOCK_KLT+x] = j + x_rate_pixel; is[y*NUM_BLOCK_KLT+x] = i; //position in previous image at level 0 js[y*NUM_BLOCK_KLT+x] = j; + ref_pixel_new_x[y*NUM_BLOCK_KLT+x] = i+1; + ref_pixel_new_y[y*NUM_BLOCK_KLT+x] = j+1; } } @@ -1002,43 +1061,91 @@ int y_rate_px_round = round(y_rate_pixel); { float nx = i-u; float ny = j-v; + ref_pixel_new_x[k] = u; + ref_pixel_new_y[k] = v; + flow_x[k] = nx; + flow_y[k] = ny; //TODO: check if patch drifted too far - take number of pyramid levels in to account for that { meanflowx += nx; meanflowy += ny; meancount++; + } } } } -/* compute mean flow */ + + + +/*compute flow */ if (meancount > 0) { + /* compute median flow */ + if(meancount == NUM_BLOCK_KLT*NUM_BLOCK_KLT) + { + qsort(flow_x, 8, sizeof (float), floatcomp); + qsort(flow_y, 8, sizeof (float), floatcomp); + + medianflowx = (flow_x[3]+flow_x[4])/2; + medianflowy = (flow_y[3]+flow_y[4])/2; + } + else + { + medianflowx = meanflowx / meancount; + medianflowy = meanflowy / meancount; + } + + /* compute mean flow */ meanflowx /= meancount; meanflowy /= meancount; - if (global_data.param[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION]) + if(global_data.param[PARAM_USE_MEDIAN] == 0) { - if(fabsf(x_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD]) - *pixel_flow_x = meanflowx - y_rate_pixel; // + (x_rate_px_round - x_rate_pixel); - else - *pixel_flow_x = meanflowx; + if (global_data.param[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION]) + { + if(fabsf(x_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD]) + *pixel_flow_x = meanflowx - y_rate_pixel; // + (x_rate_px_round - x_rate_pixel); + else + *pixel_flow_x = meanflowx; - if(fabsf(y_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD]) - *pixel_flow_y = meanflowy + x_rate_pixel; // + (y_rate_px_round - y_rate_pixel); - else - *pixel_flow_y = meanflowy; + if(fabsf(y_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD]) + *pixel_flow_y = meanflowy + x_rate_pixel; // + (y_rate_px_round - y_rate_pixel); + else + *pixel_flow_y = meanflowy; - } - else - { - /* without gyro compensation */ - *pixel_flow_x = meanflowx; - *pixel_flow_y = meanflowy; + } + else + { + /* without gyro compensation */ + *pixel_flow_x = meanflowx; + *pixel_flow_y = meanflowy; + } } + else{ + if (global_data.param[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION]) + { + if(fabsf(x_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD]) + *pixel_flow_x = medianflowx - y_rate_pixel; // + (x_rate_px_round - x_rate_pixel); + else + *pixel_flow_x = medianflowx; + + if(fabsf(y_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD]) + *pixel_flow_y = medianflowy + x_rate_pixel; // + (y_rate_px_round - y_rate_pixel); + else + *pixel_flow_y = medianflowy; + + } + else + { + /* without gyro compensation */ + *pixel_flow_x = medianflowx; + *pixel_flow_y = medianflowy; + } + } } else { @@ -1052,24 +1159,39 @@ else */ if (global_data.param[PARAM_USB_SEND_VIDEO] )//&& global_data.param[PARAM_VIDEO_USB_MODE] == FLOW_VIDEO) { - - for (j = pixLo; j < pixHi; j += pixStep) + for (j = pixLo; j <= pixHi; j += pixStep) { - for (i = pixLo; i < pixHi; i += pixStep) + for (i = pixLo; i <= pixHi; i += pixStep) { image1[j * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i] = 255; - int pixel = (j - (int)(meanflowx*4+0.5f)) * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + (i - (int)(meanflowy*4+0.5f)); - if(pixel >= 0 && pixel < global_data.param[PARAM_IMAGE_WIDTH]*global_data.param[PARAM_IMAGE_WIDTH]) - image1[pixel] = 200; - } } + + + for(i = 0; i < NUM_BLOCK_KLT*NUM_BLOCK_KLT; i++){ + int pixel = ((int)ref_pixel_new_x[i]) * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + (int)ref_pixel_new_y[i]; + if(pixel >= 0 && pixel < global_data.param[PARAM_IMAGE_WIDTH]*global_data.param[PARAM_IMAGE_WIDTH]) + image1[pixel] = 200; + } } /* return quality */ //return (uint8_t)(meancount * 255 / (NUM_BLOCK_KLT*NUM_BLOCK_KLT)); -return chi_sum/chicount; -} \ No newline at end of file +if(meancount == NUM_BLOCK_KLT*NUM_BLOCK_KLT) +{ + + float iqr_x = flow_x[5] - flow_x[2]; + float iqr_y = flow_y[5] - flow_y[2]; + float qual = sqrt((iqr_y * iqr_y) + (iqr_x * iqr_x)) / sqrt(2 * (PATCH_SIZE*PATCH_SIZE)); + return qual; +} +else +{ + return 0; +} + +} + diff --git a/src/main.c b/src/main.c index f655850..2abed81 100644 --- a/src/main.c +++ b/src/main.c @@ -49,6 +49,7 @@ #include "utils.h" #include "led.h" #include "filter.h" +#include "quality_measurement.h" #include "flow.h" #include "dcmi.h" #include "mt9v034.h" @@ -300,14 +301,18 @@ int main(void) /* variables */ uint32_t counter = 0; uint8_t qual = 0; - uint8_t qual1 = 0; - uint8_t qual2 = 0; + float qual_iqr = 0.0f; + float accumulated_iqr = 0.0f; + float accumulated_var = 0.0f; + float accumulated_mean_y = 0.0f; + + static qual_output qual_output_point = {}; + float qual_time = 0.0f; + /* bottom flow variables */ float pixel_flow_x = 0.0f; float pixel_flow_y = 0.0f; - float pixel_flow_klt_x = 0.0f; - float pixel_flow_klt_y = 0.0f; float pixel_flow_x_sum = 0.0f; float pixel_flow_y_sum = 0.0f; @@ -410,14 +415,28 @@ int main(void) if(global_data.param[PARAM_SENSOR_POSITION] == BOTTOM) { /* copy recent image to faster ram */ - dma_copy_image_buffers(¤t_image, &previous_image, image_size, 1); + dma_copy_image_buffers(¤t_image, &previous_image, image_size, 2); /* filter the new image */ - filter_image(current_image, global_data.param[PARAM_IMAGE_WIDTH]); + //filter_image(current_image, global_data.param[PARAM_IMAGE_WIDTH]); /* compute optical flow */ - qual = compute_flow(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y); -// qual = compute_klt(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y); + if(global_data.param[PARAM_ALGO_CHOICE] == 0) + { + qual = compute_flow(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y); + }else + { + qual_iqr = compute_klt(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y); + } + + accumulated_iqr += qual_iqr; + + qual_time = get_boot_time_us(); + qual_output_point = quality_new_measurement(pixel_flow_x, pixel_flow_y, get_time_between_images(), qual_iqr); + qual_time = qual_time - get_boot_time_us(); + + accumulated_var += qual_output_point.var; + accumulated_mean_y += qual_output_point.mean_y; /* * real point P (X,Y,Z), image plane projection p (x,y,z), focal-length f, distance-to-scene Z @@ -582,6 +601,8 @@ int main(void) accumulated_gyro_x, accumulated_gyro_y, accumulated_gyro_z, gyro_temp, accumulated_quality/accumulated_framecount, time_since_last_sonar_update,ground_distance); + + mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "QUALITY", get_boot_time_us(), accumulated_mean_y, accumulated_iqr, accumulated_var); } @@ -599,6 +620,10 @@ int main(void) accumulated_gyro_y = 0; accumulated_gyro_z = 0; + accumulated_iqr = 0; + accumulated_var = 0; + accumulated_mean_y = 0; + velocity_x_sum = 0.0f; velocity_y_sum = 0.0f; pixel_flow_x_sum = 0.0f; diff --git a/src/quality_measurement.c b/src/quality_measurement.c new file mode 100644 index 0000000..0e495af --- /dev/null +++ b/src/quality_measurement.c @@ -0,0 +1,176 @@ + +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Petri Tanskanen + * Lorenz Meier + * Samuel Zihlmann + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include + +#include "settings.h" + +#define __INLINE inline +#define __ASM asm +#include "core_cm4_simd.h" + +#include "quality_measurement.h" + +#define QUEUE_SIZE 80 + +typedef struct _qual_datapoint { + float x; + float y; + float x_orig; + float y_orig; + float qual_iqr; +} qual_datapoint; + +static qual_datapoint in = {}; +static qual_datapoint arr[QUEUE_SIZE] = {}; +static int head = 0; //starting index of the queue + +static qual_datapoint input_flow_filter_state = {}; + +static qual_output qual_output_point = {}; + +static void enqueue(qual_datapoint *q, int *head, qual_datapoint data) { + q[(*head)] = data; + (*head)++; + if((*head) >= QUEUE_SIZE) + (*head) = 0; +} + +int floatcompare(const void* elem1, const void* elem2) +{ + if(*(const float*)elem1 < *(const float*)elem2) + return -1; + return *(const float*)elem1 > *(const float*)elem2; +} + +static struct _qual_output quality_calculate(){ + float sum_flow_x = 0; + float sum_flow_y = 0; + float sum2_flow_x = 0; + float sum2_flow_y = 0; + float mean_qual_iqr = 0; + float sum_qual_iqr = 0; + float x_array[QUEUE_SIZE] = {}; + float y_array[QUEUE_SIZE] = {}; + + for(int i = 0; i < QUEUE_SIZE; i++) + { + x_array[i] = arr[i].x; + y_array[i] = arr[i].y; + } + + //calculate median of flow + qsort(x_array, QUEUE_SIZE, sizeof (float), floatcompare); + qsort(y_array, QUEUE_SIZE, sizeof (float), floatcompare); + if(QUEUE_SIZE % 2 == 0) + { + qual_output_point.median_x = (x_array[QUEUE_SIZE / 2 - 1] + x_array[QUEUE_SIZE / 2]) / 2; + qual_output_point.median_y = (x_array[QUEUE_SIZE / 2 - 1] + x_array[QUEUE_SIZE / 2]) / 2; + } + else + { + qual_output_point.median_x = x_array[(QUEUE_SIZE - 1) / 2]; + qual_output_point.median_y = x_array[(QUEUE_SIZE - 1) / 2]; + } + + //calculate mean + for(int i = 0; i < QUEUE_SIZE; i++){ + sum_flow_x += arr[i].x; + sum_flow_y += arr[i].y; + sum_qual_iqr += arr[i].qual_iqr; + } + qual_output_point.mean_x = sum_flow_x/QUEUE_SIZE; + qual_output_point.mean_y = sum_flow_y/QUEUE_SIZE; + mean_qual_iqr = sum_qual_iqr/QUEUE_SIZE; + + //calculate variance + for(int i = 0; i < QUEUE_SIZE; i++){ + sum2_flow_x += (arr[i].x - qual_output_point.mean_x) * (arr[i].x - qual_output_point.mean_x); + sum2_flow_y += (arr[i].y - qual_output_point.mean_y) * (arr[i].y - qual_output_point.mean_y); + } + qual_output_point.var = ((sum2_flow_x + sum2_flow_y) / QUEUE_SIZE) / (1.0 + sqrt(in.x_orig * in.x_orig + + in.y_orig * in.y_orig)); + + //calculate covariance between flow_y and flow_x + float total = 0.0f; + for(int i = 0; i < QUEUE_SIZE; i++){ + total += (arr[i].x - qual_output_point.mean_x) * (arr[i].y - qual_output_point.mean_y); + } + qual_output_point.covar = total/QUEUE_SIZE; + + qual_output_point.qual = (int)sum_flow_y/QUEUE_SIZE; + qual_output_point.qual_iqr = in.qual_iqr; + + return qual_output_point; +} + +#define FILTER_TAU 4910 + +qual_output quality_new_measurement(float pixel_flow_x, float pixel_flow_y, float dt, float qual_iqr) { + + // high pass filter with exponential filter: + in.x_orig = pixel_flow_x; + in.y_orig = pixel_flow_y; + if(global_data.param[PARAM_QUALITY_FILTER] > 0) + { + float alpha = 1.0 - exp(- dt / FILTER_TAU); + input_flow_filter_state.x = alpha*(pixel_flow_x/dt) + (1.0 - alpha)*input_flow_filter_state.x; + input_flow_filter_state.y = alpha*(pixel_flow_y/dt) + (1.0 - alpha)*input_flow_filter_state.y; + in.x = pixel_flow_x - input_flow_filter_state.x; + in.y = pixel_flow_y - input_flow_filter_state.y; + in.qual_iqr = qual_iqr; + enqueue(arr, &head, in); + } + else + { + in.x = pixel_flow_x/dt; + in.y = pixel_flow_y/dt; + in.qual_iqr = qual_iqr; + enqueue(arr, &head, in); + } + + // queue into circular buffer: + return quality_calculate(); +} + + diff --git a/src/settings.c b/src/settings.c index 620ad40..c2988bd 100644 --- a/src/settings.c +++ b/src/settings.c @@ -199,10 +199,24 @@ void global_data_reset_param_defaults(void){ strcpy(global_data.param_name[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR], "BFLOW_THROTT"); global_data.param_access[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR] = READ_WRITE; + global_data.param[PARAM_QUALITY_FILTER] = 1; + strcpy(global_data.param_name[PARAM_QUALITY_FILTER], "QUALITY_FILTER"); + global_data.param_access[PARAM_QUALITY_FILTER] = READ_WRITE; + + global_data.param[PARAM_ALGO_CHOICE] = 1; + strcpy(global_data.param_name[PARAM_ALGO_CHOICE], "ALGO_CHOICE"); + global_data.param_access[PARAM_ALGO_CHOICE] = READ_WRITE; + + global_data.param[PARAM_USE_MEDIAN] = 1; + strcpy(global_data.param_name[PARAM_USE_MEDIAN], "USE_MEDIAN"); + global_data.param_access[PARAM_USE_MEDIAN] = READ_WRITE; + global_data.param[DEBUG_VARIABLE] = 1; strcpy(global_data.param_name[DEBUG_VARIABLE], "DEBUG"); global_data.param_access[DEBUG_VARIABLE] = READ_WRITE; + + } /** From 0ae3f70bdc50dfecce1795d80300041c9b9a4534 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Thu, 9 Jul 2015 17:13:17 +0200 Subject: [PATCH 011/120] move timer functions and flow result accumulation functions into separate modules. Not tested yet. --- Makefile | 2 + inc/i2c.h | 6 +- inc/main.h | 6 +- inc/result_accumulator.h | 146 +++++++++ inc/settings.h | 4 + inc/timer.h | 82 +++++ lib/core_cmInstr.h | 14 +- src/communication.c | 2 +- src/dcmi.c | 9 +- src/i2c.c | 145 ++++----- src/main.c | 627 +++++++++++---------------------------- src/result_accumulator.c | 278 +++++++++++++++++ src/settings.c | 14 + src/stm32f4xx_it.c | 1 + src/timer.c | 197 ++++++++++++ 15 files changed, 976 insertions(+), 557 deletions(-) create mode 100644 inc/result_accumulator.h create mode 100644 inc/timer.h create mode 100644 src/result_accumulator.c create mode 100644 src/timer.c diff --git a/Makefile b/Makefile index 77c165d..32de070 100644 --- a/Makefile +++ b/Makefile @@ -18,6 +18,8 @@ CC = arm-none-eabi-gcc OBJCOPY = arm-none-eabi-objcopy SRCS = src/main.c \ + src/timer.c \ + src/result_accumulator.c \ src/utils.c \ src/led.c \ src/settings.c \ diff --git a/inc/i2c.h b/inc/i2c.h index d3c3f14..eeb544a 100644 --- a/inc/i2c.h +++ b/inc/i2c.h @@ -41,6 +41,7 @@ #ifndef I2C_H_ #define I2C_H_ #include +#include #define I2C1_OWNADDRESS_1_BASE 0x42 //7bit base address /** @@ -48,8 +49,9 @@ */ void i2c_init(void); -void update_TX_buffer(float pixel_flow_x, float pixel_flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t qual, - float ground_distance, float x_rate, float y_rate, float z_rate, int16_t gyro_temp); +void update_TX_buffer(float 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); #endif /* I2C_H_ */ diff --git a/inc/main.h b/inc/main.h index 2abc9e7..7e7f796 100644 --- a/inc/main.h +++ b/inc/main.h @@ -35,10 +35,6 @@ #define __PX4_FLOWBOARD_H extern uint32_t get_time_between_images(void); - -void timer_update(void); -void timer_update_ms(void); -uint32_t get_boot_time_ms(void); -uint32_t get_boot_time_us(void); +void buffer_reset(void); #endif /* __PX4_FLOWBOARD_H */ diff --git a/inc/result_accumulator.h b/inc/result_accumulator.h new file mode 100644 index 0000000..8d85934 --- /dev/null +++ b/inc/result_accumulator.h @@ -0,0 +1,146 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef RESULT_ACCUMULATOR_H_ +#define RESULT_ACCUMULATOR_H_ + +#include +#include + + +typedef struct _result_accumulator_ctx { + struct _last { + uint32_t frame_count; ///< Frame counter. + float dt; ///< The time delta of 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) + int16_t temperature; ///< Temperature * 100 in centi-degrees Celsius + uint8_t qual; ///< The quality output of the flow algorithm. + float pixel_flow_x; ///< The measured x-flow in the current image in pixel. Sensor linear motion along the positive X axis induces a negative flow. + float pixel_flow_y; ///< The measured y-flow in the current image in pixel. Sensor linear motion along the positive Y axis induces a negative flow. + float flow_x_rad; ///< Flow in radians around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) + float flow_y_rad; ///< Flow in radians around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) + 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; + uint8_t min_quality; + uint16_t valid_data_count; + uint32_t valid_time; + uint32_t full_time; + float gyro_x_accu; + float gyro_y_accu; + float gyro_z_accu; +} result_accumulator_ctx; + + +typedef struct _result_accumulator_output_flow { + int16_t flow_x; ///< Flow in pixels * 10 in x-sensor direction (dezi-pixels) + int16_t flow_y; ///< Flow in pixels * 10 in y-sensor direction (dezi-pixels) + float flow_comp_m_x;///< Flow in meters in x-sensor direction, angular-speed compensated + float flow_comp_m_y;///< Flow in meters in y-sensor direction, angular-speed compensated + uint8_t quality; ///< Optical flow quality / confidence. 0: bad, 255: maximum quality + float ground_distance; ///< Ground distance in meters. Positive value: distance known. Negative value: Unknown distance +} result_accumulator_output_flow; + +typedef struct _result_accumulator_output_flow_rad { + uint32_t integration_time; ///< Integration time in microseconds. Divide integrated_x and integrated_y by the integration time to obtain average flow. + float integrated_x; ///< Flow in radians around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) + float integrated_y; ///< Flow in radians around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) + float integrated_xgyro; ///< RH rotation around X axis (rad) + float integrated_ygyro; ///< RH rotation around X axis (rad) + float integrated_zgyro; ///< RH rotation around X axis (rad) + uint8_t quality; ///< Optical flow quality / confidence. 0: bad, 255: maximum quality + int16_t temperature; ///< Temperature * 100 in centi-degrees Celsius + uint32_t time_delta_distance_us; ///< Time in microseconds since the distance was sampled. + float ground_distance; ///< Distance to the center of the flow field in meters. Positive value (including zero): distance known. Negative value: Unknown distance. +} result_accumulator_output_flow_rad; + + +typedef struct _result_accumulator_output_flow_i2c { + uint32_t frame_count; ///< Frame counter. + uint16_t valid_frames; ///< Number of valid frames. + uint32_t integration_time; ///< Integration time in microseconds. + int16_t rad_flow_x; ///< Flow in 10 * mrad around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) + int16_t rad_flow_y; ///< Flow in 10 * mrad around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) + int16_t integrated_gyro_x; ///< RH rotation around X axis (10 * mrad) + int16_t integrated_gyro_y; ///< RH rotation around Y axis (10 * mrad) + int16_t integrated_gyro_z; ///< RH rotation around Z axis (10 * mrad) + int16_t gyro_x; ///< RH rotation around X axis (10 * mrad) + int16_t gyro_y; ///< RH rotation around Y axis (10 * mrad) + int16_t gyro_z; ///< RH rotation around Z axis (10 * mrad) + uint8_t quality; ///< Optical flow quality / confidence. 0: bad, 255: maximum quality + int16_t temperature; ///< Temperature * 100 in centi-degrees Celsius + uint32_t time_delta_distance_us; ///< Time in microseconds since the distance was sampled. + uint16_t ground_distance; ///< Distance to the center of the flow field in mmeters. +} result_accumulator_output_flow_i2c; + +/** Initializes the result accumulator. + */ +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 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) + * @param gyro_temp The current gyro temperature in centi-degrees Celsius + * @param qual The quality output of the flow algorithm. + * @param pixel_flow_x The measured x-flow in the current image in pixel. + * @param pixel_flow_y The measured y-flow in the current image in pixel. + * @param rad_per_pixel Pixel to radian conversion factor. + * @param distance_valid True when the distance sensor reports that its distance is valid. + * @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, + 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); + +/** Recalculates the output values of the result_accumulator. Call this before using any of the output values. + */ +void result_accumulator_calculate_output_flow(result_accumulator_ctx *ctx, uint16_t min_valid_data_count, result_accumulator_output_flow *out); +void result_accumulator_calculate_output_flow_rad(result_accumulator_ctx *ctx, uint16_t min_valid_data_count, result_accumulator_output_flow_rad *out); +void result_accumulator_calculate_output_flow_i2c(result_accumulator_ctx *ctx, uint16_t min_valid_data_count, result_accumulator_output_flow_i2c *out); + +/** Resets the accumulator to prepare it for the next accumulation phase. + */ +void result_accumulator_reset(result_accumulator_ctx *ctx); + +#endif /* RESULT_ACCUMULATOR_H_ */ diff --git a/inc/settings.h b/inc/settings.h index 0139530..e8525de 100644 --- a/inc/settings.h +++ b/inc/settings.h @@ -109,6 +109,10 @@ enum global_param_id_t PARAM_SYSTEM_SEND_STATE, PARAM_SYSTEM_SEND_LPOS, + PARAM_ALGORITHM_CHOICE, + PARAM_USE_IMAGE_FILTER, + PARAM_CAMERA_FRAME_INTERVAL, + PARAM_USART2_BAUD, PARAM_USART3_BAUD, PARAM_FOCAL_LENGTH_MM, diff --git a/inc/timer.h b/inc/timer.h new file mode 100644 index 0000000..4e61b84 --- /dev/null +++ b/inc/timer.h @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef TIMER_H_ +#define TIMER_H_ + +#include + +#define NTIMERS 16 + +/** Initializes the timer module. + */ +void timer_init(void); + +/** Registers a new timer with a corresponding function. + * @note: The function will be called from within the timer_check function. + * @param timer_fn The timer function to call when the timer rolls over. + * This function is NOT called from the interrupt handler. + * @param period_ms The period of the timer in milliseconds. + */ +void timer_register(void (*timer_fn)(void), uint32_t period_ms); + +/** Checks any pending timers and calls the respective timer functions. + */ +void timer_check(void); + +void delay(uint16_t ms); + +/** Returns the number of milliseconds since booting. + */ +uint32_t get_boot_time_ms(void); + +/** Returns the number of microseconds since booting. + */ +uint32_t get_boot_time_us(void); + +/** Computes the time delta in microseconds while taking the roll-over into account. + */ +uint32_t calculate_time_delta_us(uint32_t end, uint32_t start); + +/** Computes the time delta in microseconds while taking the roll-over into account. + */ +uint32_t get_time_delta_us(uint32_t start); + +/** + * @brief Triggered by systen timer interrupt every millisecond. + * @param None + * @retval None + */ +void timer_update(void); + +#endif /* TIMER_H_ */ diff --git a/lib/core_cmInstr.h b/lib/core_cmInstr.h index ceb4f87..17250fb 100644 --- a/lib/core_cmInstr.h +++ b/lib/core_cmInstr.h @@ -463,9 +463,11 @@ __attribute__( ( always_inline ) ) static __INLINE uint32_t __LDREXW(volatile ui */ __attribute__( ( always_inline ) ) static __INLINE uint32_t __STREXB(uint8_t value, volatile uint8_t *addr) { - uint32_t result; + register uint32_t result asm ("r0"); + register uint8_t value_r asm ("r1") = value; + register volatile uint8_t *addr_r asm ("r2") = addr; - __ASM volatile ("strexb %0, %2, [%1]" : "=r" (result) : "r" (addr), "r" (value) ); + __ASM volatile ("strexb %0, %2, [%1]" : "=r" (result) : "r" (addr_r), "r" (value_r) ); return(result); } @@ -482,8 +484,10 @@ __attribute__( ( always_inline ) ) static __INLINE uint32_t __STREXB(uint8_t val __attribute__( ( always_inline ) ) static __INLINE uint32_t __STREXH(uint16_t value, volatile uint16_t *addr) { uint32_t result; + register uint16_t value_r asm ("r1") = value; + register volatile uint16_t *addr_r asm ("r2") = addr; - __ASM volatile ("strexh %0, %2, [%1]" : "=r" (result) : "r" (addr), "r" (value) ); + __ASM volatile ("strexh %0, %2, [%1]" : "=r" (result) : "r" (addr_r), "r" (value_r) ); return(result); } @@ -500,8 +504,10 @@ __attribute__( ( always_inline ) ) static __INLINE uint32_t __STREXH(uint16_t va __attribute__( ( always_inline ) ) static __INLINE uint32_t __STREXW(uint32_t value, volatile uint32_t *addr) { uint32_t result; + register uint32_t value_r asm ("r1") = value; + register volatile uint32_t *addr_r asm ("r2") = addr; - __ASM volatile ("strex %0, %2, [%1]" : "=r" (result) : "r" (addr), "r" (value) ); + __ASM volatile ("strex %0, %2, [%1]" : "=r" (result) : "r" (addr_r), "r" (value_r) ); return(result); } diff --git a/src/communication.c b/src/communication.c index c140f1e..d7ea1a3 100644 --- a/src/communication.c +++ b/src/communication.c @@ -46,8 +46,8 @@ #include "gyro.h" #include "debug.h" #include "communication.h" +#include "timer.h" -extern uint32_t get_boot_time_us(void); extern void buffer_reset(void); extern void systemreset(bool to_bootloader); diff --git a/src/dcmi.c b/src/dcmi.c index 53868e6..fadcc02 100644 --- a/src/dcmi.c +++ b/src/dcmi.c @@ -39,6 +39,7 @@ #include #include "utils.h" #include "dcmi.h" +#include "timer.h" #include "stm32f4xx_gpio.h" #include "stm32f4xx_rcc.h" #include "stm32f4xx_i2c.h" @@ -72,10 +73,6 @@ uint8_t dcmi_image_buffer_8bit_3[FULL_IMAGE_SIZE]; uint32_t time_between_images; -/* extern functions */ -extern uint32_t get_boot_time_us(void); -extern void delay(unsigned msec); - /** * @brief Initialize DCMI DMA and enable image capturing */ @@ -202,8 +199,8 @@ void dma_swap_buffers(void) } /* set next time_between_images */ - cycle_time = get_boot_time_us() - time_last_frame; - time_last_frame = get_boot_time_us(); + cycle_time = get_time_delta_us(time_last_frame); + time_last_frame += cycle_time; if(image_counter) // image was not fetched jet { diff --git a/src/i2c.c b/src/i2c.c index 61e6084..6f63185 100644 --- a/src/i2c.c +++ b/src/i2c.c @@ -49,6 +49,7 @@ #include "gyro.h" #include "sonar.h" #include "main.h" +#include "result_accumulator.h" #include "mavlink_bridge_header.h" #include @@ -221,103 +222,70 @@ void I2C1_ER_IRQHandler(void) { } } -void update_TX_buffer(float pixel_flow_x, float pixel_flow_y, - float flow_comp_m_x, float flow_comp_m_y, uint8_t qual, - float ground_distance, float gyro_x_rate, float gyro_y_rate, - float gyro_z_rate, int16_t gyro_temp) { - static uint16_t frame_count = 0; +void update_TX_buffer(float 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; + static bool initialized = false; + + if (!initialized) { + result_accumulator_init(&accumulator); + initialized = true; + } + + /* reset if readout has been performed */ + if (stop_accumulation == 1) { + result_accumulator_reset(&accumulator); + stop_accumulation = 0; + } + + /* feed the accumulator and recalculate */ + result_accumulator_feed(&accumulator, 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); + + result_accumulator_output_flow output_flow; + result_accumulator_output_flow_i2c output_i2c; + result_accumulator_calculate_output_flow(&accumulator, 1, &output_flow); + result_accumulator_calculate_output_flow_i2c(&accumulator, 1, &output_i2c); i2c_frame f; i2c_integral_frame f_integral; - f.frame_count = frame_count; - f.pixel_flow_x_sum = pixel_flow_x * 10.0f; - f.pixel_flow_y_sum = pixel_flow_y * 10.0f; - f.flow_comp_m_x = flow_comp_m_x * 1000; - f.flow_comp_m_y = flow_comp_m_y * 1000; - f.qual = qual; - f.ground_distance = ground_distance * 1000; - - f.gyro_x_rate = gyro_x_rate * getGyroScalingFactor(); - f.gyro_y_rate = gyro_y_rate * getGyroScalingFactor(); - f.gyro_z_rate = gyro_z_rate * getGyroScalingFactor(); + /* write the i2c_frame */ + f.frame_count = accumulator.last.frame_count; + f.pixel_flow_x_sum = output_flow.flow_x; + f.pixel_flow_y_sum = output_flow.flow_y; + f.flow_comp_m_x = floor(output_flow.flow_comp_m_x * 1000.0f + 0.5f); + f.flow_comp_m_y = floor(output_flow.flow_comp_m_y * 1000.0f + 0.5f); + f.qual = output_flow.quality; + f.ground_distance = output_i2c.ground_distance; + + f.gyro_x_rate = output_i2c.gyro_x; + f.gyro_y_rate = output_i2c.gyro_y; + f.gyro_z_rate = output_i2c.gyro_z; f.gyro_range = getGyroRange(); - uint32_t time_since_last_sonar_update; - - time_since_last_sonar_update = (get_boot_time_us() - - get_sonar_measure_time()); - - if (time_since_last_sonar_update < 255 * 1000) { - f.sonar_timestamp = time_since_last_sonar_update / 1000; //convert to ms + if (output_i2c.time_delta_distance_us < 255 * 1000) { + f.sonar_timestamp = output_i2c.time_delta_distance_us / 1000; //convert to ms } else { f.sonar_timestamp = 255; } - static float accumulated_flow_x = 0; - static float accumulated_flow_y = 0; - static uint16_t accumulated_framecount = 0; - static uint16_t accumulated_quality = 0; - static float accumulated_gyro_x = 0; - static float accumulated_gyro_y = 0; - static float accumulated_gyro_z = 0; - static uint32_t integration_timespan = 0; - static uint32_t lasttime = 0; - - /* 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 - - // reset if readout has been performed - if (stop_accumulation == 1) { - - //debug output -// mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_us(), -// global_data.param[PARAM_SENSOR_ID], accumulated_flow_x * 10.0f, -// accumulated_gyro_x * 10.0f, integration_timespan, -// accumulated_framecount, (uint8_t) (accumulated_quality / accumulated_framecount), ground_distance); - - integration_timespan = 0; - accumulated_flow_x = 0; //mrad - accumulated_flow_y = 0; //mrad - accumulated_framecount = 0; - accumulated_quality = 0; - accumulated_gyro_x = 0; //mrad - accumulated_gyro_y = 0; //mrad - accumulated_gyro_z = 0; //mrad - stop_accumulation = 0; - } - - //accumulate flow and gyro values between sucessive I2C readings - //update only if qual !=0 - if (qual > 0) { - uint32_t deltatime = (get_boot_time_us() - lasttime); - integration_timespan += deltatime; - accumulated_flow_x += pixel_flow_y * 1000.0f / focal_length_px * 1.0f;//mrad axis swapped to align x flow around y axis - accumulated_flow_y += pixel_flow_x * 1000.0f / focal_length_px * -1.0f; //mrad - accumulated_framecount++; - accumulated_quality += qual; - accumulated_gyro_x += gyro_x_rate * deltatime * 0.001f; //mrad gyro_x_rate * 1000.0f*deltatime/1000000.0f; - accumulated_gyro_y += gyro_y_rate * deltatime * 0.001f; //mrad - accumulated_gyro_z += gyro_z_rate * deltatime * 0.001f; //mrad - } - - //update lasttime - lasttime = get_boot_time_us(); - - f_integral.frame_count_since_last_readout = accumulated_framecount; - f_integral.gyro_x_rate_integral = accumulated_gyro_x * 10.0f; //mrad*10 - f_integral.gyro_y_rate_integral = accumulated_gyro_y * 10.0f; //mrad*10 - f_integral.gyro_z_rate_integral = accumulated_gyro_z * 10.0f; //mrad*10 - f_integral.pixel_flow_x_integral = accumulated_flow_x * 10.0f; //mrad*10 - f_integral.pixel_flow_y_integral = accumulated_flow_y * 10.0f; //mrad*10 - f_integral.integration_timespan = integration_timespan; //microseconds - f_integral.ground_distance = ground_distance * 1000; //mmeters - f_integral.sonar_timestamp = time_since_last_sonar_update; //microseconds - f_integral.qual = - (uint8_t) (accumulated_quality / accumulated_framecount); //0-255 linear quality measurement 0=bad, 255=best - f_integral.gyro_temperature = gyro_temp;//Temperature * 100 in centi-degrees Celsius - + /* write the i2c_integral_frame */ + f_integral.frame_count_since_last_readout = output_i2c.valid_frames; + f_integral.gyro_x_rate_integral = output_i2c.integrated_gyro_x; //mrad*10 + f_integral.gyro_y_rate_integral = output_i2c.integrated_gyro_y; //mrad*10 + f_integral.gyro_z_rate_integral = output_i2c.integrated_gyro_z; //mrad*10 + f_integral.pixel_flow_x_integral = output_i2c.rad_flow_x; //mrad*10 + f_integral.pixel_flow_y_integral = output_i2c.rad_flow_y; //mrad*10 + f_integral.integration_timespan = output_i2c.integration_time; //microseconds + f_integral.ground_distance = output_i2c.ground_distance; //mmeters + f_integral.sonar_timestamp = output_i2c.time_delta_distance_us; //microseconds + f_integral.qual = output_i2c.quality; //0-255 linear quality measurement 0=bad, 255=best + f_integral.gyro_temperature = output_i2c.temperature; //Temperature * 100 in centi-degrees Celsius + + /* perform buffer swapping */ notpublishedIndexFrame1 = 1 - publishedIndexFrame1; // choose not the current published 1 buffer notpublishedIndexFrame2 = 1 - publishedIndexFrame2; // choose not the current published 2 buffer @@ -338,9 +306,6 @@ void update_TX_buffer(float pixel_flow_x, float pixel_flow_y, if (readout_done_frame2) { publishedIndexFrame2 = 1 - publishedIndexFrame2; } - - frame_count++; - } char readI2CAddressOffset(void) { diff --git a/src/main.c b/src/main.c index f655850..6f9054f 100644 --- a/src/main.c +++ b/src/main.c @@ -49,7 +49,9 @@ #include "utils.h" #include "led.h" #include "filter.h" +#include "result_accumulator.h" #include "flow.h" +#include "timer.h" #include "dcmi.h" #include "mt9v034.h" #include "gyro.h" @@ -71,7 +73,6 @@ /* prototypes */ -void delay(unsigned msec); void buffer_reset(void); __ALIGN_BEGIN USB_OTG_CORE_HANDLE USB_OTG_dev __ALIGN_END; @@ -81,38 +82,14 @@ uint8_t* image_buffer_8bit_1 = ((uint8_t*) 0x10000000); uint8_t* image_buffer_8bit_2 = ((uint8_t*) ( 0x10000000 | FULL_IMAGE_SIZE )); uint8_t buffer_reset_needed; -/* boot time in milliseconds ticks */ -volatile uint32_t boot_time_ms = 0; -/* boot time in 10 microseconds ticks */ -volatile uint32_t boot_time10_us = 0; - /* timer constants */ -#define NTIMERS 9 -#define TIMER_CIN 0 -#define TIMER_LED 1 -#define TIMER_DELAY 2 -#define TIMER_SONAR 3 -#define TIMER_SYSTEM_STATE 4 -#define TIMER_RECEIVE 5 -#define TIMER_PARAMS 6 -#define TIMER_IMAGE 7 -#define TIMER_LPOS 8 -#define MS_TIMER_COUNT 100 /* steps in 10 microseconds ticks */ -#define LED_TIMER_COUNT 500 /* steps in milliseconds ticks */ -#define SONAR_TIMER_COUNT 100 /* steps in milliseconds ticks */ -#define SYSTEM_STATE_COUNT 1000/* steps in milliseconds ticks */ -#define PARAMS_COUNT 100 /* steps in milliseconds ticks */ +#define SONAR_POLL_MS 100 /* steps in milliseconds ticks */ +#define SYSTEM_STATE_MS 1000/* steps in milliseconds ticks */ +#define PARAMS_MS 100 /* steps in milliseconds ticks */ #define LPOS_TIMER_COUNT 100 /* steps in milliseconds ticks */ -static volatile unsigned timer[NTIMERS]; -static volatile unsigned timer_ms = MS_TIMER_COUNT; - -/* timer/system booleans */ -bool send_system_state_now = true; -bool receive_now = true; -bool send_params_now = true; -bool send_image_now = true; -bool send_lpos_now = true; +static uint8_t *current_image; +static uint8_t *previous_image; /* local position estimate without orientation, useful for unit testing w/o FMU */ struct lpos_t { @@ -124,104 +101,97 @@ struct lpos_t { float vz; } lpos = {0}; -/** - * @brief Increment boot_time_ms variable and decrement timer array. - * @param None - * @retval None - */ -void timer_update_ms(void) -{ - boot_time_ms++; - - /* each timer decrements every millisecond if > 0 */ - for (unsigned i = 0; i < NTIMERS; i++) - if (timer[i] > 0) - timer[i]--; +void sonar_update_fn(void) { + sonar_trigger(); +} - if (timer[TIMER_LED] == 0) - { - /* blink activitiy */ - LEDToggle(LED_ACT); - timer[TIMER_LED] = LED_TIMER_COUNT; - } - - if (timer[TIMER_SONAR] == 0) +void system_state_send_fn(void) { + /* every second */ + if (global_data.param[PARAM_SYSTEM_SEND_STATE]) { - sonar_trigger(); - timer[TIMER_SONAR] = SONAR_TIMER_COUNT; + communication_system_state_send(); } +} - if (timer[TIMER_SYSTEM_STATE] == 0) - { - send_system_state_now = true; - timer[TIMER_SYSTEM_STATE] = SYSTEM_STATE_COUNT; - } +void system_receive_fn(void) { + /* test every 0.5s */ + communication_receive(); + communication_receive_usb(); +} - if (timer[TIMER_RECEIVE] == 0) +void send_video_fn(void) { + /* transmit raw 8-bit image */ + if (global_data.param[PARAM_USB_SEND_VIDEO]) { - receive_now = true; - timer[TIMER_RECEIVE] = SYSTEM_STATE_COUNT; - } + /* get size of image to send */ + uint16_t image_size_send; + uint16_t image_width_send; + uint16_t image_height_send; - if (timer[TIMER_PARAMS] == 0) - { - send_params_now = true; - timer[TIMER_PARAMS] = PARAMS_COUNT; - } + uint16_t image_size = global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; - if (timer[TIMER_IMAGE] == 0) - { - send_image_now = true; - timer[TIMER_IMAGE] = global_data.param[PARAM_VIDEO_RATE]; + image_size_send = image_size; + image_width_send = global_data.param[PARAM_IMAGE_WIDTH]; + image_height_send = global_data.param[PARAM_IMAGE_HEIGHT]; + + mavlink_msg_data_transmission_handshake_send( + MAVLINK_COMM_2, + MAVLINK_DATA_STREAM_IMG_RAW8U, + image_size_send, + image_width_send, + image_height_send, + image_size_send / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1, + MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN, + 100); + LEDToggle(LED_COM); + uint16_t frame = 0; + for (frame = 0; frame < image_size_send / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1; frame++) + { + mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, &((uint8_t *) previous_image)[frame * MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN]); + } } - - if (timer[TIMER_LPOS] == 0) + else if (!global_data.param[PARAM_USB_SEND_VIDEO]) { - send_lpos_now = true; - timer[TIMER_LPOS] = LPOS_TIMER_COUNT; + LEDOff(LED_COM); } } -/** - * @brief Increment boot_time10_us variable and decrement millisecond timer, triggered by timer interrupt - * @param None - * @retval None - */ -void timer_update(void) -{ - boot_time10_us++; +void send_params_fn(void) { + debug_message_send_one(); + communication_parameter_send(); +} - /* decrements every 10 microseconds*/ - timer_ms--; +void buffer_reset(void) { + buffer_reset_needed = 1; +} - if (timer_ms == 0) +void enter_image_calibration_mode() { + while(global_data.param[PARAM_VIDEO_ONLY]) { - timer_update_ms(); - timer_ms = MS_TIMER_COUNT; - } + dcmi_restart_calibration_routine(); -} + /* waiting for first quarter of image */ + while(get_frame_counter() < 2){} + dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1); + /* waiting for second quarter of image */ + while(get_frame_counter() < 3){} + dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1); -uint32_t get_boot_time_ms(void) -{ - return boot_time_ms; -} + /* waiting for all image parts */ + while(get_frame_counter() < 4){} -uint32_t get_boot_time_us(void) -{ - return boot_time10_us*10;// *10 to return microseconds -} + send_calibration_image(&previous_image, ¤t_image); -void delay(unsigned msec) -{ - timer[TIMER_DELAY] = msec; - while (timer[TIMER_DELAY] > 0) {}; -} + /* check timers */ + timer_check(); -void buffer_reset(void) { - buffer_reset_needed = 1; + LEDToggle(LED_COM); + } + + dcmi_restart_calibration_routine(); + LEDOff(LED_COM); } /** @@ -229,6 +199,9 @@ void buffer_reset(void) { */ int main(void) { + current_image = image_buffer_8bit_1; + previous_image = image_buffer_8bit_2; + /* load settings and parameters */ global_data_reset_param_defaults(); global_data_reset(); @@ -244,13 +217,8 @@ int main(void) /* enable FPU on Cortex-M4F core */ SCB_CPACR |= ((3UL << 10 * 2) | (3UL << 11 * 2)); /* set CP10 Full Access and set CP11 Full Access */ - /* init clock */ - if (SysTick_Config(SystemCoreClock / 100000))/*set timer to trigger interrupt every 10 microsecond */ - { - /* capture clock error */ - LEDOn(LED_ERR); - while (1); - } + /* init timers */ + timer_init(); /* init usb */ USBD_Init( &USB_OTG_dev, @@ -275,9 +243,6 @@ int main(void) image_buffer_8bit_2[i] = 0; } - uint8_t * current_image = image_buffer_8bit_1; - uint8_t * previous_image = image_buffer_8bit_2; - /* usart config*/ usart_init(); @@ -291,400 +256,164 @@ int main(void) sonar_config(); /* reset/start timers */ - timer[TIMER_SONAR] = SONAR_TIMER_COUNT; - timer[TIMER_SYSTEM_STATE] = SYSTEM_STATE_COUNT; - timer[TIMER_RECEIVE] = SYSTEM_STATE_COUNT / 2; - timer[TIMER_PARAMS] = PARAMS_COUNT; - timer[TIMER_IMAGE] = global_data.param[PARAM_VIDEO_RATE]; + timer_register(sonar_update_fn, SONAR_POLL_MS); + timer_register(system_state_send_fn, SYSTEM_STATE_MS); + timer_register(system_receive_fn, SYSTEM_STATE_MS / 2); + timer_register(send_params_fn, PARAMS_MS); + timer_register(send_video_fn, global_data.param[PARAM_VIDEO_RATE]); /* variables */ uint32_t counter = 0; - uint8_t qual = 0; - uint8_t qual1 = 0; - uint8_t qual2 = 0; - - /* bottom flow variables */ - float pixel_flow_x = 0.0f; - float pixel_flow_y = 0.0f; - float pixel_flow_klt_x = 0.0f; - float pixel_flow_klt_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 velocity_x_lp = 0.0f; - float velocity_y_lp = 0.0f; - int valid_frame_count = 0; - int pixel_flow_count = 0; - - static float accumulated_flow_x = 0; - static float accumulated_flow_y = 0; - static float accumulated_gyro_x = 0; - static float accumulated_gyro_y = 0; - static float accumulated_gyro_z = 0; - static uint16_t accumulated_framecount = 0; - static uint16_t accumulated_quality = 0; - static uint32_t integration_timespan = 0; - static uint32_t lasttime = 0; - uint32_t time_since_last_sonar_update= 0; + result_accumulator_ctx mavlink_accumulator; + + result_accumulator_init(&mavlink_accumulator); /* main loop */ while (1) { + /* check timers */ + timer_check(); + /* reset flow buffers if needed */ if(buffer_reset_needed) { buffer_reset_needed = 0; - for (int i = 0; i < global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; i++) - { - image_buffer_8bit_1[i] = 0; - image_buffer_8bit_2[i] = 0; - } - delay(500); + /* get two new fresh (full) images: (or 8 small images ..) */ + dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 4); + dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 4); continue; } /* calibration routine */ if(global_data.param[PARAM_VIDEO_ONLY]) { - while(global_data.param[PARAM_VIDEO_ONLY]) - { - dcmi_restart_calibration_routine(); - - /* waiting for first quarter of image */ - while(get_frame_counter() < 2){} - dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1); - - /* waiting for second quarter of image */ - while(get_frame_counter() < 3){} - dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1); - - /* waiting for all image parts */ - while(get_frame_counter() < 4){} - - send_calibration_image(&previous_image, ¤t_image); - - if (global_data.param[PARAM_SYSTEM_SEND_STATE]) - communication_system_state_send(); - - communication_receive_usb(); - debug_message_send_one(); - communication_parameter_send(); - - LEDToggle(LED_COM); - } - - dcmi_restart_calibration_routine(); - LEDOff(LED_COM); + enter_image_calibration_mode(); + continue; } uint16_t image_size = global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; + /* calculate focal_length in pixel */ + const float focal_length_px = (global_data.param[PARAM_FOCAL_LENGTH_MM]) / (4.0f * 0.006f); //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled + /* new gyroscope data */ float x_rate_sensor, y_rate_sensor, z_rate_sensor; int16_t gyro_temp; gyro_read(&x_rate_sensor, &y_rate_sensor, &z_rate_sensor,&gyro_temp); - /* gyroscope coordinate transformation */ - float x_rate = y_rate_sensor; // change x and y rates + /* gyroscope coordinate transformation to flow sensor coordinates */ + float x_rate = y_rate_sensor; // change x and y rates float y_rate = - x_rate_sensor; - float z_rate = z_rate_sensor; // z is correct - - + float z_rate = z_rate_sensor; // z is correct - /* 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 - float x_rate_pixel = x_rate * (get_time_between_images() / 1000000.0f) * focal_length_px; /* get sonar data */ distance_valid = sonar_read(&sonar_distance_filtered, &sonar_distance_raw); - /* reset to zero for invalid distances */ if (!distance_valid) { sonar_distance_filtered = 0.0f; sonar_distance_raw = 0.0f; } - /* compute optical flow */ - if(global_data.param[PARAM_SENSOR_POSITION] == BOTTOM) - { - /* copy recent image to faster ram */ - dma_copy_image_buffers(¤t_image, &previous_image, image_size, 1); + /* copy recent image to faster ram */ + dma_copy_image_buffers(¤t_image, &previous_image, image_size, (int)(global_data.param[PARAM_CAMERA_FRAME_INTERVAL] + 0.5)); + float frame_dt = get_time_between_images() * 0.000001f; - /* filter the new image */ - filter_image(current_image, global_data.param[PARAM_IMAGE_WIDTH]); - /* compute optical flow */ - qual = compute_flow(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y); -// qual = compute_klt(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y); + /* compute gyro rate in pixels */ + float x_rate_px = x_rate * (focal_length_px * frame_dt); + float y_rate_px = y_rate * (focal_length_px * frame_dt); + float z_rate_px = z_rate * (focal_length_px * frame_dt); + /* filter the new image */ + if (global_data.param[PARAM_USE_IMAGE_FILTER]) { + filter_image(current_image, global_data.param[PARAM_IMAGE_WIDTH]); + } - /* - * real point P (X,Y,Z), image plane projection p (x,y,z), focal-length f, distance-to-scene Z - * x / f = X / Z - * y / f = Y / Z - */ - float flow_compx = pixel_flow_x / focal_length_px / (get_time_between_images() / 1000000.0f); - float flow_compy = pixel_flow_y / focal_length_px / (get_time_between_images() / 1000000.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; - - time_since_last_sonar_update = (get_boot_time_us()- get_sonar_measure_time()); - - if (qual > 0) - { - velocity_x_sum += new_velocity_x; - velocity_y_sum += new_velocity_y; - valid_frame_count++; - - uint32_t deltatime = (get_boot_time_us() - lasttime); - integration_timespan += deltatime; - accumulated_flow_x += pixel_flow_y / focal_length_px * 1.0f; //rad axis swapped to align x flow around y axis - accumulated_flow_y += pixel_flow_x / focal_length_px * -1.0f;//rad - accumulated_gyro_x += x_rate * deltatime / 1000000.0f; //rad - accumulated_gyro_y += y_rate * deltatime / 1000000.0f; //rad - accumulated_gyro_z += z_rate * deltatime / 1000000.0f; //rad - accumulated_framecount++; - accumulated_quality += qual; - - /* 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; - } - //update lasttime - lasttime = get_boot_time_us(); - - pixel_flow_x_sum += pixel_flow_x; - pixel_flow_y_sum += pixel_flow_y; - pixel_flow_count++; - + /* compute optical flow in pixels */ + float pixel_flow_x = 0.0f; + float pixel_flow_y = 0.0f; + uint8_t qual = 0; + if (global_data.param[PARAM_ALGORITHM_CHOICE] == 0) { + qual = compute_flow(previous_image, current_image, x_rate_px, y_rate_px, z_rate_px, &pixel_flow_x, &pixel_flow_y); + } else { + qual = compute_klt(previous_image, current_image, x_rate_px, y_rate_px, z_rate_px, &pixel_flow_x, &pixel_flow_y); } - counter++; + /* decide which distance to use */ + float ground_distance = 0.0f; - if(global_data.param[PARAM_SENSOR_POSITION] == BOTTOM) + if(global_data.param[PARAM_SONAR_FILTERED]) { - /* send bottom flow if activated */ - - float ground_distance = 0.0f; - - - if(global_data.param[PARAM_SONAR_FILTERED]) - { - ground_distance = sonar_distance_filtered; - } - else - { - ground_distance = sonar_distance_raw; - } - - //update I2C transmitbuffer - if(valid_frame_count>0) - { - update_TX_buffer(pixel_flow_x, pixel_flow_y, velocity_x_sum/valid_frame_count, velocity_y_sum/valid_frame_count, qual, - ground_distance, x_rate, y_rate, z_rate, gyro_temp); - } - else - { - update_TX_buffer(pixel_flow_x, pixel_flow_y, 0.0f, 0.0f, qual, - ground_distance, x_rate, y_rate, z_rate, gyro_temp); - } - - //serial mavlink + usb mavlink output throttled - if (counter % (uint32_t)global_data.param[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR] == 0)//throttling factor - { - - float flow_comp_m_x = 0.0f; - float flow_comp_m_y = 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 - { - if(valid_frame_count>0) - { - flow_comp_m_x = velocity_x_sum/valid_frame_count; - flow_comp_m_y = velocity_y_sum/valid_frame_count; - } - else - { - flow_comp_m_x = 0.0f; - flow_comp_m_y = 0.0f; - } - } - - - // send flow - mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], - pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f, - flow_comp_m_x, flow_comp_m_y, qual, ground_distance); - - mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], - integration_timespan, accumulated_flow_x, accumulated_flow_y, - accumulated_gyro_x, accumulated_gyro_y, accumulated_gyro_z, - gyro_temp, accumulated_quality/accumulated_framecount, - time_since_last_sonar_update,ground_distance); - - /* send approximate local position estimate without heading */ - if (global_data.param[PARAM_SYSTEM_SEND_LPOS]) - { - /* rough local position estimate for unit testing */ - lpos.x += ground_distance*accumulated_flow_x; - lpos.y += ground_distance*accumulated_flow_y; - lpos.z = -ground_distance; - /* velocity not directly measured and not important for testing */ - lpos.vx = 0; - lpos.vy = 0; - lpos.vz = 0; - - } else { - /* toggling param allows user reset */ - lpos.x = 0; - lpos.y = 0; - lpos.z = 0; - lpos.vx = 0; - lpos.vy = 0; - lpos.vz = 0; - } - - if (global_data.param[PARAM_USB_SEND_FLOW]) - { - mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], - pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f, - flow_comp_m_x, flow_comp_m_y, qual, ground_distance); - - - mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], - integration_timespan, accumulated_flow_x, accumulated_flow_y, - accumulated_gyro_x, accumulated_gyro_y, accumulated_gyro_z, - gyro_temp, accumulated_quality/accumulated_framecount, - time_since_last_sonar_update,ground_distance); - } - - - if(global_data.param[PARAM_USB_SEND_GYRO]) - { - mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "GYRO", get_boot_time_us(), pixel_flow_x, y_rate, z_rate); - } - - integration_timespan = 0; - accumulated_flow_x = 0; - accumulated_flow_y = 0; - accumulated_framecount = 0; - accumulated_quality = 0; - accumulated_gyro_x = 0; - accumulated_gyro_y = 0; - accumulated_gyro_z = 0; - - velocity_x_sum = 0.0f; - velocity_y_sum = 0.0f; - pixel_flow_x_sum = 0.0f; - pixel_flow_y_sum = 0.0f; - valid_frame_count = 0; - pixel_flow_count = 0; - } + ground_distance = sonar_distance_filtered; } - - /* forward flow from other sensors */ - if (counter % 2) + else { - communication_receive_forward(); + ground_distance = sonar_distance_raw; } - /* send system state, receive commands */ - if (send_system_state_now) - { - /* every second */ - if (global_data.param[PARAM_SYSTEM_SEND_STATE]) - { - communication_system_state_send(); - } - send_system_state_now = false; - } + /* update I2C transmitbuffer */ + update_TX_buffer(frame_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())); - /* receive commands */ - if (receive_now) - { - /* test every second */ - communication_receive(); - communication_receive_usb(); - receive_now = false; - } + /* accumulate the results */ + result_accumulator_feed(&mavlink_accumulator, frame_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())); - /* sending debug msgs and requested parameters */ - if (send_params_now) - { - debug_message_send_one(); - communication_parameter_send(); - send_params_now = false; - } + counter++; - /* send local position estimate, for testing only, doesn't account for heading */ - if (send_lpos_now) + /* serial mavlink + usb mavlink output throttled */ + if (counter % (uint32_t)global_data.param[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR] == 0)//throttling factor { - if (global_data.param[PARAM_SYSTEM_SEND_LPOS]) + /* recalculate the output values */ + result_accumulator_output_flow output_flow; + result_accumulator_output_flow_rad output_flow_rad; + result_accumulator_calculate_output_flow(&mavlink_accumulator, 1, &output_flow); + result_accumulator_calculate_output_flow_rad(&mavlink_accumulator, 1, &output_flow_rad); + + // send flow + mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], + output_flow.flow_x, output_flow.flow_y, + output_flow.flow_comp_m_x, output_flow.flow_comp_m_y, + output_flow.quality, output_flow.ground_distance); + + mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], + output_flow_rad.integration_time, + output_flow_rad.integrated_x, output_flow_rad.integrated_y, + output_flow_rad.integrated_xgyro, output_flow_rad.integrated_ygyro, output_flow_rad.integrated_zgyro, + output_flow_rad.temperature, output_flow_rad.quality, + output_flow_rad.time_delta_distance_us,output_flow_rad.ground_distance); + + if (global_data.param[PARAM_USB_SEND_FLOW]) { - mavlink_msg_local_position_ned_send(MAVLINK_COMM_2, timer_ms, lpos.x, lpos.y, lpos.z, lpos.vx, lpos.vy, lpos.vz); + mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], + output_flow.flow_x, output_flow.flow_y, + output_flow.flow_comp_m_x, output_flow.flow_comp_m_y, + output_flow.quality, output_flow.ground_distance); + + mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], + output_flow_rad.integration_time, + output_flow_rad.integrated_x, output_flow_rad.integrated_y, + output_flow_rad.integrated_xgyro, output_flow_rad.integrated_ygyro, output_flow_rad.integrated_zgyro, + output_flow_rad.temperature, output_flow_rad.quality, + output_flow_rad.time_delta_distance_us,output_flow_rad.ground_distance); } - send_lpos_now = false; - } - /* transmit raw 8-bit image */ - if (global_data.param[PARAM_USB_SEND_VIDEO] && send_image_now) - { - /* get size of image to send */ - uint16_t image_size_send; - uint16_t image_width_send; - uint16_t image_height_send; - - image_size_send = image_size; - image_width_send = global_data.param[PARAM_IMAGE_WIDTH]; - image_height_send = global_data.param[PARAM_IMAGE_HEIGHT]; - - mavlink_msg_data_transmission_handshake_send( - MAVLINK_COMM_2, - MAVLINK_DATA_STREAM_IMG_RAW8U, - image_size_send, - image_width_send, - image_height_send, - image_size_send / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1, - MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN, - 100); - LEDToggle(LED_COM); - uint16_t frame = 0; - for (frame = 0; frame < image_size_send / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1; frame++) + if(global_data.param[PARAM_USB_SEND_GYRO]) { - mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, &((uint8_t *) previous_image)[frame * MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN]); + mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "GYRO", get_boot_time_us(), pixel_flow_x, y_rate, z_rate); } - send_image_now = false; + result_accumulator_reset(&mavlink_accumulator); } - else if (!global_data.param[PARAM_USB_SEND_VIDEO]) + + /* forward flow from other sensors */ + if (counter % 2) { - LEDOff(LED_COM); + communication_receive_forward(); } } } diff --git a/src/result_accumulator.c b/src/result_accumulator.c new file mode 100644 index 0000000..2512b81 --- /dev/null +++ b/src/result_accumulator.c @@ -0,0 +1,278 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Petri Tanskanen + * Lorenz Meier + * Samuel Zihlmann + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include + +#include "result_accumulator.h" + +void result_accumulator_init(result_accumulator_ctx *ctx) +{ + /* set everything to zero */ + memset(ctx, 0, sizeof(result_accumulator_ctx)); + /* set non-zero initial values */ + ctx->min_quality = 255; + 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, + 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.x_rate = x_rate; + ctx->last.y_rate = y_rate; + ctx->last.z_rate = z_rate; + ctx->last.temperature = gyro_temp; + ctx->last.qual = qual; + ctx->last.pixel_flow_x = pixel_flow_x; + ctx->last.pixel_flow_y = pixel_flow_y; + ctx->last.flow_x_rad = pixel_flow_y * rad_per_pixel; + ctx->last.flow_y_rad = - pixel_flow_x * rad_per_pixel; + ctx->last.ground_distance = distance_valid ? ground_distance : -1; + ctx->last.distance_age = distance_age; + /* accumulate the values */ + if (qual > 0) { + ctx->px_flow_x_accu += ctx->last.pixel_flow_x; + ctx->px_flow_y_accu += ctx->last.pixel_flow_y; + ctx->rad_flow_x_accu += ctx->last.flow_x_rad; + ctx->rad_flow_y_accu += ctx->last.flow_y_rad; + ctx->gyro_x_accu += x_rate * dt; + ctx->gyro_y_accu += y_rate * dt; + ctx->gyro_z_accu += z_rate * dt; + /* the quality is the minimum of all quality measurements */ + if (qual < ctx->min_quality || ctx->valid_data_count == 0) { + ctx->min_quality = qual; + } + ctx->valid_data_count++; + ctx->valid_time += dt; + } + ctx->frame_count++; + ctx->full_time += dt; + + ctx->last.frame_count = ctx->frame_count; +} + +void result_accumulator_calculate_output_flow(result_accumulator_ctx *ctx, uint16_t min_valid_data_count, result_accumulator_output_flow *out) +{ + if (ctx->valid_data_count >= min_valid_data_count) { + /*** calculate the output values for the flow mavlink message ***/ + /* scale the averaged values from valid time to full time (we extrapolate the invalid data sets) */ + float time_scaling_f = ctx->full_time / ctx->valid_time; + out->flow_x = floor(ctx->px_flow_x_accu * time_scaling_f + 0.5f); + out->flow_y = floor(ctx->px_flow_y_accu * time_scaling_f + 0.5f); + out->flow_comp_m_x = 0; + out->flow_comp_m_y = 0; + out->quality = ctx->min_quality; + /* averaging the distance is no use */ + out->ground_distance = ctx->last.ground_distance; + } else { + /* not enough valid data */ + out->flow_x = 0; + out->flow_y = 0; + out->flow_comp_m_x = 0; + out->flow_comp_m_y = 0; + out->quality = 0; + out->ground_distance = -1; + } +} + +void result_accumulator_calculate_output_flow_rad(result_accumulator_ctx *ctx, uint16_t min_valid_data_count, result_accumulator_output_flow_rad *out) +{ + if (ctx->valid_data_count >= min_valid_data_count) { + /*** calculate the output values for the flow_rad mavlink message ***/ + out->integration_time = ctx->valid_time * 1000000.0f; + out->integrated_x = ctx->rad_flow_x_accu; + out->integrated_y = ctx->rad_flow_y_accu; + out->integrated_xgyro = ctx->gyro_x_accu; + out->integrated_ygyro = ctx->gyro_y_accu; + out->integrated_zgyro = ctx->gyro_z_accu; + out->quality = ctx->min_quality; + /* averaging the distance and temperature is no use */ + out->temperature = ctx->last.temperature; + out->time_delta_distance_us = ctx->last.distance_age; + out->ground_distance = ctx->last.ground_distance; + } else { + /* not enough valid data */ + out->integration_time = 0; + out->integrated_x = 0; + out->integrated_y = 0; + out->integrated_xgyro = 0; + out->integrated_ygyro = 0; + out->integrated_zgyro = 0; + out->temperature = ctx->last.temperature; + out->time_delta_distance_us = 0; + out->ground_distance = -1; + } +} + +void result_accumulator_calculate_output_flow_i2c(result_accumulator_ctx *ctx, uint16_t min_valid_data_count, result_accumulator_output_flow_i2c *out) +{ + if (ctx->valid_data_count >= min_valid_data_count) { + /*** calculate the output values for the i2c message ***/ + out->frame_count = ctx->frame_count; + out->valid_frames = ctx->valid_data_count; + out->integration_time = ctx->valid_time * 1000000.0f; + out->rad_flow_x = floor(ctx->rad_flow_x_accu * 100.0f + 0.5f); + out->rad_flow_y = floor(ctx->rad_flow_y_accu * 100.0f + 0.5f); + out->integrated_gyro_x = floor(ctx->gyro_x_accu * 100.0f + 0.5f); + out->integrated_gyro_y = floor(ctx->gyro_y_accu * 100.0f + 0.5f); + out->integrated_gyro_z = floor(ctx->gyro_z_accu * 100.0f + 0.5f); + out->gyro_x = floor(ctx->gyro_x_accu * (100.0f / ctx->valid_time) + 0.5f); + out->gyro_y = floor(ctx->gyro_y_accu * (100.0f / ctx->valid_time) + 0.5f); + out->gyro_z = floor(ctx->gyro_z_accu * (100.0f / ctx->valid_time) + 0.5f); + out->quality = ctx->min_quality; + out->temperature = ctx->last.temperature; + out->time_delta_distance_us = ctx->last.distance_age; + if (ctx->last.ground_distance >= 0) { + out->ground_distance = floor(ctx->last.ground_distance * 1000.0f + 0.5f); + } else { + out->ground_distance = 0; + } + } else { + /* not enough valid data */ + out->frame_count = ctx->frame_count; + out->valid_frames = 0; + out->integration_time = 0; + out->rad_flow_x = 0; + out->rad_flow_y = 0; + out->gyro_x = 0; + out->gyro_y = 0; + out->gyro_z = 0; + out->quality = 0; + out->temperature = ctx->last.temperature; + out->time_delta_distance_us = 0; + out->ground_distance = -1; + } +} + +void result_accumulator_reset(result_accumulator_ctx *ctx) +{ + ctx->px_flow_x_accu = 0; + ctx->px_flow_y_accu = 0; + + ctx->rad_flow_x_accu = 0; + ctx->rad_flow_y_accu = 0; + + ctx->gyro_x_accu = 0; + ctx->gyro_y_accu = 0; + ctx->gyro_z_accu = 0; + + ctx->min_quality = 255; + + ctx->valid_data_count = 0; + ctx->valid_time = 0; + ctx->full_time = 0; +} + +#if 0 +/* 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; + + time_since_last_sonar_update = (get_boot_time_us()- get_sonar_measure_time()); + + if (qual > 0) + { + velocity_x_sum += new_velocity_x; + velocity_y_sum += new_velocity_y; + valid_frame_count++; + + uint32_t deltatime = (get_boot_time_us() - lasttime); + integration_timespan += deltatime; + accumulated_flow_x += pixel_flow_y / focal_length_px * 1.0f; //rad axis swapped to align x flow around y axis + accumulated_flow_y += pixel_flow_x / focal_length_px * -1.0f;//rad + accumulated_gyro_x += x_rate * deltatime / 1000000.0f; //rad + accumulated_gyro_y += y_rate * deltatime / 1000000.0f; //rad + accumulated_gyro_z += z_rate * deltatime / 1000000.0f; //rad + accumulated_framecount++; + accumulated_quality += qual; + + /* 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; + } + //update lasttime + lasttime = get_boot_time_us(); + + pixel_flow_x_sum += pixel_flow_x; + pixel_flow_y_sum += pixel_flow_y; + pixel_flow_count++; + + + + float flow_comp_m_x = 0.0f; + float flow_comp_m_y = 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 + { + if(valid_frame_count>0) + { + flow_comp_m_x = velocity_x_sum/valid_frame_count; + flow_comp_m_y = velocity_y_sum/valid_frame_count; + } + else + { + flow_comp_m_x = 0.0f; + flow_comp_m_y = 0.0f; + } + } +#endif \ No newline at end of file diff --git a/src/settings.c b/src/settings.c index 620ad40..754034c 100644 --- a/src/settings.c +++ b/src/settings.c @@ -77,6 +77,20 @@ void global_data_reset_param_defaults(void){ strcpy(global_data.param_name[PARAM_SYSTEM_SEND_LPOS], "SYS_SEND_LPOS"); global_data.param_access[PARAM_SYSTEM_SEND_LPOS] = READ_WRITE; + + global_data.param[PARAM_ALGORITHM_CHOICE] = 0; + strcpy(global_data.param_name[PARAM_ALGORITHM_CHOICE], "ALGORITHM_CHOICE"); + global_data.param_access[PARAM_ALGORITHM_CHOICE] = READ_WRITE; + + global_data.param[PARAM_USE_IMAGE_FILTER] = 0; + strcpy(global_data.param_name[PARAM_USE_IMAGE_FILTER], "USE_IMAGE_FILTER"); + global_data.param_access[PARAM_USE_IMAGE_FILTER] = READ_WRITE; + + global_data.param[PARAM_CAMERA_FRAME_INTERVAL] = 2; + strcpy(global_data.param_name[PARAM_CAMERA_FRAME_INTERVAL], "CAMERA_FRAME_INTERVAL"); + global_data.param_access[PARAM_CAMERA_FRAME_INTERVAL] = READ_WRITE; + + global_data.param[PARAM_SENSOR_POSITION] = 0; // BOTTOM strcpy(global_data.param_name[PARAM_SENSOR_POSITION], "POSITION"); global_data.param_access[PARAM_SENSOR_POSITION] = READ_WRITE; diff --git a/src/stm32f4xx_it.c b/src/stm32f4xx_it.c index eca0dc4..fd5a928 100644 --- a/src/stm32f4xx_it.c +++ b/src/stm32f4xx_it.c @@ -30,6 +30,7 @@ #include "stm32f4xx_it.h" #include "stm32f4xx_conf.h" #include "main.h" +#include "timer.h" #include "usb_core.h" #include "usb_dcd_int.h" diff --git a/src/timer.c b/src/timer.c new file mode 100644 index 0000000..085113f --- /dev/null +++ b/src/timer.c @@ -0,0 +1,197 @@ + +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Petri Tanskanen + * Lorenz Meier + * Samuel Zihlmann + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include +#include "stm32f4xx.h" + +#include "timer.h" +#include "led.h" + +#define __INLINE inline +#define __ASM asm +#include "core_cmInstr.h" + + +#define MS_TIMER_COUNT 100 /* steps in 10 microseconds ticks */ + +/* boot time in milliseconds ticks */ +volatile uint32_t boot_time_ms = 0; +volatile uint32_t boot_time_us_base = 0; + +static uint32_t ticks_per_ms; + +#define TIMER_FLAG_ENABLED 0x01 +#define TIMER_FLAG_TRIGGERED 0x02 + +typedef struct _timer_info { + uint32_t period; + uint32_t countdown; + void (*timer_fn)(void); + volatile uint8_t flags; +} timer_info; + +static timer_info timer[NTIMERS]; + +/** + * @brief Triggered by systen timer interrupt every millisecond. + * @param None + * @retval None + */ +void timer_update(void) +{ + boot_time_ms++; + /* we do it like this to have correct roll-over behaviour */ + boot_time_us_base += 1000u; + + /* each timer decrements every millisecond if > 0 */ + for (unsigned i = 0; i < NTIMERS; i++) { + /* dont need exclusive access for reading the enabled flag because it will not be changed very often */ + if (timer[i].flags & TIMER_FLAG_ENABLED) { + if (timer[i].countdown > 0) { + timer[i].countdown--; + } else { + timer[i].countdown = timer[i].period - 1; + /* set the flag: */ + timer[i].flags |= TIMER_FLAG_TRIGGERED; + /* force a fail of the flags access instructions: */ + __CLREX(); + } + } + } +} + +void timer_check(void) { + for (unsigned i = 0; i < NTIMERS; i++) { + bool triggered = false; + uint8_t tmp; + do { + tmp = __LDREXB(&timer[i].flags); + triggered = (tmp & TIMER_FLAG_TRIGGERED); + if (triggered) { + tmp &= ~TIMER_FLAG_TRIGGERED; + } + } while (!__STREXB(tmp, &timer[i].flags)); + if (triggered) { + timer[i].timer_fn(); + } + } +} + +void timer_register(void (*timer_fn)(void), uint32_t period_ms) { + /* find free timer: */ + int idx; + for (idx = 0; idx < NTIMERS; idx++) { + if ((timer[idx].flags & TIMER_FLAG_ENABLED) == 0) { + break; + } + } + if (idx >= NTIMERS) { + /* capture error */ + LEDOn(LED_ERR); + while (1); + } + /* setup the info struct: */ + timer[idx].period = period_ms; + timer[idx].countdown = period_ms - 1; + timer[idx].timer_fn = timer_fn; + /* enable it: */ + timer[idx].flags = TIMER_FLAG_ENABLED; +} + +void timer_init(void) +{ + /* init clock */ + ticks_per_ms = SystemCoreClock / 1000; + /* init all timers */ + for (int idx = 0; idx < NTIMERS; idx++) { + /* disable it: */ + timer[idx].flags = 0; + } + /* enable systick timer: */ + if (SysTick_Config(ticks_per_ms)) /* set timer to trigger interrupt every 1 millisecond */ + { + /* capture clock error */ + LEDOn(LED_ERR); + while (1); + } +} + +uint32_t get_boot_time_ms(void) +{ + // clear the COUNTFLAG: + volatile bool dummy = (SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) != 0; + volatile uint32_t val; + do { + // read the value: + val = boot_time_ms; + // make sure it did not roll over in the mean-time: + } while(SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk); + return val; +} + +uint32_t get_boot_time_us(void) +{ + // clear the COUNTFLAG: + volatile bool dummy = (SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) != 0; + volatile uint32_t val_us_base; + volatile uint32_t val_tick; + do { + // read the value: + val_us_base = boot_time_us_base; + val_tick = SysTick->VAL; + // make sure it did not roll over in the mean-time: + } while(SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk); + // return the calculated value: + return ((val_tick & SysTick_VAL_CURRENT_Msk) / ticks_per_ms) + val_us_base; +} + +uint32_t calculate_time_delta_us(uint32_t end, uint32_t start) { + return (int32_t)end - (int32_t)start; +} + +uint32_t get_time_delta_us(uint32_t start) +{ + uint32_t now = get_boot_time_us(); + return calculate_time_delta_us(now, start); +} + +void delay(uint16_t ms) { + int32_t endt = get_boot_time_us() + (uint32_t)ms * 1000u; + while ((endt - (int32_t)get_boot_time_us()) > 0); +} From 51059d980238faf15b5c9ea002443c5f9faae504 Mon Sep 17 00:00:00 2001 From: Severin Latkovic Date: Thu, 9 Jul 2015 18:04:49 +0200 Subject: [PATCH 012/120] make the code work. - fix timer functions. do not use exclusive access instructions. - fix time accumulation data type. --- inc/result_accumulator.h | 4 ++-- src/main.c | 2 +- src/mt9v034.c | 2 +- src/timer.c | 14 ++++---------- 4 files changed, 8 insertions(+), 14 deletions(-) diff --git a/inc/result_accumulator.h b/inc/result_accumulator.h index 8d85934..a1e6fcb 100644 --- a/inc/result_accumulator.h +++ b/inc/result_accumulator.h @@ -61,8 +61,8 @@ typedef struct _result_accumulator_ctx { float rad_flow_y_accu; uint8_t min_quality; uint16_t valid_data_count; - uint32_t valid_time; - uint32_t full_time; + float valid_time; + float full_time; float gyro_x_accu; float gyro_y_accu; float gyro_z_accu; diff --git a/src/main.c b/src/main.c index 6f9054f..23a0957 100644 --- a/src/main.c +++ b/src/main.c @@ -268,7 +268,7 @@ int main(void) result_accumulator_ctx mavlink_accumulator; result_accumulator_init(&mavlink_accumulator); - + /* main loop */ while (1) { diff --git a/src/mt9v034.c b/src/mt9v034.c index 7be73b8..989a4dd 100644 --- a/src/mt9v034.c +++ b/src/mt9v034.c @@ -96,7 +96,7 @@ void mt9v034_context_configuration(void) * */ uint16_t new_readmode_context_a = 0x30A ; // row + col bin 4 enable, (9:8) default - uint16_t new_readmode_context_b = 0x305 ; // row bin 2 col bin 4 enable, (9:8) default + uint16_t new_readmode_context_b = 0x305 ; // row bin 2 col bin 2 enable, (9:8) default /* * Settings for both context: diff --git a/src/timer.c b/src/timer.c index 085113f..484110b 100644 --- a/src/timer.c +++ b/src/timer.c @@ -89,8 +89,6 @@ void timer_update(void) timer[i].countdown = timer[i].period - 1; /* set the flag: */ timer[i].flags |= TIMER_FLAG_TRIGGERED; - /* force a fail of the flags access instructions: */ - __CLREX(); } } } @@ -99,14 +97,10 @@ void timer_update(void) void timer_check(void) { for (unsigned i = 0; i < NTIMERS; i++) { bool triggered = false; - uint8_t tmp; - do { - tmp = __LDREXB(&timer[i].flags); - triggered = (tmp & TIMER_FLAG_TRIGGERED); - if (triggered) { - tmp &= ~TIMER_FLAG_TRIGGERED; - } - } while (!__STREXB(tmp, &timer[i].flags)); + triggered = (timer[i].flags & TIMER_FLAG_TRIGGERED); + if (triggered) { + timer[i].flags &= ~TIMER_FLAG_TRIGGERED; + } if (triggered) { timer[i].timer_fn(); } From 8cb3c49934d0db84f6e79d5006c963e17a3e57f8 Mon Sep 17 00:00:00 2001 From: Severin Latkovic Date: Thu, 9 Jul 2015 18:13:24 +0200 Subject: [PATCH 013/120] better fix than before. --- src/timer.c | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/timer.c b/src/timer.c index 484110b..2bb0756 100644 --- a/src/timer.c +++ b/src/timer.c @@ -89,6 +89,8 @@ void timer_update(void) timer[i].countdown = timer[i].period - 1; /* set the flag: */ timer[i].flags |= TIMER_FLAG_TRIGGERED; + /* force a fail of the flags access instructions: */ + __CLREX(); } } } @@ -97,10 +99,14 @@ void timer_update(void) void timer_check(void) { for (unsigned i = 0; i < NTIMERS; i++) { bool triggered = false; - triggered = (timer[i].flags & TIMER_FLAG_TRIGGERED); - if (triggered) { - timer[i].flags &= ~TIMER_FLAG_TRIGGERED; - } + uint8_t tmp; + do { + tmp = __LDREXB(&timer[i].flags); + triggered = (tmp & TIMER_FLAG_TRIGGERED); + if (triggered) { + tmp &= ~TIMER_FLAG_TRIGGERED; + } + } while (__STREXB(tmp, &timer[i].flags)); if (triggered) { timer[i].timer_fn(); } From 0dfddb6d9d9e9e4988b225c7bee0ce628e7980b9 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Thu, 9 Jul 2015 23:21:20 +0200 Subject: [PATCH 014/120] move calculation of mean flow out of flow measurement functions. cleanup of code and small optimisations. --- inc/flow.h | 41 ++- src/flow.c | 850 ++++++++++++++++------------------------------------ src/main.c | 39 ++- src/timer.c | 3 - 4 files changed, 325 insertions(+), 608 deletions(-) diff --git a/inc/flow.h b/inc/flow.h index d182fdd..5a0b0d7 100644 --- a/inc/flow.h +++ b/inc/flow.h @@ -36,16 +36,45 @@ #include +typedef struct _flow_raw_result { + float x; ///< The flow in x direction + float y; ///< The flow in y direction + float quality; ///< The quality of this result. 0 = bad + uint8_t at_x; ///< The mid-position of the patch that was used to calculate the flow. + uint8_t at_y; ///< The mid-position of the patch that was used to calculate the flow. +} flow_raw_result; + /** - * @brief Computes pixel flow from image1 to image2 + * @brief Computes pixel flow from image1 to image2 + * Searches the corresponding position in the new image (image2) of max. 64 pixels from the old image (image1). + * @param image1 The older image + * @param image2 The new image + * @param x_rate The gyro x-rate during the frame interval in pixels. (In the image x direction) + * @param y_rate The gyro y-rate during the frame interval in pixels. (In the image y direction) + * @param z_rate The gyro z-rate during the frame interval in radians. + * @param out Array which receives the raw result vectors computed for the blocks in the image. + * @param result_count The available space in the out buffer. + * @return The number of results written to the out buffer. */ -uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate, - float *histflowx, float *histflowy); +uint16_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate, + flow_raw_result *out, uint16_t max_out); /** - * @brief Computes pixel flow from image1 to image2 with KLT method + * @brief Computes pixel flow from image1 to image2 + * Searches the corresponding position in the new image (image2) of max. 64 pixels from the old image (image1) + * with the KLT method and outputs the value of all flow vectors. + * @param image1 The older image + * @param image2 The new image + * @param x_rate The gyro x-rate during the frame interval in pixels. (In the image x direction) + * @param y_rate The gyro y-rate during the frame interval in pixels. (In the image y direction) + * @param z_rate The gyro z-rate during the frame interval in radians. + * @param out Array which receives the raw result vectors computed for the blocks in the image. + * @param result_count The available space in the out buffer. + * @return The number of results written to the out buffer. */ -uint8_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate, - float *pixel_flow_x, float *pixel_flow_y); +uint16_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate, + flow_raw_result *out, uint16_t max_out); + +uint8_t flow_extract_result(flow_raw_result *in, uint16_t result_count, float *px_flow_x, float *px_flow_y); #endif /* FLOW_H_ */ diff --git a/src/flow.c b/src/flow.c index 34663e5..4decc4e 100644 --- a/src/flow.c +++ b/src/flow.c @@ -41,6 +41,7 @@ #include "mavlink_bridge_header.h" #include +#include "flow.h" #include "dcmi.h" #include "debug.h" @@ -64,8 +65,6 @@ float Jy[PATCH_SIZE*PATCH_SIZE]; #define sign(x) (( x > 0 ) - ( x < 0 )) -uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate, float *pixel_flow_x, float *pixel_flow_y); - // compliments of Adam Williams #define ABSDIFF(frame1, frame2) \ ({ \ @@ -390,46 +389,24 @@ static inline uint32_t compute_sad_8x8(uint8_t *image1, uint8_t *image2, uint16_ return acc; } -/** - * @brief Computes pixel flow from image1 to image2 - * - * Searches the corresponding position in the new image (image2) of max. 64 pixels from the old image (image1) - * and calculates the average offset of all. - * - * @param image1 previous image buffer - * @param image2 current image buffer (new) - * @param x_rate gyro x rate - * @param y_rate gyro y rate - * @param z_rate gyro z rate - * - * @return quality of flow calculation - */ -uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate, float *pixel_flow_x, float *pixel_flow_y) { - +uint16_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate, + flow_raw_result *out, uint16_t max_out) +{ /* constants */ - const int16_t winmin = -SEARCH_SIZE; - const int16_t winmax = SEARCH_SIZE; - const uint16_t hist_size = 2*(winmax-winmin+1)+1; + const uint16_t search_size = SEARCH_SIZE; + const uint16_t frame_size = FRAME_SIZE; + const int16_t winmin = -search_size; + const int16_t winmax = search_size; /* variables */ - uint16_t pixLo = SEARCH_SIZE + 1; - uint16_t pixHi = FRAME_SIZE - (SEARCH_SIZE + 1) - TILE_SIZE; - uint16_t pixStep = (pixHi - pixLo) / NUM_BLOCKS + 1; + uint16_t pixLo = search_size + 1; + uint16_t pixHi = frame_size - (search_size + 1) - TILE_SIZE; + uint16_t pixStep = (pixHi - pixLo - 1) / NUM_BLOCKS; + pixHi = pixLo + pixStep * NUM_BLOCKS; uint16_t i, j; uint32_t acc[8]; // subpixels - uint16_t histx[hist_size]; // counter for x shift - uint16_t histy[hist_size]; // counter for y shift - int8_t dirsx[64]; // shift directions in x - int8_t dirsy[64]; // shift directions in y - uint8_t subdirs[64]; // shift directions of best subpixels - float meanflowx = 0.0f; - float meanflowy = 0.0f; - uint16_t meancount = 0; - float histflowx = 0.0f; - float histflowy = 0.0f; - - /* initialize with 0 */ - for (j = 0; j < hist_size; j++) { histx[j] = 0; histy[j] = 0; } + + uint16_t result_count = 0; /* iterate over all patterns */ @@ -437,8 +414,20 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat { for (i = pixLo; i < pixHi; i += pixStep) { + /* abort if the output buffer is full */ + if (result_count >= max_out) break; + flow_raw_result *result = &out[result_count]; + /* init the result */ + result->x = 0; + result->y = 0; + result->quality = 0; + result->at_x = i + TILE_SIZE / 2; + result->at_y = j + TILE_SIZE / 2; + + result_count++; + /* test pixel if it is suitable for flow tracking */ - uint32_t diff = compute_diff(image1, i, j, (uint16_t) global_data.param[PARAM_IMAGE_WIDTH]); + uint32_t diff = compute_diff(image1, i, j, frame_size); if (diff < global_data.param[PARAM_BOTTOM_FLOW_FEATURE_THRESHOLD]) { continue; @@ -449,15 +438,15 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat int8_t sumy = 0; int8_t ii, jj; - uint8_t *base1 = image1 + j * (uint16_t) global_data.param[PARAM_IMAGE_WIDTH] + i; + uint8_t *base1 = image1 + j * frame_size + i; for (jj = winmin; jj <= winmax; jj++) { - uint8_t *base2 = image2 + (j+jj) * (uint16_t) global_data.param[PARAM_IMAGE_WIDTH] + i; + uint8_t *base2 = image2 + (j+jj) * frame_size + i; for (ii = winmin; ii <= winmax; ii++) { -// uint32_t temp_dist = compute_sad_8x8(image1, image2, i, j, i + ii, j + jj, (uint16_t) global_data.param[PARAM_IMAGE_WIDTH]); +// uint32_t temp_dist = compute_sad_8x8(image1, image2, i, j, i + ii, j + jj, frame_size); uint32_t temp_dist = ABSDIFF(base1, base2 + ii); if (temp_dist < dist) { @@ -471,10 +460,7 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat /* acceptance SAD distance threshhold */ if (dist < global_data.param[PARAM_BOTTOM_FLOW_VALUE_THRESHOLD]) { - meanflowx += (float) sumx; - meanflowy += (float) sumy; - - compute_subpixel(image1, image2, i, j, i + sumx, j + sumy, acc, (uint16_t) global_data.param[PARAM_IMAGE_WIDTH]); + compute_subpixel(image1, image2, i, j, i + sumx, j + sumy, acc, frame_size); uint32_t mindist = dist; // best SAD until now uint8_t mindir = 8; // direction 8 for no direction for(uint8_t k = 0; k < 8; k++) @@ -486,590 +472,274 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat mindir = k; } } - dirsx[meancount] = sumx; - dirsy[meancount] = sumy; - subdirs[meancount] = mindir; - meancount++; - - /* feed histogram filter*/ - uint8_t hist_index_x = 2*sumx + (winmax-winmin+1); - if (subdirs[i] == 0 || subdirs[i] == 1 || subdirs[i] == 7) hist_index_x += 1; - if (subdirs[i] == 3 || subdirs[i] == 4 || subdirs[i] == 5) hist_index_x += -1; - uint8_t hist_index_y = 2*sumy + (winmax-winmin+1); - if (subdirs[i] == 5 || subdirs[i] == 6 || subdirs[i] == 7) hist_index_y += -1; - if (subdirs[i] == 1 || subdirs[i] == 2 || subdirs[i] == 3) hist_index_y += 1; - - histx[hist_index_x]++; - histy[hist_index_y]++; - + /* store the flow value */ + result->x = sumx; + result->y = sumy; + if (mindir == 0 || mindir == 1 || mindir == 7) result->x += 0.5f; + if (mindir == 3 || mindir == 4 || mindir == 5) result->x -= 0.5f; + if (mindir == 5 || mindir == 6 || mindir == 7) result->y -= 0.5f; + if (mindir == 1 || mindir == 2 || mindir == 3) result->y += 0.5f; + /* gyro compensation */ + result->x -= x_rate; + result->y -= y_rate; + result->quality = 1.0; } } } - /* create flow image if needed (image1 is not needed anymore) - * -> can be used for debugging purpose - */ - if (global_data.param[PARAM_USB_SEND_VIDEO] )//&& global_data.param[PARAM_VIDEO_USB_MODE] == FLOW_VIDEO) - { - int pixel_ind = 0; - for (j = pixLo; j < pixHi; j += pixStep) - { - for (i = pixLo; i < pixHi; i += pixStep) - { + return result_count; +} - uint32_t diff = compute_diff(image1, i, j, (uint16_t) global_data.param[PARAM_IMAGE_WIDTH]); - if (diff > global_data.param[PARAM_BOTTOM_FLOW_FEATURE_THRESHOLD]) - { - image1[j * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i] = 255; - if(subdirs[pixel_ind] == 0) - image1[j * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i+1] = 200; - if(subdirs[pixel_ind] == 1) - image1[(j+1) * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i+1] = 200; - if(subdirs[pixel_ind] == 2) - image1[(j+1) * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i] = 200; - if (subdirs[pixel_ind] == 3) - image1[(j+1) * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i-1] = 200; - if(subdirs[pixel_ind] == 4) - image1[j * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i-1] = 200; - if(subdirs[pixel_ind] == 5) - image1[(j-1) * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i-1] = 200; - if(subdirs[pixel_ind] == 6) - image1[(j-1) * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i] = 200; - if(subdirs[pixel_ind] == 7) - image1[(j-1) * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i + 1] = 200; - pixel_ind++; - } - } - } - } +uint16_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate, + flow_raw_result *out, uint16_t max_out) +{ + /* variables */ + uint16_t i, j; - /* evaluate flow calculation */ - if (meancount > 5) + float chi_sum = 0.0f; + uint8_t chicount = 0; + + /* + * compute image pyramid for current frame + * there is 188*120 bytes per buffer, we are only using 64*64 per buffer, + * so just add the pyramid levels after the image + */ + //first compute the offsets in the memory for the pyramid levels + uint16_t lvl_ofs[PYR_LVLS]; + uint16_t frame_size = (uint16_t)(FRAME_SIZE+0.5); + uint16_t s = frame_size; + uint16_t off = 0; + for (int l = 0; l < PYR_LVLS; l++) { - meanflowx /= meancount; - meanflowy /= meancount; + lvl_ofs[l] = off; + off += s*s; + s /= 2; + } - int16_t maxpositionx = 0; - int16_t maxpositiony = 0; - uint16_t maxvaluex = 0; - uint16_t maxvaluey = 0; + //then subsample the images consecutively, no blurring is done before the subsampling (if someone volunteers, please go ahead...) + for (int l = 1; l < PYR_LVLS; l++) + { + uint16_t src_size = frame_size >> (l-1); + uint16_t tar_size = frame_size >> l; + uint8_t *source = &image2[lvl_ofs[l-1]]; //pointer to the beginning of the previous level + uint8_t *target = &image2[lvl_ofs[l]]; //pointer to the beginning of the current level + for (j = 0; j < tar_size; j++) + for (i = 0; i < tar_size; i+=2) + { + //subsample the image by 2, use the halving-add instruction to do so + uint32_t l1 = (__UHADD8(*((uint32_t*) &source[(j*2+0)*src_size + i*2]), *((uint32_t*) &source[(j*2+0)*src_size + i*2+1]))); + uint32_t l2 = (__UHADD8(*((uint32_t*) &source[(j*2+1)*src_size + i*2]), *((uint32_t*) &source[(j*2+1)*src_size + i*2+1]))); + uint32_t r = __UHADD8(l1, l2); + + //the first and the third byte are the values we want to have + target[j*tar_size + i+0] = (uint8_t) r; + target[j*tar_size + i+1] = (uint8_t) (r>>16); + } + } - /* position of maximal histogram peek */ - for (j = 0; j < hist_size; j++) + //need to store the flow values between pyramid level changes + float us[NUM_BLOCK_KLT*NUM_BLOCK_KLT]; + float vs[NUM_BLOCK_KLT*NUM_BLOCK_KLT]; + uint16_t is[NUM_BLOCK_KLT*NUM_BLOCK_KLT]; + uint16_t js[NUM_BLOCK_KLT*NUM_BLOCK_KLT]; + + //initialize flow values with the pixel value of the previous image + uint16_t pixBaseStep = 1 << (PYR_LVLS - 1); + uint16_t pixStep = frame_size / (NUM_BLOCK_KLT + 1); + uint16_t pixLo = pixStep; + /* align with pixBaseStep */ + pixStep = ((uint16_t)(pixStep )) & ~((uint16_t)(pixBaseStep - 1)); // round down + pixLo = ((uint16_t)(pixLo + (pixBaseStep - 1))) & ~((uint16_t)(pixBaseStep - 1)); // round up + //uint16_t pixHi = pixLo + pixStep * (NUM_BLOCK_KLT - 1); + + j = pixLo; + for (int y = 0; y < NUM_BLOCK_KLT; y++, j += pixStep) + { + i = pixLo; + for (int x = 0; x < NUM_BLOCK_KLT; x++, i += pixStep) { - if (histx[j] > maxvaluex) - { - maxvaluex = histx[j]; - maxpositionx = j; - } - if (histy[j] > maxvaluey) - { - maxvaluey = histy[j]; - maxpositiony = j; + uint16_t idx = y*NUM_BLOCK_KLT+x; + /* use the gyro measurement to guess the initial position in the new image */ + us[idx] = i + x_rate; //position in new image at level 0 + vs[idx] = j + y_rate; + if ((int16_t)us[idx] < HALF_PATCH_SIZE) us[idx] = HALF_PATCH_SIZE; + if ((int16_t)us[idx] > frame_size - HALF_PATCH_SIZE - 1) us[idx] = frame_size - HALF_PATCH_SIZE - 1; + if ((int16_t)vs[idx] < HALF_PATCH_SIZE) vs[idx] = HALF_PATCH_SIZE; + if ((int16_t)vs[idx] > frame_size - HALF_PATCH_SIZE - 1) vs[idx] = frame_size - HALF_PATCH_SIZE - 1; + is[idx] = i; //position in previous image at level 0 + js[idx] = j; + /* init output vector */ + if (idx < max_out) { + out[idx].x = 0; + out[idx].y = 0; + out[idx].quality = 0; + out[idx].at_x = i; + out[idx].at_y = j; } } + } - /* check if there is a peak value in histogram */ - if (1) //(histx[maxpositionx] > meancount / 6 && histy[maxpositiony] > meancount / 6) + //for all pyramid levels, start from the smallest level + for (int l = PYR_LVLS-1; l >= 0; l--) + { + //iterate over all patterns + for (int k = 0; k < NUM_BLOCK_KLT*NUM_BLOCK_KLT; k++) { - if (global_data.param[PARAM_BOTTOM_FLOW_HIST_FILTER]) - { + uint16_t i = is[k] >> l; //reference pixel for the current level + uint16_t j = js[k] >> l; - /* use histogram filter peek value */ - uint16_t hist_x_min = maxpositionx; - uint16_t hist_x_max = maxpositionx; - uint16_t hist_y_min = maxpositiony; - uint16_t hist_y_max = maxpositiony; + uint16_t iwidth = frame_size >> l; + uint8_t *base1 = image1 + lvl_ofs[l] + j * iwidth + i; - /* x direction */ - if (maxpositionx > 1 && maxpositionx < hist_size-2) - { - hist_x_min = maxpositionx - 2; - hist_x_max = maxpositionx + 2; - } - else if (maxpositionx == 0) - { - hist_x_min = maxpositionx; - hist_x_max = maxpositionx + 2; - } - else if (maxpositionx == hist_size-1) - { - hist_x_min = maxpositionx - 2; - hist_x_max = maxpositionx; - } - else if (maxpositionx == 1) - { - hist_x_min = maxpositionx - 1; - hist_x_max = maxpositionx + 2; - } - else if (maxpositionx == hist_size-2) - { - hist_x_min = maxpositionx - 2; - hist_x_max = maxpositionx + 1; - } + float JTJ[4]; //the 2x2 Hessian + JTJ[0] = 0; + JTJ[1] = 0; + JTJ[2] = 0; + JTJ[3] = 0; + int c = 0; - /* y direction */ - if (maxpositiony > 1 && maxpositiony < hist_size-2) - { - hist_y_min = maxpositiony - 2; - hist_y_max = maxpositiony + 2; - } - else if (maxpositiony == 0) - { - hist_y_min = maxpositiony; - hist_y_max = maxpositiony + 2; - } - else if (maxpositiony == hist_size-1) - { - hist_y_min = maxpositiony - 2; - hist_y_max = maxpositiony; - } - else if (maxpositiony == 1) - { - hist_y_min = maxpositiony - 1; - hist_y_max = maxpositiony + 2; - } - else if (maxpositiony == hist_size-2) - { - hist_y_min = maxpositiony - 2; - hist_y_max = maxpositiony + 1; - } - - float hist_x_value = 0.0f; - float hist_x_weight = 0.0f; - - float hist_y_value = 0.0f; - float hist_y_weight = 0.0f; - - for (uint8_t i = hist_x_min; i < hist_x_max+1; i++) - { - hist_x_value += (float) (i*histx[i]); - hist_x_weight += (float) histx[i]; - } - - for (uint8_t i = hist_y_min; i global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD]) + //Now do some Gauss-Newton iterations for flow + for (int iters = 0; iters < 5; iters++) { - /* calc pixel of gyro */ - float y_rate_pixel = y_rate * (get_time_between_images() / 1000000.0f) * focal_length_px; - float comp_x = histflowx + y_rate_pixel; - - /* clamp value to maximum search window size plus half pixel from subpixel search */ - if (comp_x < (-SEARCH_SIZE - 0.5f)) - *pixel_flow_x = (-SEARCH_SIZE - 0.5f); - else if (comp_x > (SEARCH_SIZE + 0.5f)) - *pixel_flow_x = (SEARCH_SIZE + 0.5f); - else - *pixel_flow_x = comp_x; - } - else - { - *pixel_flow_x = histflowx; - } + float JTe_x = 0; //accumulators for Jac transposed times error + float JTe_y = 0; + + uint8_t *base2 = image2 + lvl_ofs[l] + (uint16_t)v * iwidth + (uint16_t)u; + + //extract bilinearly filtered pixel values for the current location in image2 + float dX = u - floorf(u); + float dY = v - floorf(v); + float fMixTL = (1.f - dX) * (1.f - dY); + float fMixTR = (dX) * (1.f - dY); + float fMixBL = (1.f - dX) * (dY); + float fMixBR = (dX) * (dY); + + float chi_sq = 0.f; + int c = 0; + for (int8_t jj = -HALF_PATCH_SIZE; jj <= HALF_PATCH_SIZE; jj++) + { + uint8_t *left1 = base1 + jj*iwidth; + uint8_t *left2 = base2 + jj*iwidth; + + for (int8_t ii = -HALF_PATCH_SIZE; ii <= HALF_PATCH_SIZE; ii++) + { + float fPixel = fMixTL * left2[ii] + fMixTR * left2[ii+1] + fMixBL * left2[ii+iwidth] + fMixBR * left2[ii+iwidth+1]; + float fDiff = fPixel - left1[ii]; + JTe_x += fDiff * Jx[c]; + JTe_y += fDiff * Jy[c]; + chi_sq += fDiff*fDiff; + c++; + } + } - if(fabsf(x_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD]) - { - /* calc pixel of gyro */ - float x_rate_pixel = x_rate * (get_time_between_images() / 1000000.0f) * focal_length_px; - float comp_y = histflowy - x_rate_pixel; - - /* clamp value to maximum search window size plus/minus half pixel from subpixel search */ - if (comp_y < (-SEARCH_SIZE - 0.5f)) - *pixel_flow_y = (-SEARCH_SIZE - 0.5f); - else if (comp_y > (SEARCH_SIZE + 0.5f)) - *pixel_flow_y = (SEARCH_SIZE + 0.5f); + //only update if the error got smaller + if (iters == 0 || chi_sq_previous > chi_sq) + { + //compute update and shift current position accordingly + float updx = JTJinv[0]*JTe_x + JTJinv[1]*JTe_y; + float updy = JTJinv[2]*JTe_x + JTJinv[3]*JTe_y; + float new_u = u-updx; + float new_v = v-updy; + + //check if we drifted outside the image + if (((int16_t)new_u < HALF_PATCH_SIZE) || (int16_t)new_u > (iwidth-HALF_PATCH_SIZE-1) || ((int16_t)new_v < HALF_PATCH_SIZE) || (int16_t)new_v > (iwidth-HALF_PATCH_SIZE-1)) + { + result_good = false; + break; + } + else + { + u = new_u; + v = new_v; + } + } else - *pixel_flow_y = comp_y; + { + chi_sum += chi_sq_previous; + chicount++; + break; + } + chi_sq_previous = chi_sq; } - else + if (l > 0) { - *pixel_flow_y = histflowy; + us[k] = u * (1<> (l-1); - uint16_t tar_size = frame_size >> l; - uint8_t *source = &image2[lvl_off[l-1]]; //pointer to the beginning of the previous level - uint8_t *target = &image2[lvl_off[l]]; //pointer to the beginning of the current level - for (j = 0; j < tar_size; j++) - for (i = 0; i < tar_size; i+=2) - { - //subsample the image by 2, use the halving-add instruction to do so - uint32_t l1 = (__UHADD8(*((uint32_t*) &source[(j*2+0)*src_size + i*2]), *((uint32_t*) &source[(j*2+0)*src_size + i*2+1]))); - uint32_t l2 = (__UHADD8(*((uint32_t*) &source[(j*2+1)*src_size + i*2]), *((uint32_t*) &source[(j*2+1)*src_size + i*2+1]))); - uint32_t r = __UHADD8(l1, l2); - - //the first and the third byte are the values we want to have - target[j*tar_size + i+0] = (uint8_t) r; - target[j*tar_size + i+1] = (uint8_t) (r>>16); - } - } - - //need to store the flow values between pyramid level changes - float us[NUM_BLOCK_KLT*NUM_BLOCK_KLT]; - float vs[NUM_BLOCK_KLT*NUM_BLOCK_KLT]; - uint16_t is[NUM_BLOCK_KLT*NUM_BLOCK_KLT]; - uint16_t js[NUM_BLOCK_KLT*NUM_BLOCK_KLT]; - - - //initialize flow values with the pixel value of the previous image - uint16_t pixLo = frame_size / (NUM_BLOCK_KLT + 1); - if (pixLo < PATCH_SIZE) pixLo = PYR_LVLS * PATCH_SIZE; - uint16_t pixHi = (uint16_t)frame_size * NUM_BLOCK_KLT / (NUM_BLOCK_KLT + 1); - if (pixHi > (frame_size - PATCH_SIZE)) pixHi = frame_size - (PYR_LVLS * PATCH_SIZE); - uint16_t pixStep = (pixHi - pixLo) / (NUM_BLOCK_KLT-1); - - j = pixLo; - for (int y = 0; y < NUM_BLOCK_KLT; y++, j += pixStep) - { - i = pixLo; - for (int x = 0; x < NUM_BLOCK_KLT; x++, i += pixStep) - { - //TODO: for proper rotation compensation, insert gyro values here and then substract at the end - - us[y*NUM_BLOCK_KLT+x] = i - y_rate_pixel; //position in new image at level 0 - vs[y*NUM_BLOCK_KLT+x] = j + x_rate_pixel; - is[y*NUM_BLOCK_KLT+x] = i; //position in previous image at level 0 - js[y*NUM_BLOCK_KLT+x] = j; - } - } - - //for all pyramid levels, start from the smallest level - for (int l = PYR_LVLS-1; l >= 0; l--) - { - //iterate over all patterns - for (int k = 0; k < NUM_BLOCK_KLT*NUM_BLOCK_KLT; k++) - { - uint16_t i = is[k] >> l; //reference pixel for the current level - uint16_t j = js[k] >> l; - - uint16_t iwidth = frame_size >> l; - uint8_t *base1 = image1 + lvl_off[l] + j * iwidth + i; - - float JTJ[4]; //the 2x2 Hessian - JTJ[0] = 0; - JTJ[1] = 0; - JTJ[2] = 0; - JTJ[3] = 0; - int c = 0; - - //compute jacobians and the hessian for the patch at the current location - for (int8_t jj = -HALF_PATCH_SIZE; jj <= HALF_PATCH_SIZE; jj++) - { - uint8_t *left = base1 + jj*iwidth; - for (int8_t ii = -HALF_PATCH_SIZE; ii <= HALF_PATCH_SIZE; ii++) - { - const float jx = ((uint16_t)left[ii+1] - (uint16_t)left[ii-1]) * 0.5f; - const float jy = ((uint16_t)left[ii+iwidth] - (uint16_t)left[ii-iwidth]) * 0.5f; - Jx[c] = jx; - Jy[c] = jy; - JTJ[0] += jx*jx; - JTJ[1] += jx*jy; - JTJ[2] += jx*jy; - JTJ[3] += jy*jy; - c++; - } - } - - // us and vs store the sample position in level 0 pixel coordinates - float u = (us[k] / (1< chi_sq) - { - //compute inverse of hessian - float det = (JTJ[0]*JTJ[3]-JTJ[1]*JTJ[2]); - if (det != 0.f) - { - float detinv = 1.f / det; - float JTJinv[4]; - JTJinv[0] = detinv * JTJ[3]; - JTJinv[1] = detinv * -JTJ[1]; - JTJinv[2] = detinv * -JTJ[2]; - JTJinv[3] = detinv * JTJ[0]; - - //compute update and shift current position accordingly - float updx = JTJinv[0]*JTe_x + JTJinv[1]*JTe_y; - float updy = JTJinv[2]*JTe_x + JTJinv[3]*JTe_y; - float new_u = u-updx; - float new_v = v-updy; - - //check if we drifted outside the image - if (((int16_t)new_u < HALF_PATCH_SIZE) || (int16_t)new_u > (iwidth-HALF_PATCH_SIZE-1) || ((int16_t)new_v < HALF_PATCH_SIZE) || (int16_t)new_v > (iwidth-HALF_PATCH_SIZE-1)) - { - break; - } - else - { - u = new_u; - v = new_v; - } - } - else - break; - } - else - break; - chi_sum += chi_sq; - chicount++; - chi_sq_previous = chi_sq; - } - - if (l > 0) - { - us[k] = u * (1< 0) -{ - meanflowx /= meancount; - meanflowy /= meancount; - - if (global_data.param[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION]) - { - if(fabsf(x_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD]) - *pixel_flow_x = meanflowx - y_rate_pixel; // + (x_rate_px_round - x_rate_pixel); - else - *pixel_flow_x = meanflowx; - - if(fabsf(y_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD]) - *pixel_flow_y = meanflowy + x_rate_pixel; // + (y_rate_px_round - y_rate_pixel); - else - *pixel_flow_y = meanflowy; - - - } - else - { - /* without gyro compensation */ - *pixel_flow_x = meanflowx; - *pixel_flow_y = meanflowy; - } - -} -else +uint8_t flow_extract_result(flow_raw_result *in, uint16_t result_count, float *px_flow_x, float *px_flow_y) { - *pixel_flow_x = 0.0f; - *pixel_flow_y = 0.0f; - return 0; -} - -/* create flow image if needed (image1 is not needed anymore) - * -> can be used for debugging purpose - */ -if (global_data.param[PARAM_USB_SEND_VIDEO] )//&& global_data.param[PARAM_VIDEO_USB_MODE] == FLOW_VIDEO) -{ - - for (j = pixLo; j < pixHi; j += pixStep) - { - for (i = pixLo; i < pixHi; i += pixStep) - { - - - image1[j * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i] = 255; - int pixel = (j - (int)(meanflowx*4+0.5f)) * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + (i - (int)(meanflowy*4+0.5f)); - if(pixel >= 0 && pixel < global_data.param[PARAM_IMAGE_WIDTH]*global_data.param[PARAM_IMAGE_WIDTH]) - image1[pixel] = 200; - - + /* calculate mean */ + float sumx = 0; + float sumy = 0; + uint16_t avg_c = 0; + for (int i = 0; i < result_count; i++) { + if (in[i].quality > 0) { + sumx += in[i].x; + sumy += in[i].y; + avg_c++; } } + if (avg_c > 0) { + *px_flow_x = sumx / avg_c; + *px_flow_y = sumy / avg_c; + return avg_c * 255 / result_count; + } else { + *px_flow_x = 0; + *px_flow_y = 0; + return 0; + } } -/* return quality */ -//return (uint8_t)(meancount * 255 / (NUM_BLOCK_KLT*NUM_BLOCK_KLT)); -return chi_sum/chicount; -} \ No newline at end of file diff --git a/src/main.c b/src/main.c index 23a0957..00c7978 100644 --- a/src/main.c +++ b/src/main.c @@ -319,10 +319,10 @@ int main(void) dma_copy_image_buffers(¤t_image, &previous_image, image_size, (int)(global_data.param[PARAM_CAMERA_FRAME_INTERVAL] + 0.5)); float frame_dt = get_time_between_images() * 0.000001f; - /* compute gyro rate in pixels */ - float x_rate_px = x_rate * (focal_length_px * frame_dt); - float y_rate_px = y_rate * (focal_length_px * frame_dt); - float z_rate_px = z_rate * (focal_length_px * frame_dt); + /* 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); + float z_rate_fr = z_rate * frame_dt; /* filter the new image */ if (global_data.param[PARAM_USE_IMAGE_FILTER]) { @@ -330,13 +330,34 @@ int main(void) } /* compute optical flow in pixels */ - float pixel_flow_x = 0.0f; - float pixel_flow_y = 0.0f; - uint8_t qual = 0; + flow_raw_result flow_rslt[32]; + uint16_t flow_rslt_count = 0; if (global_data.param[PARAM_ALGORITHM_CHOICE] == 0) { - qual = compute_flow(previous_image, current_image, x_rate_px, y_rate_px, z_rate_px, &pixel_flow_x, &pixel_flow_y); + flow_rslt_count = compute_flow(previous_image, current_image, x_rate_px, y_rate_px, z_rate_fr, flow_rslt, 32); } else { - qual = compute_klt(previous_image, current_image, x_rate_px, y_rate_px, z_rate_px, &pixel_flow_x, &pixel_flow_y); + flow_rslt_count = compute_klt(previous_image, current_image, x_rate_px, y_rate_px, z_rate_fr, flow_rslt, 32); + } + + /* calculate flow value from the raw results */ + float pixel_flow_x; + float pixel_flow_y; + uint8_t qual = flow_extract_result(flow_rslt, flow_rslt_count, &pixel_flow_x, &pixel_flow_y); + + /* create flow image if needed (previous_image is not needed anymore) + * -> can be used for debugging purpose + */ + if (global_data.param[PARAM_USB_SEND_VIDEO]) + { + uint16_t frame_size = global_data.param[PARAM_IMAGE_WIDTH]; + for (int i = 0; i < flow_rslt_count; i++) { + if (flow_rslt[i].quality > 0) { + previous_image[flow_rslt[i].at_y * frame_size + flow_rslt[i].at_x] = 255; + int ofs = (int)floor(flow_rslt[i].at_y + flow_rslt[i].y * 2 + 0.5f) * frame_size + (int)floor(flow_rslt[i].at_x + flow_rslt[i].x * 2 + 0.5f); + if (ofs >= 0 && ofs < frame_size * frame_size) { + previous_image[ofs] = 200; + } + } + } } /* decide which distance to use */ diff --git a/src/timer.c b/src/timer.c index 2bb0756..0ffa584 100644 --- a/src/timer.c +++ b/src/timer.c @@ -47,9 +47,6 @@ #define __ASM asm #include "core_cmInstr.h" - -#define MS_TIMER_COUNT 100 /* steps in 10 microseconds ticks */ - /* boot time in milliseconds ticks */ volatile uint32_t boot_time_ms = 0; volatile uint32_t boot_time_us_base = 0; From b3552db713f8dfbfded4d1497164a57432aae8c6 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Thu, 9 Jul 2015 23:24:43 +0200 Subject: [PATCH 015/120] added some remarks about future improvements --- src/flow.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/flow.c b/src/flow.c index 4decc4e..b385071 100644 --- a/src/flow.c +++ b/src/flow.c @@ -618,6 +618,7 @@ uint16_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat } //compute inverse of hessian + // TODO: evaluate using condition of this matrix to decide whether we should continue float det = (JTJ[0]*JTJ[3]-JTJ[1]*JTJ[2]); if (det != 0.f) { @@ -701,6 +702,7 @@ uint16_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat } if (l > 0) { + // TODO: evaluate recording failure at each level to calculate a final quality value us[k] = u * (1< Date: Fri, 10 Jul 2015 12:32:45 +0200 Subject: [PATCH 016/120] Gyro compensation & bug fixes - Fixed Gyro compensation - Added global parameters for result filtering - small fixes in result accumulator --- inc/flow.h | 13 +++++++- inc/result_accumulator.h | 7 +++-- inc/settings.h | 4 +++ src/flow.c | 67 ++++++++++++++++++++++++++++------------ src/i2c.c | 5 +-- src/main.c | 16 ++++++---- src/result_accumulator.c | 24 ++++++++------ src/settings.c | 20 +++++++++--- 8 files changed, 111 insertions(+), 45 deletions(-) diff --git a/inc/flow.h b/inc/flow.h index 5a0b0d7..1ac24ac 100644 --- a/inc/flow.h +++ b/inc/flow.h @@ -75,6 +75,17 @@ uint16_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_ra uint16_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate, flow_raw_result *out, uint16_t max_out); -uint8_t flow_extract_result(flow_raw_result *in, uint16_t result_count, float *px_flow_x, float *px_flow_y); +/** +* +* @brief Extracts pixel flow from the result vector +* @param in Raw result vector from flow calculation. +* @param result_count Number of results in flow_raw_result. +* @param px_flow_x Receives the pixel flow in x direction. +* @param px_flow_y Receives the pixel flow in y direction. +* @param accuracy_p Outlier detection threshold in percent. (0 - 1). +* @param accuracy_px Minimum outlier detection threshold in absolute pixel flow values. +*/ +uint8_t flow_extract_result(flow_raw_result *in, uint16_t result_count, float *px_flow_x, float *px_flow_y, + float accuracy_p, float accuracy_px); #endif /* FLOW_H_ */ diff --git a/inc/result_accumulator.h b/inc/result_accumulator.h index a1e6fcb..7a2e545 100644 --- a/inc/result_accumulator.h +++ b/inc/result_accumulator.h @@ -60,6 +60,7 @@ typedef struct _result_accumulator_ctx { float rad_flow_x_accu; float rad_flow_y_accu; uint8_t min_quality; + uint16_t data_count; uint16_t valid_data_count; float valid_time; float full_time; @@ -135,9 +136,9 @@ void result_accumulator_feed(result_accumulator_ctx *ctx, float dt, float x_rate /** Recalculates the output values of the result_accumulator. Call this before using any of the output values. */ -void result_accumulator_calculate_output_flow(result_accumulator_ctx *ctx, uint16_t min_valid_data_count, result_accumulator_output_flow *out); -void result_accumulator_calculate_output_flow_rad(result_accumulator_ctx *ctx, uint16_t min_valid_data_count, result_accumulator_output_flow_rad *out); -void result_accumulator_calculate_output_flow_i2c(result_accumulator_ctx *ctx, uint16_t min_valid_data_count, result_accumulator_output_flow_i2c *out); +void result_accumulator_calculate_output_flow(result_accumulator_ctx *ctx, uint16_t min_valid_data_count_percent, result_accumulator_output_flow *out); +void result_accumulator_calculate_output_flow_rad(result_accumulator_ctx *ctx, uint16_t min_valid_data_count_percent, result_accumulator_output_flow_rad *out); +void result_accumulator_calculate_output_flow_i2c(result_accumulator_ctx *ctx, uint16_t min_valid_data_count_percent, result_accumulator_output_flow_i2c *out); /** Resets the accumulator to prepare it for the next accumulation phase. */ diff --git a/inc/settings.h b/inc/settings.h index 48d88bc..016ebed 100644 --- a/inc/settings.h +++ b/inc/settings.h @@ -151,6 +151,10 @@ enum global_param_id_t PARAM_SENSOR_POSITION, DEBUG_VARIABLE, + PARAM_MINIMUM_OUTLIER_THRESHOLD, + PARAM_OUTLIER_THRESHOLD, + PARAM_MIN_VALID_RATIO, + ONBOARD_PARAM_COUNT }; diff --git a/src/flow.c b/src/flow.c index ca79be1..3bae186 100644 --- a/src/flow.c +++ b/src/flow.c @@ -479,9 +479,11 @@ uint16_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_ra if (mindir == 3 || mindir == 4 || mindir == 5) result->x -= 0.5f; if (mindir == 5 || mindir == 6 || mindir == 7) result->y -= 0.5f; if (mindir == 1 || mindir == 2 || mindir == 3) result->y += 0.5f; - /* gyro compensation */ - result->x -= x_rate; - result->y -= y_rate; + if (global_data.param[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION]) { + /* gyro compensation */ + result->x -= x_rate; + result->y -= y_rate; + } result->quality = 1.0; } } @@ -560,13 +562,18 @@ uint16_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat for (int x = 0; x < NUM_BLOCK_KLT; x++, i += pixStep) { uint16_t idx = y*NUM_BLOCK_KLT+x; - /* use the gyro measurement to guess the initial position in the new image */ - us[idx] = i + x_rate; //position in new image at level 0 - vs[idx] = j + y_rate; - if ((int16_t)us[idx] < HALF_PATCH_SIZE) us[idx] = HALF_PATCH_SIZE; - if ((int16_t)us[idx] > frame_size - HALF_PATCH_SIZE - 1) us[idx] = frame_size - HALF_PATCH_SIZE - 1; - if ((int16_t)vs[idx] < HALF_PATCH_SIZE) vs[idx] = HALF_PATCH_SIZE; - if ((int16_t)vs[idx] > frame_size - HALF_PATCH_SIZE - 1) vs[idx] = frame_size - HALF_PATCH_SIZE - 1; + if (global_data.param[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION]) { + /* use the gyro measurement to guess the initial position in the new image */ + us[idx] = i + x_rate; //position in new image at level 0 + vs[idx] = j + y_rate; + if ((int16_t)us[idx] < HALF_PATCH_SIZE) us[idx] = HALF_PATCH_SIZE; + if ((int16_t)us[idx] > frame_size - HALF_PATCH_SIZE - 1) us[idx] = frame_size - HALF_PATCH_SIZE - 1; + if ((int16_t)vs[idx] < HALF_PATCH_SIZE) vs[idx] = HALF_PATCH_SIZE; + if ((int16_t)vs[idx] > frame_size - HALF_PATCH_SIZE - 1) vs[idx] = frame_size - HALF_PATCH_SIZE - 1; + } else { + us[idx] = i; //position in new image at level 0 + vs[idx] = j; + } is[idx] = i; //position in previous image at level 0 js[idx] = j; /* init output vector */ @@ -709,9 +716,14 @@ uint16_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat else //for the last level compute the actual flow in pixels { if (result_good && k < max_out) { - /* compute flow and compensate gyro */ - out[k].x = u - i - x_rate; - out[k].y = v - j - y_rate; + if (global_data.param[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION]) { + /* compute flow and compensate gyro */ + out[k].x = u - i - x_rate; + out[k].y = v - j - y_rate; + } else { + out[k].x = u - i; + out[k].y = v - j; + } out[k].quality = 1.0f; } } @@ -735,7 +747,8 @@ int flow_res_dim_value_compare(const void* elem1, const void* elem2) return v1 > v2; } -uint8_t flow_extract_result(flow_raw_result *in, uint16_t result_count, float *px_flow_x, float *px_flow_y) +uint8_t flow_extract_result(flow_raw_result *in, uint16_t result_count, float *px_flow_x, float *px_flow_y, + float accuracy_p, float accuracy_px) { /* extract all valid results: */ struct flow_res_dim_value xvalues[result_count]; @@ -757,13 +770,14 @@ uint8_t flow_extract_result(flow_raw_result *in, uint16_t result_count, float *p } struct flow_res_dim_value *axes[2] = {xvalues, yvalues}; float *output[2] = {px_flow_x, px_flow_y}; - const float accuracy_p = 0.1f; ///< accuracy in percent (0 - 1) - const float accuracy_rad = 0.2e-3f; ///< minimal absolute accuracy + float max_spread_val[2] = {}; + float spread_val[2] = {}; uint16_t total_avg_c = 0; for (int i = 0; i < 2; ++i) { struct flow_res_dim_value *axis = axes[i]; /* sort them */ qsort(axis, valid_c, sizeof(struct flow_res_dim_value), flow_res_dim_value_compare); + spread_val[i] = axis[valid_c * 3 / 4].value - axis[valid_c / 4].value; /* start with one element */ uint16_t s = valid_c / 2; uint16_t e = valid_c / 2 + 1; @@ -778,7 +792,8 @@ uint8_t flow_extract_result(flow_raw_result *in, uint16_t result_count, float *p /* calculate average and maximum spread to throw away outliers */ avg = avg_sum / avg_c; float max_spread = fabs(avg) * accuracy_p; - max_spread = accuracy_rad * accuracy_rad / (accuracy_rad + max_spread) + max_spread; + max_spread = accuracy_px * accuracy_px / (accuracy_px + max_spread) + max_spread; + max_spread_val[i] = max_spread; /* decide on which side to add new data-point (to remain centered in the sorted set) */ if (s > valid_c - e && s > 0) { s--; @@ -794,10 +809,22 @@ uint8_t flow_extract_result(flow_raw_result *in, uint16_t result_count, float *p if (axis[e - 1].value - axis[s].value > max_spread) break; /* its good. continue .. */ } - /* we have a result */ - *output[i] = avg; - total_avg_c += e_res - s_res; + if (e_res - s_res > 1) { + /* we have a result */ + *output[i] = avg; + total_avg_c += e_res - s_res; + } else { + *px_flow_x = 0; + *px_flow_y = 0; + return 0; + } + } + static int ctr = 0; + if (ctr++ % 10 == 0) { + mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "SPREAD_X", get_boot_time_us(), spread_val[0], max_spread_val[0], *output[0]); + mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "SPREAD_Y", get_boot_time_us(), spread_val[1], max_spread_val[1], *output[1]); } + total_avg_c = total_avg_c / 2; return (total_avg_c * 255) / result_count; } diff --git a/src/i2c.c b/src/i2c.c index 6f63185..2358b0d 100644 --- a/src/i2c.c +++ b/src/i2c.c @@ -246,8 +246,9 @@ void update_TX_buffer(float dt, float x_rate, float y_rate, float z_rate, int16_ result_accumulator_output_flow output_flow; result_accumulator_output_flow_i2c output_i2c; - result_accumulator_calculate_output_flow(&accumulator, 1, &output_flow); - result_accumulator_calculate_output_flow_i2c(&accumulator, 1, &output_i2c); + int min_valid_ratio = global_data.param[PARAM_MIN_VALID_RATIO]; + result_accumulator_calculate_output_flow(&accumulator, min_valid_ratio, &output_flow); + result_accumulator_calculate_output_flow_i2c(&accumulator, min_valid_ratio, &output_i2c); i2c_frame f; i2c_integral_frame f_integral; diff --git a/src/main.c b/src/main.c index ccbf650..4cfcf29 100644 --- a/src/main.c +++ b/src/main.c @@ -321,9 +321,9 @@ int main(void) float frame_dt = get_time_between_images() * 0.000001f; /* 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); - float z_rate_fr = z_rate * frame_dt; + float x_rate_px = - y_rate * (focal_length_px * frame_dt); + float y_rate_px = x_rate * (focal_length_px * frame_dt); + float z_rate_fr = - z_rate * frame_dt; /* filter the new image */ if (global_data.param[PARAM_USE_IMAGE_FILTER]) { @@ -342,7 +342,10 @@ int main(void) /* calculate flow value from the raw results */ float pixel_flow_x; float pixel_flow_y; - uint8_t qual = flow_extract_result(flow_rslt, flow_rslt_count, &pixel_flow_x, &pixel_flow_y); + float outlier_threshold = global_data.param[PARAM_OUTLIER_THRESHOLD]; + float min_outlier_threshold = global_data.param[PARAM_MINIMUM_OUTLIER_THRESHOLD]; + uint8_t qual = flow_extract_result(flow_rslt, flow_rslt_count, &pixel_flow_x, &pixel_flow_y, + outlier_threshold, min_outlier_threshold); /* create flow image if needed (previous_image is not needed anymore) * -> can be used for debugging purpose @@ -393,8 +396,9 @@ int main(void) /* recalculate the output values */ result_accumulator_output_flow output_flow; result_accumulator_output_flow_rad output_flow_rad; - result_accumulator_calculate_output_flow(&mavlink_accumulator, 1, &output_flow); - result_accumulator_calculate_output_flow_rad(&mavlink_accumulator, 1, &output_flow_rad); + int min_valid_ratio = global_data.param[PARAM_MIN_VALID_RATIO]; + result_accumulator_calculate_output_flow(&mavlink_accumulator, min_valid_ratio, &output_flow); + result_accumulator_calculate_output_flow_rad(&mavlink_accumulator, min_valid_ratio, &output_flow_rad); // send flow mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], diff --git a/src/result_accumulator.c b/src/result_accumulator.c index 2512b81..5571499 100644 --- a/src/result_accumulator.c +++ b/src/result_accumulator.c @@ -82,20 +82,21 @@ void result_accumulator_feed(result_accumulator_ctx *ctx, float dt, float x_rate ctx->valid_data_count++; ctx->valid_time += dt; } + ctx->data_count++; ctx->frame_count++; ctx->full_time += dt; ctx->last.frame_count = ctx->frame_count; } -void result_accumulator_calculate_output_flow(result_accumulator_ctx *ctx, uint16_t min_valid_data_count, result_accumulator_output_flow *out) +void result_accumulator_calculate_output_flow(result_accumulator_ctx *ctx, uint16_t min_valid_data_count_percent, result_accumulator_output_flow *out) { - if (ctx->valid_data_count >= min_valid_data_count) { + if (ctx->valid_data_count * 100u >= ctx->data_count * min_valid_data_count_percent && ctx->valid_data_count > 0) { /*** calculate the output values for the flow mavlink message ***/ /* scale the averaged values from valid time to full time (we extrapolate the invalid data sets) */ float time_scaling_f = ctx->full_time / ctx->valid_time; - out->flow_x = floor(ctx->px_flow_x_accu * time_scaling_f + 0.5f); - out->flow_y = floor(ctx->px_flow_y_accu * time_scaling_f + 0.5f); + out->flow_x = floor(ctx->px_flow_x_accu * time_scaling_f * 10.0f + 0.5f); + out->flow_y = floor(ctx->px_flow_y_accu * time_scaling_f * 10.0f + 0.5f); out->flow_comp_m_x = 0; out->flow_comp_m_y = 0; out->quality = ctx->min_quality; @@ -112,9 +113,9 @@ void result_accumulator_calculate_output_flow(result_accumulator_ctx *ctx, uint1 } } -void result_accumulator_calculate_output_flow_rad(result_accumulator_ctx *ctx, uint16_t min_valid_data_count, result_accumulator_output_flow_rad *out) +void result_accumulator_calculate_output_flow_rad(result_accumulator_ctx *ctx, uint16_t min_valid_data_count_percent, result_accumulator_output_flow_rad *out) { - if (ctx->valid_data_count >= min_valid_data_count) { + if (ctx->valid_data_count * 100u >= ctx->data_count * min_valid_data_count_percent && ctx->valid_data_count > 0) { /*** calculate the output values for the flow_rad mavlink message ***/ out->integration_time = ctx->valid_time * 1000000.0f; out->integrated_x = ctx->rad_flow_x_accu; @@ -135,15 +136,16 @@ void result_accumulator_calculate_output_flow_rad(result_accumulator_ctx *ctx, u out->integrated_xgyro = 0; out->integrated_ygyro = 0; out->integrated_zgyro = 0; + out->quality = 0; out->temperature = ctx->last.temperature; out->time_delta_distance_us = 0; out->ground_distance = -1; } } -void result_accumulator_calculate_output_flow_i2c(result_accumulator_ctx *ctx, uint16_t min_valid_data_count, result_accumulator_output_flow_i2c *out) +void result_accumulator_calculate_output_flow_i2c(result_accumulator_ctx *ctx, uint16_t min_valid_data_count_percent, result_accumulator_output_flow_i2c *out) { - if (ctx->valid_data_count >= min_valid_data_count) { + if (ctx->valid_data_count * 100u >= ctx->data_count * min_valid_data_count_percent && ctx->valid_data_count > 0) { /*** calculate the output values for the i2c message ***/ out->frame_count = ctx->frame_count; out->valid_frames = ctx->valid_data_count; @@ -171,13 +173,16 @@ void result_accumulator_calculate_output_flow_i2c(result_accumulator_ctx *ctx, u out->integration_time = 0; out->rad_flow_x = 0; out->rad_flow_y = 0; + out->integrated_gyro_x = 0; + out->integrated_gyro_y = 0; + out->integrated_gyro_z = 0; out->gyro_x = 0; out->gyro_y = 0; out->gyro_z = 0; out->quality = 0; out->temperature = ctx->last.temperature; out->time_delta_distance_us = 0; - out->ground_distance = -1; + out->ground_distance = 0; } } @@ -196,6 +201,7 @@ void result_accumulator_reset(result_accumulator_ctx *ctx) ctx->min_quality = 255; ctx->valid_data_count = 0; + ctx->data_count = 0; ctx->valid_time = 0; ctx->full_time = 0; } diff --git a/src/settings.c b/src/settings.c index d75a57a..1990f25 100644 --- a/src/settings.c +++ b/src/settings.c @@ -78,15 +78,15 @@ void global_data_reset_param_defaults(void){ global_data.param_access[PARAM_SYSTEM_SEND_LPOS] = READ_WRITE; - global_data.param[PARAM_ALGORITHM_CHOICE] = 0; - strcpy(global_data.param_name[PARAM_ALGORITHM_CHOICE], "ALGORITHM_CHOICE"); + global_data.param[PARAM_ALGORITHM_CHOICE] = 1; + strcpy(global_data.param_name[PARAM_ALGORITHM_CHOICE], "ALGO_CHOICE"); global_data.param_access[PARAM_ALGORITHM_CHOICE] = READ_WRITE; global_data.param[PARAM_USE_IMAGE_FILTER] = 0; - strcpy(global_data.param_name[PARAM_USE_IMAGE_FILTER], "USE_IMAGE_FILTER"); + strcpy(global_data.param_name[PARAM_USE_IMAGE_FILTER], "USE_IMAGE_FILT"); global_data.param_access[PARAM_USE_IMAGE_FILTER] = READ_WRITE; - global_data.param[PARAM_FRAME_INTERVAL] = 2; + global_data.param[PARAM_FRAME_INTERVAL] = 1; strcpy(global_data.param_name[PARAM_FRAME_INTERVAL], "FRAME_INTERVAL"); global_data.param_access[PARAM_FRAME_INTERVAL] = READ_WRITE; @@ -222,6 +222,18 @@ void global_data_reset_param_defaults(void){ strcpy(global_data.param_name[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR], "BFLOW_THROTT"); global_data.param_access[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR] = READ_WRITE; + global_data.param[PARAM_MINIMUM_OUTLIER_THRESHOLD] = 0.1f; + strcpy(global_data.param_name[PARAM_MINIMUM_OUTLIER_THRESHOLD], "MIN_OUT_THRSH"); + global_data.param_access[PARAM_MINIMUM_OUTLIER_THRESHOLD] = READ_WRITE; + + global_data.param[PARAM_OUTLIER_THRESHOLD] = 0.2f; + strcpy(global_data.param_name[PARAM_OUTLIER_THRESHOLD], "OUTLIER_THRSH"); + global_data.param_access[PARAM_OUTLIER_THRESHOLD] = READ_WRITE; + + global_data.param[PARAM_MIN_VALID_RATIO] = 50; + strcpy(global_data.param_name[PARAM_MIN_VALID_RATIO], "MIN_VALID_RATIO"); + global_data.param_access[PARAM_MIN_VALID_RATIO] = READ_WRITE; + global_data.param[DEBUG_VARIABLE] = 1; strcpy(global_data.param_name[DEBUG_VARIABLE], "DEBUG"); global_data.param_access[DEBUG_VARIABLE] = READ_WRITE; From 02b16c8bdbf540af5de7dc182d598f71d6c48842 Mon Sep 17 00:00:00 2001 From: Severin Latkovic Date: Fri, 10 Jul 2015 13:18:15 +0200 Subject: [PATCH 017/120] - Added switch for the minimum outlier threshold for the two algorithms - Changed min amount of valid results from 2 to 3 --- inc/settings.h | 3 ++- src/flow.c | 2 +- src/main.c | 9 ++++++++- src/settings.c | 12 ++++++++---- 4 files changed, 19 insertions(+), 7 deletions(-) diff --git a/inc/settings.h b/inc/settings.h index 016ebed..09bd5b8 100644 --- a/inc/settings.h +++ b/inc/settings.h @@ -151,7 +151,8 @@ enum global_param_id_t PARAM_SENSOR_POSITION, DEBUG_VARIABLE, - PARAM_MINIMUM_OUTLIER_THRESHOLD, + PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_0, + PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_1, PARAM_OUTLIER_THRESHOLD, PARAM_MIN_VALID_RATIO, diff --git a/src/flow.c b/src/flow.c index 3bae186..5bb7a0c 100644 --- a/src/flow.c +++ b/src/flow.c @@ -763,7 +763,7 @@ uint8_t flow_extract_result(flow_raw_result *in, uint16_t result_count, float *p valid_c++; } } - if (valid_c < (result_count + 4) / 4 || valid_c < 2) { + if (valid_c < (result_count + 4) / 4 || valid_c < 3) { *px_flow_x = 0; *px_flow_y = 0; return 0; diff --git a/src/main.c b/src/main.c index 4cfcf29..290e263 100644 --- a/src/main.c +++ b/src/main.c @@ -343,7 +343,14 @@ int main(void) float pixel_flow_x; float pixel_flow_y; float outlier_threshold = global_data.param[PARAM_OUTLIER_THRESHOLD]; - float min_outlier_threshold = global_data.param[PARAM_MINIMUM_OUTLIER_THRESHOLD]; + float min_outlier_threshold = 0; + if(global_data.param[PARAM_ALGORITHM_CHOICE] == 0) + { + min_outlier_threshold = global_data.param[PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_0]; + }else + { + min_outlier_threshold = global_data.param[PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_1]; + } uint8_t qual = flow_extract_result(flow_rslt, flow_rslt_count, &pixel_flow_x, &pixel_flow_y, outlier_threshold, min_outlier_threshold); diff --git a/src/settings.c b/src/settings.c index 1990f25..00f9032 100644 --- a/src/settings.c +++ b/src/settings.c @@ -222,12 +222,16 @@ void global_data_reset_param_defaults(void){ strcpy(global_data.param_name[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR], "BFLOW_THROTT"); global_data.param_access[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR] = READ_WRITE; - global_data.param[PARAM_MINIMUM_OUTLIER_THRESHOLD] = 0.1f; - strcpy(global_data.param_name[PARAM_MINIMUM_OUTLIER_THRESHOLD], "MIN_OUT_THRSH"); - global_data.param_access[PARAM_MINIMUM_OUTLIER_THRESHOLD] = READ_WRITE; + global_data.param[PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_0] = 1.0f; + strcpy(global_data.param_name[PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_0], "MIN_OUT_THR_A0"); + global_data.param_access[PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_0] = READ_WRITE; + + global_data.param[PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_1] = 0.1f; + strcpy(global_data.param_name[PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_1], "MIN_OUT_THR_A1"); + global_data.param_access[PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_1] = READ_WRITE; global_data.param[PARAM_OUTLIER_THRESHOLD] = 0.2f; - strcpy(global_data.param_name[PARAM_OUTLIER_THRESHOLD], "OUTLIER_THRSH"); + strcpy(global_data.param_name[PARAM_OUTLIER_THRESHOLD], "OUTLIER_THR"); global_data.param_access[PARAM_OUTLIER_THRESHOLD] = READ_WRITE; global_data.param[PARAM_MIN_VALID_RATIO] = 50; From 995548a59df6f66eb18145d4a948b083dc9daf13 Mon Sep 17 00:00:00 2001 From: Severin Latkovic Date: Fri, 10 Jul 2015 16:03:55 +0200 Subject: [PATCH 018/120] - Added check if klt flow is outside of 2*topPyrStep. - Added global parameter PARAM_USB_SEND_QUAL_0. It sets whether to send packets with 0 quality or not. --- inc/settings.h | 2 ++ src/flow.c | 18 +++++++++++++----- src/main.c | 2 +- src/settings.c | 4 ++++ 4 files changed, 20 insertions(+), 6 deletions(-) diff --git a/inc/settings.h b/inc/settings.h index 09bd5b8..db57762 100644 --- a/inc/settings.h +++ b/inc/settings.h @@ -153,6 +153,8 @@ enum global_param_id_t PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_0, PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_1, + PARAM_USB_SEND_QUAL_0, + PARAM_OUTLIER_THRESHOLD, PARAM_MIN_VALID_RATIO, diff --git a/src/flow.c b/src/flow.c index 5bb7a0c..3067c8f 100644 --- a/src/flow.c +++ b/src/flow.c @@ -547,12 +547,12 @@ uint16_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat uint16_t js[NUM_BLOCK_KLT*NUM_BLOCK_KLT]; //initialize flow values with the pixel value of the previous image - uint16_t pixBaseStep = 1 << (PYR_LVLS - 1); + uint16_t topPyrStep = 1 << (PYR_LVLS - 1); uint16_t pixStep = frame_size / (NUM_BLOCK_KLT + 1); uint16_t pixLo = pixStep; - /* align with pixBaseStep */ - pixStep = ((uint16_t)(pixStep )) & ~((uint16_t)(pixBaseStep - 1)); // round down - pixLo = ((uint16_t)(pixLo + (pixBaseStep - 1))) & ~((uint16_t)(pixBaseStep - 1)); // round up + /* align with topPyrStep */ + pixStep = ((uint16_t)(pixStep )) & ~((uint16_t)(topPyrStep - 1)); // round down + pixLo = ((uint16_t)(pixLo + (topPyrStep - 1))) & ~((uint16_t)(topPyrStep - 1)); // round up //uint16_t pixHi = pixLo + pixStep * (NUM_BLOCK_KLT - 1); j = pixLo; @@ -724,7 +724,15 @@ uint16_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat out[k].x = u - i; out[k].y = v - j; } - out[k].quality = 1.0f; + if (fabs(out[k].x) < (float)(2 * topPyrStep) && + fabs(out[k].y) < (float)(2 * topPyrStep)) { + out[k].quality = 1.0f; + } else { + /* drifted too far */ + out[k].x = 0; + out[k].y = 0; + out[k].quality = 0; + } } } } diff --git a/src/main.c b/src/main.c index 290e263..aa70a7e 100644 --- a/src/main.c +++ b/src/main.c @@ -420,7 +420,7 @@ int main(void) output_flow_rad.temperature, output_flow_rad.quality, output_flow_rad.time_delta_distance_us,output_flow_rad.ground_distance); - if (global_data.param[PARAM_USB_SEND_FLOW]) + if (global_data.param[PARAM_USB_SEND_FLOW] && (qual > 0 || global_data.param[PARAM_USB_SEND_QUAL_0])) { mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], output_flow.flow_x, output_flow.flow_y, diff --git a/src/settings.c b/src/settings.c index 00f9032..e1d7fde 100644 --- a/src/settings.c +++ b/src/settings.c @@ -234,6 +234,10 @@ void global_data_reset_param_defaults(void){ strcpy(global_data.param_name[PARAM_OUTLIER_THRESHOLD], "OUTLIER_THR"); global_data.param_access[PARAM_OUTLIER_THRESHOLD] = READ_WRITE; + global_data.param[PARAM_USB_SEND_QUAL_0] = 1; + strcpy(global_data.param_name[PARAM_USB_SEND_QUAL_0], "SEND_QUAL_0"); + global_data.param_access[PARAM_USB_SEND_QUAL_0] = READ_WRITE; + global_data.param[PARAM_MIN_VALID_RATIO] = 50; strcpy(global_data.param_name[PARAM_MIN_VALID_RATIO], "MIN_VALID_RATIO"); global_data.param_access[PARAM_MIN_VALID_RATIO] = READ_WRITE; From ec201c5d7d3e0601630041ea4a49bfa553745464 Mon Sep 17 00:00:00 2001 From: Severin Latkovic Date: Fri, 10 Jul 2015 17:23:08 +0200 Subject: [PATCH 019/120] - Re-added flow in meter using the same calculation as before --- inc/result_accumulator.h | 5 +++++ src/result_accumulator.c | 27 +++++++++++++++++++++++++-- 2 files changed, 30 insertions(+), 2 deletions(-) diff --git a/inc/result_accumulator.h b/inc/result_accumulator.h index 7a2e545..93c304c 100644 --- a/inc/result_accumulator.h +++ b/inc/result_accumulator.h @@ -51,6 +51,8 @@ typedef struct _result_accumulator_ctx { float pixel_flow_y; ///< The measured y-flow in the current image in pixel. Sensor linear motion along the positive Y axis induces a negative flow. float flow_x_rad; ///< Flow in radians around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) float flow_y_rad; ///< Flow in radians around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) + float flow_x_m; ///< The measured x-flow in the current image in meters. + float flow_y_m; ///< The measured x-flow in the current image in meters. float ground_distance; ///< The measured distance to the ground in meter. uint32_t distance_age; ///< Age of the distance measurement in us. } last; @@ -59,9 +61,12 @@ typedef struct _result_accumulator_ctx { 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; diff --git a/src/result_accumulator.c b/src/result_accumulator.c index 5571499..faa4534 100644 --- a/src/result_accumulator.c +++ b/src/result_accumulator.c @@ -66,12 +66,20 @@ void result_accumulator_feed(result_accumulator_ctx *ctx, float dt, float x_rate ctx->last.flow_y_rad = - pixel_flow_x * rad_per_pixel; ctx->last.ground_distance = distance_valid ? ground_distance : -1; ctx->last.distance_age = distance_age; + if(ctx->last.ground_distance >= 0) { + ctx->last.flow_x_m = ctx->last.flow_x_rad * ctx->last.ground_distance; + ctx->last.flow_y_m = ctx->last.flow_y_rad * ctx->last.ground_distance; + }else{ + ctx->last.flow_x_m = 0; + ctx->last.flow_y_m = 0; + } /* accumulate the values */ if (qual > 0) { ctx->px_flow_x_accu += ctx->last.pixel_flow_x; ctx->px_flow_y_accu += ctx->last.pixel_flow_y; ctx->rad_flow_x_accu += ctx->last.flow_x_rad; ctx->rad_flow_y_accu += ctx->last.flow_y_rad; + ctx->gyro_x_accu += x_rate * dt; ctx->gyro_y_accu += y_rate * dt; ctx->gyro_z_accu += z_rate * dt; @@ -81,6 +89,11 @@ void result_accumulator_feed(result_accumulator_ctx *ctx, float dt, float x_rate } ctx->valid_data_count++; ctx->valid_time += dt; + if(ctx->last.ground_distance >= 0){ + ctx->m_flow_x_accu += ctx->last.flow_x_m; + ctx->m_flow_y_accu += ctx->last.flow_y_m; + ctx->valid_dist_time += dt; + } } ctx->data_count++; ctx->frame_count++; @@ -97,8 +110,14 @@ void result_accumulator_calculate_output_flow(result_accumulator_ctx *ctx, uint1 float time_scaling_f = ctx->full_time / ctx->valid_time; out->flow_x = floor(ctx->px_flow_x_accu * time_scaling_f * 10.0f + 0.5f); out->flow_y = floor(ctx->px_flow_y_accu * time_scaling_f * 10.0f + 0.5f); - out->flow_comp_m_x = 0; - out->flow_comp_m_y = 0; + if(ctx->valid_dist_time > 0){ + float time_scaling_dist = ctx->full_time / ctx->valid_dist_time; + out->flow_comp_m_x = ctx->m_flow_x_accu * time_scaling_dist; + out->flow_comp_m_y = ctx->m_flow_y_accu * time_scaling_dist; + }else{ + out->flow_comp_m_x = 0; + out->flow_comp_m_y = 0; + } out->quality = ctx->min_quality; /* averaging the distance is no use */ out->ground_distance = ctx->last.ground_distance; @@ -194,6 +213,9 @@ void result_accumulator_reset(result_accumulator_ctx *ctx) ctx->rad_flow_x_accu = 0; ctx->rad_flow_y_accu = 0; + ctx->m_flow_x_accu = 0; + ctx->m_flow_y_accu = 0; + ctx->gyro_x_accu = 0; ctx->gyro_y_accu = 0; ctx->gyro_z_accu = 0; @@ -204,6 +226,7 @@ void result_accumulator_reset(result_accumulator_ctx *ctx) ctx->data_count = 0; ctx->valid_time = 0; ctx->full_time = 0; + ctx->valid_dist_time = 0; } #if 0 From f923e35ec50fb64b7259c3231d7cb34fa1df4df9 Mon Sep 17 00:00:00 2001 From: Severin Latkovic Date: Fri, 10 Jul 2015 17:50:44 +0200 Subject: [PATCH 020/120] - Removed all traces from sensor position (BOTTOM, TOP, FRONT, ...), which were used earlier, but are not needed anymore. --- inc/result_accumulator.h | 4 +- inc/settings.h | 35 +++++----------- src/communication.c | 10 +---- src/flow.c | 10 ++--- src/main.c | 2 +- src/result_accumulator.c | 18 ++++----- src/settings.c | 86 ++++++++++++++-------------------------- 7 files changed, 58 insertions(+), 107 deletions(-) diff --git a/inc/result_accumulator.h b/inc/result_accumulator.h index 93c304c..9d86283 100644 --- a/inc/result_accumulator.h +++ b/inc/result_accumulator.h @@ -78,8 +78,8 @@ typedef struct _result_accumulator_ctx { typedef struct _result_accumulator_output_flow { int16_t flow_x; ///< Flow in pixels * 10 in x-sensor direction (dezi-pixels) int16_t flow_y; ///< Flow in pixels * 10 in y-sensor direction (dezi-pixels) - float flow_comp_m_x;///< Flow in meters in x-sensor direction, angular-speed compensated - float flow_comp_m_y;///< Flow in meters in y-sensor direction, angular-speed compensated + float flow_comp_m_x; ///< Flow in meters in x-sensor direction, angular-speed compensated + float flow_comp_m_y; ///< Flow in meters in y-sensor direction, angular-speed compensated uint8_t quality; ///< Optical flow quality / confidence. 0: bad, 255: maximum quality float ground_distance; ///< Ground distance in meters. Positive value: distance known. Negative value: Unknown distance } result_accumulator_output_flow; diff --git a/inc/settings.h b/inc/settings.h index db57762..9da8b31 100644 --- a/inc/settings.h +++ b/inc/settings.h @@ -38,9 +38,9 @@ #include #define ONBOARD_PARAM_NAME_LENGTH 15 -#define BOTTOM_FLOW_IMAGE_HEIGHT 64 -#define BOTTOM_FLOW_IMAGE_WIDTH 64 -#define BOTTOM_FLOW_SEARCH_WINDOW_SIZE 4 +#define FLOW_IMAGE_HEIGHT 64 +#define FLOW_IMAGE_WIDTH 64 +#define FLOW_SEARCH_WINDOW_SIZE 4 /****************************************************************** * ALL TYPE DEFINITIONS @@ -55,18 +55,6 @@ typedef enum READ_WRITE = 1, } ParameterAccess_TypeDef; -/** - * @brief sensor position enumeration - */ -typedef enum -{ - BOTTOM = 0x00, /*!< at bottom position */ - FRONT = 0x01, /*!< at front position */ - TOP = 0x02, /*!< at top position */ - BACK = 0x03, /*!< at back position */ - RIGHT = 0x04, /*!< at right position */ - LEFT = 0x05 /*!< at left position */ -} SensorPosition_TypeDef; /** * @brief sensor position enumeration @@ -140,21 +128,20 @@ enum global_param_id_t PARAM_VIDEO_ONLY, PARAM_VIDEO_RATE, - PARAM_BOTTOM_FLOW_FEATURE_THRESHOLD, - PARAM_BOTTOM_FLOW_VALUE_THRESHOLD, - PARAM_BOTTOM_FLOW_HIST_FILTER, - PARAM_BOTTOM_FLOW_GYRO_COMPENSATION, - PARAM_BOTTOM_FLOW_LP_FILTERED, - PARAM_BOTTOM_FLOW_WEIGHT_NEW, - PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR, + PARAM_FLOW_FEATURE_THRESHOLD, + PARAM_FLOW_VALUE_THRESHOLD, + PARAM_FLOW_HIST_FILTER, + PARAM_FLOW_GYRO_COMPENSATION, + PARAM_FLOW_LP_FILTERED, + PARAM_FLOW_WEIGHT_NEW, + PARAM_FLOW_SERIAL_THROTTLE_FACTOR, - PARAM_SENSOR_POSITION, DEBUG_VARIABLE, PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_0, PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_1, PARAM_USB_SEND_QUAL_0, - + PARAM_OUTLIER_THRESHOLD, PARAM_MIN_VALID_RATIO, diff --git a/src/communication.c b/src/communication.c index d7ea1a3..ab27291 100644 --- a/src/communication.c +++ b/src/communication.c @@ -235,17 +235,9 @@ void handle_mavlink_message(mavlink_channel_t chan, { global_data.param[i] = set.param_value; - /* handle sensor position */ - if(i == PARAM_SENSOR_POSITION) - { - set_sensor_position_settings((uint8_t) set.param_value); - mt9v034_context_configuration(); - dma_reconfigure(); - buffer_reset(); - } /* handle low light mode and noise correction */ - else if(i == PARAM_IMAGE_LOW_LIGHT || i == PARAM_IMAGE_ROW_NOISE_CORR|| i == PARAM_IMAGE_TEST_PATTERN) + if(i == PARAM_IMAGE_LOW_LIGHT || i == PARAM_IMAGE_ROW_NOISE_CORR|| i == PARAM_IMAGE_TEST_PATTERN) { mt9v034_context_configuration(); dma_reconfigure(); diff --git a/src/flow.c b/src/flow.c index 3067c8f..61dd5a7 100644 --- a/src/flow.c +++ b/src/flow.c @@ -428,7 +428,7 @@ uint16_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_ra /* test pixel if it is suitable for flow tracking */ uint32_t diff = compute_diff(image1, i, j, frame_size); - if (diff < global_data.param[PARAM_BOTTOM_FLOW_FEATURE_THRESHOLD]) + if (diff < global_data.param[PARAM_FLOW_FEATURE_THRESHOLD]) { continue; } @@ -458,7 +458,7 @@ uint16_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_ra } /* acceptance SAD distance threshhold */ - if (dist < global_data.param[PARAM_BOTTOM_FLOW_VALUE_THRESHOLD]) + if (dist < global_data.param[PARAM_FLOW_VALUE_THRESHOLD]) { compute_subpixel(image1, image2, i, j, i + sumx, j + sumy, acc, frame_size); uint32_t mindist = dist; // best SAD until now @@ -479,7 +479,7 @@ uint16_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_ra if (mindir == 3 || mindir == 4 || mindir == 5) result->x -= 0.5f; if (mindir == 5 || mindir == 6 || mindir == 7) result->y -= 0.5f; if (mindir == 1 || mindir == 2 || mindir == 3) result->y += 0.5f; - if (global_data.param[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION]) { + if (global_data.param[PARAM_FLOW_GYRO_COMPENSATION]) { /* gyro compensation */ result->x -= x_rate; result->y -= y_rate; @@ -562,7 +562,7 @@ uint16_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat for (int x = 0; x < NUM_BLOCK_KLT; x++, i += pixStep) { uint16_t idx = y*NUM_BLOCK_KLT+x; - if (global_data.param[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION]) { + if (global_data.param[PARAM_FLOW_GYRO_COMPENSATION]) { /* use the gyro measurement to guess the initial position in the new image */ us[idx] = i + x_rate; //position in new image at level 0 vs[idx] = j + y_rate; @@ -716,7 +716,7 @@ uint16_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat else //for the last level compute the actual flow in pixels { if (result_good && k < max_out) { - if (global_data.param[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION]) { + if (global_data.param[PARAM_FLOW_GYRO_COMPENSATION]) { /* compute flow and compensate gyro */ out[k].x = u - i - x_rate; out[k].y = v - j - y_rate; diff --git a/src/main.c b/src/main.c index aa70a7e..469e533 100644 --- a/src/main.c +++ b/src/main.c @@ -398,7 +398,7 @@ int main(void) counter++; /* serial mavlink + usb mavlink output throttled */ - if (counter % (uint32_t)global_data.param[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR] == 0)//throttling factor + if (counter % (uint32_t)global_data.param[PARAM_FLOW_SERIAL_THROTTLE_FACTOR] == 0)//throttling factor { /* recalculate the output values */ result_accumulator_output_flow output_flow; diff --git a/src/result_accumulator.c b/src/result_accumulator.c index faa4534..3bbe49a 100644 --- a/src/result_accumulator.c +++ b/src/result_accumulator.c @@ -256,23 +256,23 @@ void result_accumulator_reset(result_accumulator_ctx *ctx) accumulated_quality += qual; /* 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; + velocity_x_lp = global_data.param[PARAM_FLOW_WEIGHT_NEW] * new_velocity_x + + (1.0f - global_data.param[PARAM_FLOW_WEIGHT_NEW]) * velocity_x_lp; + velocity_y_lp = global_data.param[PARAM_FLOW_WEIGHT_NEW] * new_velocity_y + + (1.0f - global_data.param[PARAM_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; + velocity_x_lp = (1.0f - global_data.param[PARAM_FLOW_WEIGHT_NEW]) * velocity_x_lp; + velocity_y_lp = (1.0f - global_data.param[PARAM_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; + velocity_x_lp = (1.0f - global_data.param[PARAM_FLOW_WEIGHT_NEW]) * velocity_x_lp; + velocity_y_lp = (1.0f - global_data.param[PARAM_FLOW_WEIGHT_NEW]) * velocity_y_lp; } //update lasttime lasttime = get_boot_time_us(); @@ -286,7 +286,7 @@ void result_accumulator_reset(result_accumulator_ctx *ctx) float flow_comp_m_x = 0.0f; float flow_comp_m_y = 0.0f; - if(global_data.param[PARAM_BOTTOM_FLOW_LP_FILTERED]) + if(global_data.param[PARAM_FLOW_LP_FILTERED]) { flow_comp_m_x = velocity_x_lp; flow_comp_m_y = velocity_y_lp; diff --git a/src/settings.c b/src/settings.c index e1d7fde..26e11e7 100644 --- a/src/settings.c +++ b/src/settings.c @@ -98,12 +98,6 @@ void global_data_reset_param_defaults(void){ strcpy(global_data.param_name[PARAM_USE_MEDIAN], "USE_MEDIAN"); global_data.param_access[PARAM_USE_MEDIAN] = READ_WRITE; - - - global_data.param[PARAM_SENSOR_POSITION] = 0; // BOTTOM - strcpy(global_data.param_name[PARAM_SENSOR_POSITION], "POSITION"); - global_data.param_access[PARAM_SENSOR_POSITION] = READ_WRITE; - global_data.param[PARAM_USART2_BAUD] = 115200; strcpy(global_data.param_name[PARAM_USART2_BAUD], "USART_2_BAUD"); global_data.param_access[PARAM_USART2_BAUD] = READ_ONLY; @@ -117,11 +111,11 @@ void global_data_reset_param_defaults(void){ strcpy(global_data.param_name[PARAM_FOCAL_LENGTH_MM], "LENS_FOCAL_LEN"); global_data.param_access[PARAM_FOCAL_LENGTH_MM] = READ_WRITE; - global_data.param[PARAM_IMAGE_WIDTH] = BOTTOM_FLOW_IMAGE_WIDTH; + global_data.param[PARAM_IMAGE_WIDTH] = FLOW_IMAGE_WIDTH; strcpy(global_data.param_name[PARAM_IMAGE_WIDTH], "IMAGE_WIDTH"); global_data.param_access[PARAM_IMAGE_WIDTH] = READ_ONLY; - global_data.param[PARAM_IMAGE_HEIGHT] = BOTTOM_FLOW_IMAGE_HEIGHT; + global_data.param[PARAM_IMAGE_HEIGHT] = FLOW_IMAGE_HEIGHT; strcpy(global_data.param_name[PARAM_IMAGE_HEIGHT], "IMAGE_HEIGHT"); global_data.param_access[PARAM_IMAGE_HEIGHT] = READ_ONLY; @@ -186,41 +180,41 @@ void global_data_reset_param_defaults(void){ strcpy(global_data.param_name[PARAM_VIDEO_RATE], "VIDEO_RATE"); global_data.param_access[PARAM_VIDEO_RATE] = READ_WRITE; - global_data.param[PARAM_MAX_FLOW_PIXEL] = BOTTOM_FLOW_SEARCH_WINDOW_SIZE; - strcpy(global_data.param_name[PARAM_MAX_FLOW_PIXEL], "BFLOW_MAX_PIX"); + global_data.param[PARAM_MAX_FLOW_PIXEL] = FLOW_SEARCH_WINDOW_SIZE; + strcpy(global_data.param_name[PARAM_MAX_FLOW_PIXEL], "FLOW_MAX_PIX"); global_data.param_access[PARAM_MAX_FLOW_PIXEL] = READ_ONLY; -// global_data.param[PARAM_BOTTOM_FLOW_VALUE_THRESHOLD] = 8 * 8 * 20; - global_data.param[PARAM_BOTTOM_FLOW_VALUE_THRESHOLD] = 5000; // threshold is irrelevant with this value - strcpy(global_data.param_name[PARAM_BOTTOM_FLOW_VALUE_THRESHOLD], "BFLOW_V_THLD"); - global_data.param_access[PARAM_BOTTOM_FLOW_VALUE_THRESHOLD] = READ_WRITE; +// global_data.param[PARAM_FLOW_VALUE_THRESHOLD] = 8 * 8 * 20; + global_data.param[PARAM_FLOW_VALUE_THRESHOLD] = 5000; // threshold is irrelevant with this value + strcpy(global_data.param_name[PARAM_FLOW_VALUE_THRESHOLD], "FLOW_V_THLD"); + global_data.param_access[PARAM_FLOW_VALUE_THRESHOLD] = READ_WRITE; -// global_data.param[PARAM_BOTTOM_FLOW_FEATURE_THRESHOLD] = 100; - global_data.param[PARAM_BOTTOM_FLOW_FEATURE_THRESHOLD] = 30; // threshold is irrelevant with this value - strcpy(global_data.param_name[PARAM_BOTTOM_FLOW_FEATURE_THRESHOLD], "BFLOW_F_THLD"); - global_data.param_access[PARAM_BOTTOM_FLOW_FEATURE_THRESHOLD] = READ_WRITE; +// global_data.param[PARAM_FLOW_FEATURE_THRESHOLD] = 100; + global_data.param[PARAM_FLOW_FEATURE_THRESHOLD] = 30; // threshold is irrelevant with this value + strcpy(global_data.param_name[PARAM_FLOW_FEATURE_THRESHOLD], "FLOW_F_THLD"); + global_data.param_access[PARAM_FLOW_FEATURE_THRESHOLD] = READ_WRITE; - global_data.param[PARAM_BOTTOM_FLOW_HIST_FILTER] = 0; -// global_data.param[PARAM_BOTTOM_FLOW_HIST_FILTER] = 1; - strcpy(global_data.param_name[PARAM_BOTTOM_FLOW_HIST_FILTER], "BFLOW_HIST_FIL"); - global_data.param_access[PARAM_BOTTOM_FLOW_HIST_FILTER] = READ_WRITE; + global_data.param[PARAM_FLOW_HIST_FILTER] = 0; +// global_data.param[PARAM_FLOW_HIST_FILTER] = 1; + strcpy(global_data.param_name[PARAM_FLOW_HIST_FILTER], "FLOW_HIST_FIL"); + global_data.param_access[PARAM_FLOW_HIST_FILTER] = READ_WRITE; -// global_data.param[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION] = 0; - global_data.param[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION] = 1; - strcpy(global_data.param_name[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION], "BFLOW_GYRO_COM"); - global_data.param_access[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION] = READ_WRITE; +// global_data.param[PARAM_FLOW_GYRO_COMPENSATION] = 0; + global_data.param[PARAM_FLOW_GYRO_COMPENSATION] = 1; + strcpy(global_data.param_name[PARAM_FLOW_GYRO_COMPENSATION], "FLOW_GYRO_COM"); + global_data.param_access[PARAM_FLOW_GYRO_COMPENSATION] = READ_WRITE; - global_data.param[PARAM_BOTTOM_FLOW_LP_FILTERED] = 0; - strcpy(global_data.param_name[PARAM_BOTTOM_FLOW_LP_FILTERED], "BFLOW_LP_FIL"); - global_data.param_access[PARAM_BOTTOM_FLOW_LP_FILTERED] = READ_WRITE; + global_data.param[PARAM_FLOW_LP_FILTERED] = 0; + strcpy(global_data.param_name[PARAM_FLOW_LP_FILTERED], "FLOW_LP_FIL"); + global_data.param_access[PARAM_FLOW_LP_FILTERED] = READ_WRITE; - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] = 0.3f; - strcpy(global_data.param_name[PARAM_BOTTOM_FLOW_WEIGHT_NEW], "BFLOW_W_NEW"); - global_data.param_access[PARAM_BOTTOM_FLOW_WEIGHT_NEW] = READ_WRITE; + global_data.param[PARAM_FLOW_WEIGHT_NEW] = 0.3f; + strcpy(global_data.param_name[PARAM_FLOW_WEIGHT_NEW], "FLOW_W_NEW"); + global_data.param_access[PARAM_FLOW_WEIGHT_NEW] = READ_WRITE; - global_data.param[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR] = 10.0f; - strcpy(global_data.param_name[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR], "BFLOW_THROTT"); - global_data.param_access[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR] = READ_WRITE; + global_data.param[PARAM_FLOW_SERIAL_THROTTLE_FACTOR] = 10.0f; + strcpy(global_data.param_name[PARAM_FLOW_SERIAL_THROTTLE_FACTOR], "FLOW_THROTT"); + global_data.param_access[PARAM_FLOW_SERIAL_THROTTLE_FACTOR] = READ_WRITE; global_data.param[PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_0] = 1.0f; strcpy(global_data.param_name[PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_0], "MIN_OUT_THR_A0"); @@ -258,25 +252,3 @@ void global_data_reset(void) // not in use anymore } -/** - * @brief changes read only settings depending on sensor position - */ -void set_sensor_position_settings(uint8_t sensor_position) -{ - - switch(sensor_position) - { - case(BOTTOM): - global_data.param[PARAM_IMAGE_WIDTH] = BOTTOM_FLOW_IMAGE_WIDTH; - global_data.param[PARAM_IMAGE_HEIGHT] = BOTTOM_FLOW_IMAGE_HEIGHT; - break; - - default: - debug_int_message_buffer("Unused sensor position:", sensor_position); - return; - } - - debug_int_message_buffer("Set sensor position:", sensor_position); - return; -} - From 3abbca084def60a2767eb3c00e265a624ba6535a Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Sat, 11 Jul 2015 20:35:34 +0200 Subject: [PATCH 021/120] finally fixed the boot time us function. it now consistently counts UP. --- src/timer.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/timer.c b/src/timer.c index 64dd6fa..3544e75 100644 --- a/src/timer.c +++ b/src/timer.c @@ -48,9 +48,10 @@ /* boot time in milliseconds ticks */ volatile uint32_t boot_time_ms = 0; -volatile uint32_t boot_time_us_base = 0; +volatile uint32_t boot_time_us_base = 999; static uint32_t ticks_per_ms; +static uint32_t ticks_per_us; #define TIMER_FLAG_ENABLED 0x01 #define TIMER_FLAG_TRIGGERED 0x02 @@ -133,7 +134,8 @@ void timer_register(void (*timer_fn)(void), uint32_t period_ms) { void timer_init(void) { /* init clock */ - ticks_per_ms = SystemCoreClock / 1000; + ticks_per_ms = SystemCoreClock / 1000u; + ticks_per_us = ticks_per_ms / 1000u; /* init all timers */ for (int idx = 0; idx < NTIMERS; idx++) { /* disable it: */ @@ -174,7 +176,7 @@ uint32_t get_boot_time_us(void) // make sure it did not roll over in the mean-time: } while(SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk); // return the calculated value: - return ((val_tick & SysTick_VAL_CURRENT_Msk) / ticks_per_ms) + val_us_base; + return val_us_base - ((val_tick & SysTick_VAL_CURRENT_Msk) / ticks_per_us); } uint32_t calculate_time_delta_us(uint32_t end, uint32_t start) { From 6c76a3d32d09b2c152095e9751f714954360654e Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Sat, 11 Jul 2015 20:39:29 +0200 Subject: [PATCH 022/120] experimented with higher block count and lower iterations for KLT. added experimental minimum determinant parameter. --- inc/settings.h | 1 + src/flow.c | 10 +++++----- src/main.c | 8 +++++++- src/settings.c | 4 ++++ 4 files changed, 17 insertions(+), 6 deletions(-) diff --git a/inc/settings.h b/inc/settings.h index db57762..0222a39 100644 --- a/inc/settings.h +++ b/inc/settings.h @@ -112,6 +112,7 @@ enum global_param_id_t PARAM_ALGORITHM_CHOICE, PARAM_USE_IMAGE_FILTER, PARAM_FRAME_INTERVAL, + PARAM_KLT_DET_VALUE_MIN, PARAM_USART2_BAUD, PARAM_USART3_BAUD, diff --git a/src/flow.c b/src/flow.c index 3067c8f..b475a22 100644 --- a/src/flow.c +++ b/src/flow.c @@ -53,7 +53,7 @@ #define SEARCH_SIZE global_data.param[PARAM_MAX_FLOW_PIXEL] // maximum offset to search: 4 + 1/2 pixels #define TILE_SIZE 8 // x & y tile size #define NUM_BLOCKS 5 // x & y number of tiles to check -#define NUM_BLOCK_KLT 3 +#define NUM_BLOCK_KLT 4 //this are the settings for KLT based flow #define PYR_LVLS 2 @@ -446,8 +446,8 @@ uint16_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_ra for (ii = winmin; ii <= winmax; ii++) { -// uint32_t temp_dist = compute_sad_8x8(image1, image2, i, j, i + ii, j + jj, frame_size); - uint32_t temp_dist = ABSDIFF(base1, base2 + ii); + uint32_t temp_dist = compute_sad_8x8(image1, image2, i, j, i + ii, j + jj, frame_size); +// uint32_t temp_dist = ABSDIFF(base1, base2 + ii); if (temp_dist < dist) { sumx = ii; @@ -627,7 +627,7 @@ uint16_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat //compute inverse of hessian // TODO: evaluate using condition of this matrix to decide whether we should continue float det = (JTJ[0]*JTJ[3]-JTJ[1]*JTJ[2]); - if (det != 0.f) + if (fabs(det) > global_data.param[PARAM_KLT_DET_VALUE_MIN]) { float detinv = 1.f / det; float JTJinv[4]; @@ -645,7 +645,7 @@ uint16_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat bool result_good = true; //Now do some Gauss-Newton iterations for flow - for (int iters = 0; iters < 5; iters++) + for (int iters = 0; iters < 3; iters++) { float JTe_x = 0; //accumulators for Jac transposed times error float JTe_y = 0; diff --git a/src/main.c b/src/main.c index aa70a7e..3adffad 100644 --- a/src/main.c +++ b/src/main.c @@ -320,6 +320,8 @@ int main(void) dma_copy_image_buffers(¤t_image, &previous_image, image_size, (int)(global_data.param[PARAM_FRAME_INTERVAL] + 0.5)); float frame_dt = get_time_between_images() * 0.000001f; + uint32_t start_computations = get_boot_time_us(); + /* 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); @@ -397,9 +399,13 @@ int main(void) counter++; + uint32_t computaiton_time_us = get_time_delta_us(start_computations); + /* serial mavlink + usb mavlink output throttled */ if (counter % (uint32_t)global_data.param[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR] == 0)//throttling factor { + mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "TIMING", get_boot_time_us(), computaiton_time_us, 0, 0); + /* recalculate the output values */ result_accumulator_output_flow output_flow; result_accumulator_output_flow_rad output_flow_rad; @@ -437,7 +443,7 @@ int main(void) if(global_data.param[PARAM_USB_SEND_GYRO]) { - mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "GYRO", get_boot_time_us(), pixel_flow_x, y_rate, z_rate); + mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "GYRO", get_boot_time_us(), x_rate, y_rate, z_rate); } result_accumulator_reset(&mavlink_accumulator); diff --git a/src/settings.c b/src/settings.c index e1d7fde..e99f3de 100644 --- a/src/settings.c +++ b/src/settings.c @@ -98,6 +98,10 @@ void global_data_reset_param_defaults(void){ strcpy(global_data.param_name[PARAM_USE_MEDIAN], "USE_MEDIAN"); global_data.param_access[PARAM_USE_MEDIAN] = READ_WRITE; + global_data.param[PARAM_KLT_DET_VALUE_MIN] = 0.1f; + strcpy(global_data.param_name[PARAM_KLT_DET_VALUE_MIN], "DET_VALUE_MIN"); + global_data.param_access[PARAM_KLT_DET_VALUE_MIN] = READ_WRITE; + global_data.param[PARAM_SENSOR_POSITION] = 0; // BOTTOM From 34201b7729a1f3bbd030d86554569b08ca33e99b Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Mon, 13 Jul 2015 08:09:49 +0200 Subject: [PATCH 023/120] small code cleanup --- src/main.c | 4 +-- src/result_accumulator.c | 77 ---------------------------------------- 2 files changed, 2 insertions(+), 79 deletions(-) diff --git a/src/main.c b/src/main.c index 469e533..c1c9118 100644 --- a/src/main.c +++ b/src/main.c @@ -400,7 +400,7 @@ int main(void) /* serial mavlink + usb mavlink output throttled */ if (counter % (uint32_t)global_data.param[PARAM_FLOW_SERIAL_THROTTLE_FACTOR] == 0)//throttling factor { - /* recalculate the output values */ + /* calculate the output values */ result_accumulator_output_flow output_flow; result_accumulator_output_flow_rad output_flow_rad; int min_valid_ratio = global_data.param[PARAM_MIN_VALID_RATIO]; @@ -437,7 +437,7 @@ int main(void) if(global_data.param[PARAM_USB_SEND_GYRO]) { - mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "GYRO", get_boot_time_us(), pixel_flow_x, y_rate, z_rate); + mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "GYRO", get_boot_time_us(), x_rate, y_rate, z_rate); } result_accumulator_reset(&mavlink_accumulator); diff --git a/src/result_accumulator.c b/src/result_accumulator.c index 3bbe49a..82400b7 100644 --- a/src/result_accumulator.c +++ b/src/result_accumulator.c @@ -228,80 +228,3 @@ void result_accumulator_reset(result_accumulator_ctx *ctx) ctx->full_time = 0; ctx->valid_dist_time = 0; } - -#if 0 -/* 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; - - time_since_last_sonar_update = (get_boot_time_us()- get_sonar_measure_time()); - - if (qual > 0) - { - velocity_x_sum += new_velocity_x; - velocity_y_sum += new_velocity_y; - valid_frame_count++; - - uint32_t deltatime = (get_boot_time_us() - lasttime); - integration_timespan += deltatime; - accumulated_flow_x += pixel_flow_y / focal_length_px * 1.0f; //rad axis swapped to align x flow around y axis - accumulated_flow_y += pixel_flow_x / focal_length_px * -1.0f;//rad - accumulated_gyro_x += x_rate * deltatime / 1000000.0f; //rad - accumulated_gyro_y += y_rate * deltatime / 1000000.0f; //rad - accumulated_gyro_z += z_rate * deltatime / 1000000.0f; //rad - accumulated_framecount++; - accumulated_quality += qual; - - /* lowpass velocity output */ - velocity_x_lp = global_data.param[PARAM_FLOW_WEIGHT_NEW] * new_velocity_x + - (1.0f - global_data.param[PARAM_FLOW_WEIGHT_NEW]) * velocity_x_lp; - velocity_y_lp = global_data.param[PARAM_FLOW_WEIGHT_NEW] * new_velocity_y + - (1.0f - global_data.param[PARAM_FLOW_WEIGHT_NEW]) * velocity_y_lp; - } - else - { - /* taking flow as zero */ - velocity_x_lp = (1.0f - global_data.param[PARAM_FLOW_WEIGHT_NEW]) * velocity_x_lp; - velocity_y_lp = (1.0f - global_data.param[PARAM_FLOW_WEIGHT_NEW]) * velocity_y_lp; - } - } - else - { - /* taking flow as zero */ - velocity_x_lp = (1.0f - global_data.param[PARAM_FLOW_WEIGHT_NEW]) * velocity_x_lp; - velocity_y_lp = (1.0f - global_data.param[PARAM_FLOW_WEIGHT_NEW]) * velocity_y_lp; - } - //update lasttime - lasttime = get_boot_time_us(); - - pixel_flow_x_sum += pixel_flow_x; - pixel_flow_y_sum += pixel_flow_y; - pixel_flow_count++; - - - - float flow_comp_m_x = 0.0f; - float flow_comp_m_y = 0.0f; - - if(global_data.param[PARAM_FLOW_LP_FILTERED]) - { - flow_comp_m_x = velocity_x_lp; - flow_comp_m_y = velocity_y_lp; - } - else - { - if(valid_frame_count>0) - { - flow_comp_m_x = velocity_x_sum/valid_frame_count; - flow_comp_m_y = velocity_y_sum/valid_frame_count; - } - else - { - flow_comp_m_x = 0.0f; - flow_comp_m_y = 0.0f; - } - } -#endif \ No newline at end of file From 0192245b697e3226b3fc779a6e4545d046f319c6 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Mon, 13 Jul 2015 10:31:38 +0200 Subject: [PATCH 024/120] some more tweaking to KLT. Implemented corner detector to decide whether a patch is a good patch. --- inc/settings.h | 2 ++ src/flow.c | 19 ++++++++++++++----- src/main.c | 14 ++++++++++++-- src/settings.c | 9 ++++++++- 4 files changed, 36 insertions(+), 8 deletions(-) diff --git a/inc/settings.h b/inc/settings.h index 0222a39..5cf6cfc 100644 --- a/inc/settings.h +++ b/inc/settings.h @@ -113,6 +113,8 @@ enum global_param_id_t PARAM_USE_IMAGE_FILTER, PARAM_FRAME_INTERVAL, PARAM_KLT_DET_VALUE_MIN, + PARAM_KLT_MAX_ITERS, + PARAM_CORNER_KAPPA, PARAM_USART2_BAUD, PARAM_USART3_BAUD, diff --git a/src/flow.c b/src/flow.c index b475a22..f965fd2 100644 --- a/src/flow.c +++ b/src/flow.c @@ -53,11 +53,11 @@ #define SEARCH_SIZE global_data.param[PARAM_MAX_FLOW_PIXEL] // maximum offset to search: 4 + 1/2 pixels #define TILE_SIZE 8 // x & y tile size #define NUM_BLOCKS 5 // x & y number of tiles to check -#define NUM_BLOCK_KLT 4 +#define NUM_BLOCK_KLT 4 //this are the settings for KLT based flow #define PYR_LVLS 2 -#define HALF_PATCH_SIZE 4 //this is half the wanted patch size minus 1 +#define HALF_PATCH_SIZE 3 //this is half the wanted patch size minus 1 #define PATCH_SIZE (HALF_PATCH_SIZE*2+1) float Jx[PATCH_SIZE*PATCH_SIZE]; @@ -502,6 +502,8 @@ uint16_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat float chi_sum = 0.0f; uint8_t chicount = 0; + uint16_t max_iters = global_data.param[PARAM_KLT_MAX_ITERS]; + /* * compute image pyramid for current frame * there is 188*120 bytes per buffer, we are only using 64*64 per buffer, @@ -607,11 +609,16 @@ uint16_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat int c = 0; //compute jacobians and the hessian for the patch at the current location + uint8_t min_val = 255; + uint8_t max_val = 0; for (int8_t jj = -HALF_PATCH_SIZE; jj <= HALF_PATCH_SIZE; jj++) { uint8_t *left = base1 + jj*iwidth; for (int8_t ii = -HALF_PATCH_SIZE; ii <= HALF_PATCH_SIZE; ii++) { + uint8_t val = left[ii]; + if (val > max_val) max_val = val; + if (val < min_val) min_val = val; const float jx = ((uint16_t)left[ii+1] - (uint16_t)left[ii-1]) * 0.5f; const float jy = ((uint16_t)left[ii+iwidth] - (uint16_t)left[ii-iwidth]) * 0.5f; Jx[c] = jx; @@ -625,9 +632,11 @@ uint16_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat } //compute inverse of hessian - // TODO: evaluate using condition of this matrix to decide whether we should continue float det = (JTJ[0]*JTJ[3]-JTJ[1]*JTJ[2]); - if (fabs(det) > global_data.param[PARAM_KLT_DET_VALUE_MIN]) + float dyn_range = (float)(max_val - min_val) + 1; + float trace = (JTJ[0] + JTJ[3]); + float M_c = det - global_data.param[PARAM_CORNER_KAPPA] * trace * trace; + if (fabs(det) > global_data.param[PARAM_KLT_DET_VALUE_MIN] * dyn_range && M_c > 0.0f) { float detinv = 1.f / det; float JTJinv[4]; @@ -645,7 +654,7 @@ uint16_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat bool result_good = true; //Now do some Gauss-Newton iterations for flow - for (int iters = 0; iters < 3; iters++) + for (int iters = 0; iters < max_iters; iters++) { float JTe_x = 0; //accumulators for Jac transposed times error float JTe_y = 0; diff --git a/src/main.c b/src/main.c index 3adffad..f13ae4e 100644 --- a/src/main.c +++ b/src/main.c @@ -270,6 +270,8 @@ int main(void) result_accumulator_init(&mavlink_accumulator); + uint32_t fps_timing_start = get_boot_time_us(); + uint16_t fps_counter = 0; /* main loop */ while (1) { @@ -398,13 +400,21 @@ int main(void) distance_valid, ground_distance, get_time_delta_us(get_sonar_measure_time())); counter++; + fps_counter++; uint32_t computaiton_time_us = get_time_delta_us(start_computations); /* serial mavlink + usb mavlink output throttled */ if (counter % (uint32_t)global_data.param[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR] == 0)//throttling factor { - mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "TIMING", get_boot_time_us(), computaiton_time_us, 0, 0); + float fps = 0; + if (fps_counter > 0) { + uint32_t dt = get_time_delta_us(fps_timing_start); + fps_timing_start += dt; + fps = (float)fps_counter / ((float)dt * 1e-6f); + fps_counter = 0; + } + mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "TIMING", get_boot_time_us(), computaiton_time_us, fps, 0); /* recalculate the output values */ result_accumulator_output_flow output_flow; @@ -426,7 +436,7 @@ int main(void) output_flow_rad.temperature, output_flow_rad.quality, output_flow_rad.time_delta_distance_us,output_flow_rad.ground_distance); - if (global_data.param[PARAM_USB_SEND_FLOW] && (qual > 0 || global_data.param[PARAM_USB_SEND_QUAL_0])) + if (global_data.param[PARAM_USB_SEND_FLOW] && (output_flow.quality > 0 || global_data.param[PARAM_USB_SEND_QUAL_0])) { mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], output_flow.flow_x, output_flow.flow_y, diff --git a/src/settings.c b/src/settings.c index e99f3de..b876f3c 100644 --- a/src/settings.c +++ b/src/settings.c @@ -98,10 +98,17 @@ void global_data_reset_param_defaults(void){ strcpy(global_data.param_name[PARAM_USE_MEDIAN], "USE_MEDIAN"); global_data.param_access[PARAM_USE_MEDIAN] = READ_WRITE; - global_data.param[PARAM_KLT_DET_VALUE_MIN] = 0.1f; + global_data.param[PARAM_KLT_MAX_ITERS] = 5; + strcpy(global_data.param_name[PARAM_KLT_MAX_ITERS], "KLT_MAX_ITERS"); + global_data.param_access[PARAM_KLT_MAX_ITERS] = READ_WRITE; + + global_data.param[PARAM_KLT_DET_VALUE_MIN] = 90; strcpy(global_data.param_name[PARAM_KLT_DET_VALUE_MIN], "DET_VALUE_MIN"); global_data.param_access[PARAM_KLT_DET_VALUE_MIN] = READ_WRITE; + global_data.param[PARAM_CORNER_KAPPA] = 0.06; + strcpy(global_data.param_name[PARAM_CORNER_KAPPA], "CORNER_KAPPA"); + global_data.param_access[PARAM_CORNER_KAPPA] = READ_WRITE; global_data.param[PARAM_SENSOR_POSITION] = 0; // BOTTOM From a9141ebebb92bbfeb456e2ef7a324220cb2d4bd2 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Mon, 13 Jul 2015 15:20:49 +0200 Subject: [PATCH 025/120] implemented forcing capturing two images in fast sequence if algorithm takes too long. parameter tweaking. --- inc/dcmi.h | 2 +- inc/flow.h | 8 +++++ inc/settings.h | 1 + src/communication.c | 2 +- src/dcmi.c | 7 ++-- src/flow.c | 83 +++++++++++++++++++++++++++++---------------- src/main.c | 47 ++++++++++++++++++------- src/settings.c | 9 +++-- 8 files changed, 110 insertions(+), 49 deletions(-) diff --git a/inc/dcmi.h b/inc/dcmi.h index b127273..303b6c2 100644 --- a/inc/dcmi.h +++ b/inc/dcmi.h @@ -42,7 +42,7 @@ /** * @brief Copy image to fast RAM address */ -void dma_copy_image_buffers(uint8_t ** current_image, uint8_t ** previous_image, uint16_t buffer_size, uint8_t image_step); +int dma_copy_image_buffers(uint8_t ** current_image, uint8_t ** previous_image, uint16_t buffer_size, uint8_t image_step); /** * @brief Send calibration image with MAVLINK over USB diff --git a/inc/flow.h b/inc/flow.h index 1ac24ac..0d5cb93 100644 --- a/inc/flow.h +++ b/inc/flow.h @@ -59,10 +59,18 @@ typedef struct _flow_raw_result { uint16_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate, flow_raw_result *out, uint16_t max_out); +/** + * Preprocesses the image for use with compute_klt. + * This will add the pyramid levels. + * @note Image buffer needs to be at least twice as big as the original image. + */ +void klt_preprocess_image(uint8_t *image); + /** * @brief Computes pixel flow from image1 to image2 * Searches the corresponding position in the new image (image2) of max. 64 pixels from the old image (image1) * with the KLT method and outputs the value of all flow vectors. + * @NOTE call klt_preprocess_image on the images first! (No need to call it on the previous image again if it has been treated already) * @param image1 The older image * @param image2 The new image * @param x_rate The gyro x-rate during the frame interval in pixels. (In the image x direction) diff --git a/inc/settings.h b/inc/settings.h index 5cf6cfc..b620b1d 100644 --- a/inc/settings.h +++ b/inc/settings.h @@ -134,6 +134,7 @@ enum global_param_id_t PARAM_USB_SEND_VIDEO, PARAM_USB_SEND_FLOW, PARAM_USB_SEND_GYRO, + PARAM_USB_SEND_FLOW_OUTL, PARAM_USB_SEND_FORWARD, PARAM_USB_SEND_DEBUG, diff --git a/src/communication.c b/src/communication.c index d7ea1a3..1176db6 100644 --- a/src/communication.c +++ b/src/communication.c @@ -245,7 +245,7 @@ void handle_mavlink_message(mavlink_channel_t chan, } /* handle low light mode and noise correction */ - else if(i == PARAM_IMAGE_LOW_LIGHT || i == PARAM_IMAGE_ROW_NOISE_CORR|| i == PARAM_IMAGE_TEST_PATTERN) + else if(i == PARAM_IMAGE_LOW_LIGHT || i == PARAM_IMAGE_ROW_NOISE_CORR || i == PARAM_IMAGE_TEST_PATTERN) { mt9v034_context_configuration(); dma_reconfigure(); diff --git a/src/dcmi.c b/src/dcmi.c index fadcc02..8c69464 100644 --- a/src/dcmi.c +++ b/src/dcmi.c @@ -232,8 +232,9 @@ uint32_t get_frame_counter(void){ * @param previous_image Previous image buffer * @param image_size Image size of the image to copy * @param image_step Image to wait for (if 1 no waiting) + * @return the number of skipped images. */ -void dma_copy_image_buffers(uint8_t ** current_image, uint8_t ** previous_image, uint16_t image_size, uint8_t image_step){ +int dma_copy_image_buffers(uint8_t ** current_image, uint8_t ** previous_image, uint16_t image_size, uint8_t image_step){ /* swap image buffers */ uint8_t * tmp_image = *current_image; @@ -242,7 +243,8 @@ void dma_copy_image_buffers(uint8_t ** current_image, uint8_t ** previous_image, /* wait for new image if needed */ while(image_counter < image_step) {} - image_counter = 0; + int img_ctr_val = image_counter; + image_counter -= img_ctr_val; /* time between images */ time_between_images = time_between_next_images; @@ -263,6 +265,7 @@ void dma_copy_image_buffers(uint8_t ** current_image, uint8_t ** previous_image, for (uint16_t pixel = 0; pixel < image_size; pixel++) (*current_image)[pixel] = (uint8_t)(dcmi_image_buffer_8bit_3[pixel]); } + return img_ctr_val - image_step; } /** diff --git a/src/flow.c b/src/flow.c index f965fd2..d9f5c58 100644 --- a/src/flow.c +++ b/src/flow.c @@ -492,6 +492,48 @@ uint16_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_ra return result_count; } +void klt_preprocess_image(uint8_t *image) { + uint16_t i, j; + + /* + * compute image pyramid for current frame + * there is 188*120 bytes per buffer, we are only using 64*64 per buffer, + * so just add the pyramid levels after the image + */ + //first compute the offsets in the memory for the pyramid levels + uint16_t lvl_ofs[PYR_LVLS]; + uint16_t frame_size = (uint16_t)(FRAME_SIZE+0.5); + uint16_t s = frame_size; + uint16_t off = 0; + for (int l = 0; l < PYR_LVLS; l++) + { + lvl_ofs[l] = off; + off += s*s; + s /= 2; + } + + //then subsample the images consecutively, no blurring is done before the subsampling (if someone volunteers, please go ahead...) + for (int l = 1; l < PYR_LVLS; l++) + { + uint16_t src_size = frame_size >> (l-1); + uint16_t tar_size = frame_size >> l; + uint8_t *source = &image[lvl_ofs[l-1]]; //pointer to the beginning of the previous level + uint8_t *target = &image[lvl_ofs[l]]; //pointer to the beginning of the current level + for (j = 0; j < tar_size; j++) { + for (i = 0; i < tar_size; i+=2) + { + //subsample the image by 2, use the halving-add instruction to do so + uint32_t l1 = (__UHADD8(*((uint32_t*) &source[(j*2+0)*src_size + i*2]), *((uint32_t*) &source[(j*2+0)*src_size + i*2+1]))); + uint32_t l2 = (__UHADD8(*((uint32_t*) &source[(j*2+1)*src_size + i*2]), *((uint32_t*) &source[(j*2+1)*src_size + i*2+1]))); + uint32_t r = __UHADD8(l1, l2); + + //the first and the third byte are the values we want to have + target[j*tar_size + i+0] = (uint8_t) r; + target[j*tar_size + i+1] = (uint8_t) (r>>16); + } + } + } +} uint16_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate, flow_raw_result *out, uint16_t max_out) @@ -505,10 +547,10 @@ uint16_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat uint16_t max_iters = global_data.param[PARAM_KLT_MAX_ITERS]; /* - * compute image pyramid for current frame - * there is 188*120 bytes per buffer, we are only using 64*64 per buffer, - * so just add the pyramid levels after the image - */ + * compute image pyramid for current frame + * there is 188*120 bytes per buffer, we are only using 64*64 per buffer, + * so just add the pyramid levels after the image + */ //first compute the offsets in the memory for the pyramid levels uint16_t lvl_ofs[PYR_LVLS]; uint16_t frame_size = (uint16_t)(FRAME_SIZE+0.5); @@ -521,27 +563,6 @@ uint16_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat s /= 2; } - //then subsample the images consecutively, no blurring is done before the subsampling (if someone volunteers, please go ahead...) - for (int l = 1; l < PYR_LVLS; l++) - { - uint16_t src_size = frame_size >> (l-1); - uint16_t tar_size = frame_size >> l; - uint8_t *source = &image2[lvl_ofs[l-1]]; //pointer to the beginning of the previous level - uint8_t *target = &image2[lvl_ofs[l]]; //pointer to the beginning of the current level - for (j = 0; j < tar_size; j++) - for (i = 0; i < tar_size; i+=2) - { - //subsample the image by 2, use the halving-add instruction to do so - uint32_t l1 = (__UHADD8(*((uint32_t*) &source[(j*2+0)*src_size + i*2]), *((uint32_t*) &source[(j*2+0)*src_size + i*2+1]))); - uint32_t l2 = (__UHADD8(*((uint32_t*) &source[(j*2+1)*src_size + i*2]), *((uint32_t*) &source[(j*2+1)*src_size + i*2+1]))); - uint32_t r = __UHADD8(l1, l2); - - //the first and the third byte are the values we want to have - target[j*tar_size + i+0] = (uint8_t) r; - target[j*tar_size + i+1] = (uint8_t) (r>>16); - } - } - //need to store the flow values between pyramid level changes float us[NUM_BLOCK_KLT*NUM_BLOCK_KLT]; float vs[NUM_BLOCK_KLT*NUM_BLOCK_KLT]; @@ -780,7 +801,7 @@ uint8_t flow_extract_result(flow_raw_result *in, uint16_t result_count, float *p valid_c++; } } - if (valid_c < (result_count + 4) / 4 || valid_c < 3) { + if (valid_c < (result_count + 2) / 3 || valid_c < 3) { *px_flow_x = 0; *px_flow_y = 0; return 0; @@ -836,10 +857,12 @@ uint8_t flow_extract_result(flow_raw_result *in, uint16_t result_count, float *p return 0; } } - static int ctr = 0; - if (ctr++ % 10 == 0) { - mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "SPREAD_X", get_boot_time_us(), spread_val[0], max_spread_val[0], *output[0]); - mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "SPREAD_Y", get_boot_time_us(), spread_val[1], max_spread_val[1], *output[1]); + if (global_data.param[PARAM_USB_SEND_FLOW_OUTL]) { + static int ctr = 0; + if (ctr++ % 20 == 0) { + mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "SPREAD_X", get_boot_time_us(), spread_val[0], max_spread_val[0], *output[0]); + mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "SPREAD_Y", get_boot_time_us(), spread_val[1], max_spread_val[1], *output[1]); + } } total_avg_c = total_avg_c / 2; return (total_avg_c * 255) / result_count; diff --git a/src/main.c b/src/main.c index f13ae4e..6e5a0d9 100644 --- a/src/main.c +++ b/src/main.c @@ -272,6 +272,7 @@ int main(void) uint32_t fps_timing_start = get_boot_time_us(); uint16_t fps_counter = 0; + uint16_t fps_skipped_counter = 0; /* main loop */ while (1) { @@ -318,26 +319,42 @@ int main(void) sonar_distance_raw = 0.0f; } + bool use_klt = global_data.param[PARAM_ALGORITHM_CHOICE] != 0; + + uint32_t start_computations = 0; + /* copy recent image to faster ram */ - dma_copy_image_buffers(¤t_image, &previous_image, image_size, (int)(global_data.param[PARAM_FRAME_INTERVAL] + 0.5)); - float frame_dt = get_time_between_images() * 0.000001f; + int skipped_frames = 0; + int loop_count = 0; //< make sure that we dont end up in an infinite loop inside here .. + do { + skipped_frames = dma_copy_image_buffers(¤t_image, &previous_image, image_size, (int)(global_data.param[PARAM_FRAME_INTERVAL] + 0.5)); + + start_computations = get_boot_time_us(); - uint32_t start_computations = get_boot_time_us(); + /* filter the new image */ + if (global_data.param[PARAM_USE_IMAGE_FILTER]) { + filter_image(current_image, global_data.param[PARAM_IMAGE_WIDTH]); + } + + /* Preprocessing needed by the klt algorithm: */ + if (use_klt) { + klt_preprocess_image(current_image); + } + /* make sure no frames have been skipped. request a new image if a frame was skipped to ensure minimal time delta between images */ + fps_skipped_counter += skipped_frames; + loop_count++; + } while (skipped_frames > 0 && loop_count < 2); + float frame_dt = get_time_between_images() * 0.000001f; /* 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); float z_rate_fr = - z_rate * frame_dt; - /* filter the new image */ - if (global_data.param[PARAM_USE_IMAGE_FILTER]) { - filter_image(current_image, global_data.param[PARAM_IMAGE_WIDTH]); - } - /* compute optical flow in pixels */ flow_raw_result flow_rslt[32]; uint16_t flow_rslt_count = 0; - if (global_data.param[PARAM_ALGORITHM_CHOICE] == 0) { + if (!use_klt) { flow_rslt_count = compute_flow(previous_image, current_image, x_rate_px, y_rate_px, z_rate_fr, flow_rslt, 32); } else { flow_rslt_count = compute_klt(previous_image, current_image, x_rate_px, y_rate_px, z_rate_fr, flow_rslt, 32); @@ -399,22 +416,26 @@ int main(void) qual, pixel_flow_x, pixel_flow_y, 1.0f / focal_length_px, distance_valid, ground_distance, get_time_delta_us(get_sonar_measure_time())); + uint32_t computaiton_time_us = get_time_delta_us(start_computations); + counter++; fps_counter++; - uint32_t computaiton_time_us = get_time_delta_us(start_computations); - /* serial mavlink + usb mavlink output throttled */ if (counter % (uint32_t)global_data.param[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR] == 0)//throttling factor { float fps = 0; - if (fps_counter > 0) { + float fps_skip = 0; + if (fps_counter + fps_skipped_counter > 100) { uint32_t dt = get_time_delta_us(fps_timing_start); fps_timing_start += dt; fps = (float)fps_counter / ((float)dt * 1e-6f); + fps_skip = (float)fps_skipped_counter / ((float)dt * 1e-6f); fps_counter = 0; + fps_skipped_counter = 0; + + mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "TIMING", get_boot_time_us(), computaiton_time_us, fps, fps_skip); } - mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "TIMING", get_boot_time_us(), computaiton_time_us, fps, 0); /* recalculate the output values */ result_accumulator_output_flow output_flow; diff --git a/src/settings.c b/src/settings.c index b876f3c..a54d999 100644 --- a/src/settings.c +++ b/src/settings.c @@ -189,6 +189,11 @@ void global_data_reset_param_defaults(void){ strcpy(global_data.param_name[PARAM_USB_SEND_DEBUG], "USB_SEND_DEBUG"); global_data.param_access[PARAM_USB_SEND_DEBUG] = READ_WRITE; + global_data.param[PARAM_USB_SEND_FLOW_OUTL] = 1; // send flow outlier debug + strcpy(global_data.param_name[PARAM_USB_SEND_FLOW_OUTL], "USB_SEND_FLOW_O"); + global_data.param_access[PARAM_USB_SEND_FLOW_OUTL] = READ_WRITE; + + global_data.param[PARAM_VIDEO_ONLY] = 0; strcpy(global_data.param_name[PARAM_VIDEO_ONLY], "VIDEO_ONLY"); global_data.param_access[PARAM_VIDEO_ONLY] = READ_WRITE; @@ -237,11 +242,11 @@ void global_data_reset_param_defaults(void){ strcpy(global_data.param_name[PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_0], "MIN_OUT_THR_A0"); global_data.param_access[PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_0] = READ_WRITE; - global_data.param[PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_1] = 0.1f; + global_data.param[PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_1] = 0.2f; strcpy(global_data.param_name[PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_1], "MIN_OUT_THR_A1"); global_data.param_access[PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_1] = READ_WRITE; - global_data.param[PARAM_OUTLIER_THRESHOLD] = 0.2f; + global_data.param[PARAM_OUTLIER_THRESHOLD] = 0.15f; strcpy(global_data.param_name[PARAM_OUTLIER_THRESHOLD], "OUTLIER_THR"); global_data.param_access[PARAM_OUTLIER_THRESHOLD] = READ_WRITE; From 173b529824f980c0fafc2485d2a3f2d81beca6e5 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Mon, 13 Jul 2015 15:21:50 +0200 Subject: [PATCH 026/120] increase processor speed to 192MHz. Looking good so far. 465Hz frame rate. --- src/system_stm32f4xx.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/system_stm32f4xx.c b/src/system_stm32f4xx.c index a7e7563..9a937a4 100644 --- a/src/system_stm32f4xx.c +++ b/src/system_stm32f4xx.c @@ -126,18 +126,18 @@ /************************* PLL Parameters *************************************/ /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N */ #define PLL_M 24 -#define PLL_N 336 +#define PLL_N 384 /* SYSCLK = PLL_VCO / PLL_P */ #define PLL_P 2 /* USB OTG FS, SDIO and RNG Clock = PLL_VCO / PLLQ */ -#define PLL_Q 7 +#define PLL_Q 8 /******************************************************************************/ -uint32_t SystemCoreClock = 168000000; +uint32_t SystemCoreClock = 192000000; __I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9}; From ffcccdc71f41f18511debfaa69957288b6da6bcd Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Mon, 13 Jul 2015 16:27:51 +0200 Subject: [PATCH 027/120] reorder and rename parameters to have a coherent naming scheme. fix paremeter maximum length. --- inc/settings.h | 45 +++++++++-------- src/flow.c | 5 +- src/i2c.c | 2 +- src/main.c | 12 ++--- src/settings.c | 128 +++++++++++++++++++++---------------------------- 5 files changed, 87 insertions(+), 105 deletions(-) diff --git a/inc/settings.h b/inc/settings.h index dcb0703..8b109dc 100644 --- a/inc/settings.h +++ b/inc/settings.h @@ -37,7 +37,7 @@ #include -#define ONBOARD_PARAM_NAME_LENGTH 15 +#define ONBOARD_PARAM_NAME_LENGTH 16 ///< Parameter length including zero termination. #define FLOW_IMAGE_HEIGHT 64 #define FLOW_IMAGE_WIDTH 64 #define FLOW_SEARCH_WINDOW_SIZE 4 @@ -97,24 +97,33 @@ enum global_param_id_t PARAM_SYSTEM_SEND_STATE, PARAM_SYSTEM_SEND_LPOS, - PARAM_ALGORITHM_CHOICE, - PARAM_USE_IMAGE_FILTER, - PARAM_FRAME_INTERVAL, - PARAM_KLT_DET_VALUE_MIN, - PARAM_KLT_MAX_ITERS, - PARAM_CORNER_KAPPA, - PARAM_USART2_BAUD, PARAM_USART3_BAUD, + PARAM_FOCAL_LENGTH_MM, + PARAM_IMAGE_WIDTH, PARAM_IMAGE_HEIGHT, - PARAM_MAX_FLOW_PIXEL, + PARAM_IMAGE_LOW_LIGHT, PARAM_IMAGE_ROW_NOISE_CORR, PARAM_IMAGE_TEST_PATTERN, + PARAM_IMAGE_INTERVAL, + + PARAM_ALGORITHM_CHOICE, + PARAM_ALGORITHM_IMAGE_FILTER, + PARAM_ALGORITHM_CORNER_KAPPA, + PARAM_ALGORITHM_OUTLIER_THR_BLOCK, + PARAM_ALGORITHM_OUTLIER_THR_KLT, + PARAM_ALGORITHM_OUTLIER_THR_RATIO, + PARAM_ALGORITHM_MIN_VALID_RATIO, + + PARAM_KLT_DET_VALUE_MIN, + PARAM_KLT_MAX_ITERS, + PARAM_GYRO_SENSITIVITY_DPS, PARAM_GYRO_COMPENSATION_THRESHOLD, + PARAM_SONAR_FILTERED, PARAM_SONAR_KALMAN_L1, PARAM_SONAR_KALMAN_L2, @@ -122,32 +131,22 @@ enum global_param_id_t PARAM_USB_SEND_VIDEO, PARAM_USB_SEND_FLOW, PARAM_USB_SEND_GYRO, - PARAM_USB_SEND_FLOW_OUTL, PARAM_USB_SEND_FORWARD, PARAM_USB_SEND_DEBUG, - - PARAM_QUALITY_FILTER, - PARAM_USE_MEDIAN, + PARAM_USB_SEND_FLOW_OUTL, + PARAM_USB_SEND_QUAL_0, PARAM_VIDEO_ONLY, PARAM_VIDEO_RATE, - PARAM_FLOW_FEATURE_THRESHOLD, + PARAM_FLOW_MAX_PIXEL, PARAM_FLOW_VALUE_THRESHOLD, - PARAM_FLOW_HIST_FILTER, + PARAM_FLOW_FEATURE_THRESHOLD, PARAM_FLOW_GYRO_COMPENSATION, - PARAM_FLOW_LP_FILTERED, - PARAM_FLOW_WEIGHT_NEW, PARAM_FLOW_SERIAL_THROTTLE_FACTOR, DEBUG_VARIABLE, - PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_0, - PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_1, - PARAM_USB_SEND_QUAL_0, - - PARAM_OUTLIER_THRESHOLD, - PARAM_MIN_VALID_RATIO, ONBOARD_PARAM_COUNT diff --git a/src/flow.c b/src/flow.c index edf7662..f2a73c4 100644 --- a/src/flow.c +++ b/src/flow.c @@ -44,13 +44,14 @@ #include "flow.h" #include "dcmi.h" #include "debug.h" +#include "timer.h" #define __INLINE inline #define __ASM asm #include "core_cm4_simd.h" #define FRAME_SIZE global_data.param[PARAM_IMAGE_WIDTH] -#define SEARCH_SIZE global_data.param[PARAM_MAX_FLOW_PIXEL] // maximum offset to search: 4 + 1/2 pixels +#define SEARCH_SIZE global_data.param[PARAM_FLOW_MAX_PIXEL] // maximum offset to search: 4 + 1/2 pixels #define TILE_SIZE 8 // x & y tile size #define NUM_BLOCKS 5 // x & y number of tiles to check #define NUM_BLOCK_KLT 4 @@ -656,7 +657,7 @@ uint16_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat float det = (JTJ[0]*JTJ[3]-JTJ[1]*JTJ[2]); float dyn_range = (float)(max_val - min_val) + 1; float trace = (JTJ[0] + JTJ[3]); - float M_c = det - global_data.param[PARAM_CORNER_KAPPA] * trace * trace; + float M_c = det - global_data.param[PARAM_ALGORITHM_CORNER_KAPPA] * trace * trace; if (fabs(det) > global_data.param[PARAM_KLT_DET_VALUE_MIN] * dyn_range && M_c > 0.0f) { float detinv = 1.f / det; diff --git a/src/i2c.c b/src/i2c.c index 2358b0d..962a3ad 100644 --- a/src/i2c.c +++ b/src/i2c.c @@ -246,7 +246,7 @@ void update_TX_buffer(float dt, float x_rate, float y_rate, float z_rate, int16_ result_accumulator_output_flow output_flow; result_accumulator_output_flow_i2c output_i2c; - int min_valid_ratio = global_data.param[PARAM_MIN_VALID_RATIO]; + int min_valid_ratio = global_data.param[PARAM_ALGORITHM_MIN_VALID_RATIO]; result_accumulator_calculate_output_flow(&accumulator, min_valid_ratio, &output_flow); result_accumulator_calculate_output_flow_i2c(&accumulator, min_valid_ratio, &output_i2c); diff --git a/src/main.c b/src/main.c index ec721dd..1895f2e 100644 --- a/src/main.c +++ b/src/main.c @@ -327,12 +327,12 @@ int main(void) int skipped_frames = 0; int loop_count = 0; //< make sure that we dont end up in an infinite loop inside here .. do { - skipped_frames = dma_copy_image_buffers(¤t_image, &previous_image, image_size, (int)(global_data.param[PARAM_FRAME_INTERVAL] + 0.5)); + skipped_frames = dma_copy_image_buffers(¤t_image, &previous_image, image_size, (int)(global_data.param[PARAM_IMAGE_INTERVAL] + 0.5)); start_computations = get_boot_time_us(); /* filter the new image */ - if (global_data.param[PARAM_USE_IMAGE_FILTER]) { + if (global_data.param[PARAM_ALGORITHM_IMAGE_FILTER]) { filter_image(current_image, global_data.param[PARAM_IMAGE_WIDTH]); } @@ -363,14 +363,14 @@ int main(void) /* calculate flow value from the raw results */ float pixel_flow_x; float pixel_flow_y; - float outlier_threshold = global_data.param[PARAM_OUTLIER_THRESHOLD]; + float outlier_threshold = global_data.param[PARAM_ALGORITHM_OUTLIER_THR_RATIO]; float min_outlier_threshold = 0; if(global_data.param[PARAM_ALGORITHM_CHOICE] == 0) { - min_outlier_threshold = global_data.param[PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_0]; + min_outlier_threshold = global_data.param[PARAM_ALGORITHM_OUTLIER_THR_BLOCK]; }else { - min_outlier_threshold = global_data.param[PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_1]; + min_outlier_threshold = global_data.param[PARAM_ALGORITHM_OUTLIER_THR_KLT]; } uint8_t qual = flow_extract_result(flow_rslt, flow_rslt_count, &pixel_flow_x, &pixel_flow_y, outlier_threshold, min_outlier_threshold); @@ -440,7 +440,7 @@ int main(void) /* calculate the output values */ result_accumulator_output_flow output_flow; result_accumulator_output_flow_rad output_flow_rad; - int min_valid_ratio = global_data.param[PARAM_MIN_VALID_RATIO]; + int min_valid_ratio = global_data.param[PARAM_ALGORITHM_MIN_VALID_RATIO]; result_accumulator_calculate_output_flow(&mavlink_accumulator, min_valid_ratio, &output_flow); result_accumulator_calculate_output_flow_rad(&mavlink_accumulator, min_valid_ratio, &output_flow_rad); diff --git a/src/settings.c b/src/settings.c index dfd99b2..e0b3ab4 100644 --- a/src/settings.c +++ b/src/settings.c @@ -78,39 +78,6 @@ void global_data_reset_param_defaults(void){ global_data.param_access[PARAM_SYSTEM_SEND_LPOS] = READ_WRITE; - global_data.param[PARAM_ALGORITHM_CHOICE] = 1; - strcpy(global_data.param_name[PARAM_ALGORITHM_CHOICE], "ALGO_CHOICE"); - global_data.param_access[PARAM_ALGORITHM_CHOICE] = READ_WRITE; - - global_data.param[PARAM_USE_IMAGE_FILTER] = 0; - strcpy(global_data.param_name[PARAM_USE_IMAGE_FILTER], "USE_IMAGE_FILT"); - global_data.param_access[PARAM_USE_IMAGE_FILTER] = READ_WRITE; - - global_data.param[PARAM_FRAME_INTERVAL] = 1; - strcpy(global_data.param_name[PARAM_FRAME_INTERVAL], "FRAME_INTERVAL"); - global_data.param_access[PARAM_FRAME_INTERVAL] = READ_WRITE; - - global_data.param[PARAM_QUALITY_FILTER] = 1; - strcpy(global_data.param_name[PARAM_QUALITY_FILTER], "QUALITY_FILTER"); - global_data.param_access[PARAM_QUALITY_FILTER] = READ_WRITE; - - global_data.param[PARAM_USE_MEDIAN] = 1; - strcpy(global_data.param_name[PARAM_USE_MEDIAN], "USE_MEDIAN"); - global_data.param_access[PARAM_USE_MEDIAN] = READ_WRITE; - - global_data.param[PARAM_KLT_MAX_ITERS] = 5; - strcpy(global_data.param_name[PARAM_KLT_MAX_ITERS], "KLT_MAX_ITERS"); - global_data.param_access[PARAM_KLT_MAX_ITERS] = READ_WRITE; - - global_data.param[PARAM_KLT_DET_VALUE_MIN] = 90; - strcpy(global_data.param_name[PARAM_KLT_DET_VALUE_MIN], "DET_VALUE_MIN"); - global_data.param_access[PARAM_KLT_DET_VALUE_MIN] = READ_WRITE; - - global_data.param[PARAM_CORNER_KAPPA] = 0.06; - strcpy(global_data.param_name[PARAM_CORNER_KAPPA], "CORNER_KAPPA"); - global_data.param_access[PARAM_CORNER_KAPPA] = READ_WRITE; - - global_data.param[PARAM_USART2_BAUD] = 115200; strcpy(global_data.param_name[PARAM_USART2_BAUD], "USART_2_BAUD"); global_data.param_access[PARAM_USART2_BAUD] = READ_ONLY; @@ -120,10 +87,12 @@ void global_data_reset_param_defaults(void){ strcpy(global_data.param_name[PARAM_USART3_BAUD], "USART_3_BAUD"); global_data.param_access[PARAM_USART3_BAUD] = READ_ONLY; + global_data.param[PARAM_FOCAL_LENGTH_MM] = 16.0f; strcpy(global_data.param_name[PARAM_FOCAL_LENGTH_MM], "LENS_FOCAL_LEN"); global_data.param_access[PARAM_FOCAL_LENGTH_MM] = READ_WRITE; + global_data.param[PARAM_IMAGE_WIDTH] = FLOW_IMAGE_WIDTH; strcpy(global_data.param_name[PARAM_IMAGE_WIDTH], "IMAGE_WIDTH"); global_data.param_access[PARAM_IMAGE_WIDTH] = READ_ONLY; @@ -145,6 +114,49 @@ void global_data_reset_param_defaults(void){ strcpy(global_data.param_name[PARAM_IMAGE_TEST_PATTERN], "IMAGE_TEST_PAT"); global_data.param_access[PARAM_IMAGE_TEST_PATTERN] = READ_WRITE; + global_data.param[PARAM_IMAGE_INTERVAL] = 1; + strcpy(global_data.param_name[PARAM_IMAGE_INTERVAL], "IMAGE_INTERVAL"); + global_data.param_access[PARAM_IMAGE_INTERVAL] = READ_WRITE; + + + global_data.param[PARAM_ALGORITHM_CHOICE] = 1; + strcpy(global_data.param_name[PARAM_ALGORITHM_CHOICE], "ALG_CHOICE"); + global_data.param_access[PARAM_ALGORITHM_CHOICE] = READ_WRITE; + + global_data.param[PARAM_ALGORITHM_IMAGE_FILTER] = 0; + strcpy(global_data.param_name[PARAM_ALGORITHM_IMAGE_FILTER], "ALG_IMG_FILT"); + global_data.param_access[PARAM_ALGORITHM_IMAGE_FILTER] = READ_WRITE; + + global_data.param[PARAM_ALGORITHM_CORNER_KAPPA] = 0.06; + strcpy(global_data.param_name[PARAM_ALGORITHM_CORNER_KAPPA], "ALG_CORN_KAPPA"); + global_data.param_access[PARAM_ALGORITHM_CORNER_KAPPA] = READ_WRITE; + + global_data.param[PARAM_ALGORITHM_OUTLIER_THR_BLOCK] = 1.0f; + strcpy(global_data.param_name[PARAM_ALGORITHM_OUTLIER_THR_BLOCK], "ALG_THR_BM"); + global_data.param_access[PARAM_ALGORITHM_OUTLIER_THR_BLOCK] = READ_WRITE; + + global_data.param[PARAM_ALGORITHM_OUTLIER_THR_KLT] = 0.2f; + strcpy(global_data.param_name[PARAM_ALGORITHM_OUTLIER_THR_KLT], "ALG_THR_KLT"); + global_data.param_access[PARAM_ALGORITHM_OUTLIER_THR_KLT] = READ_WRITE; + + global_data.param[PARAM_ALGORITHM_OUTLIER_THR_RATIO] = 0.15f; + strcpy(global_data.param_name[PARAM_ALGORITHM_OUTLIER_THR_RATIO], "ALG_THR_RATIO"); + global_data.param_access[PARAM_ALGORITHM_OUTLIER_THR_RATIO] = READ_WRITE; + + global_data.param[PARAM_ALGORITHM_MIN_VALID_RATIO] = 50; + strcpy(global_data.param_name[PARAM_ALGORITHM_MIN_VALID_RATIO], "ALG_VALID_RATIO"); + global_data.param_access[PARAM_ALGORITHM_MIN_VALID_RATIO] = READ_WRITE; + + + global_data.param[PARAM_KLT_MAX_ITERS] = 5; + strcpy(global_data.param_name[PARAM_KLT_MAX_ITERS], "KLT_MAX_ITERS"); + global_data.param_access[PARAM_KLT_MAX_ITERS] = READ_WRITE; + + global_data.param[PARAM_KLT_DET_VALUE_MIN] = 90; + strcpy(global_data.param_name[PARAM_KLT_DET_VALUE_MIN], "KLT_DET_MIN"); + global_data.param_access[PARAM_KLT_DET_VALUE_MIN] = READ_WRITE; + + global_data.param[PARAM_GYRO_SENSITIVITY_DPS] = 250; strcpy(global_data.param_name[PARAM_GYRO_SENSITIVITY_DPS], "GYRO_SENS_DPS"); global_data.param_access[PARAM_GYRO_SENSITIVITY_DPS] = READ_WRITE; @@ -165,6 +177,7 @@ void global_data_reset_param_defaults(void){ strcpy(global_data.param_name[PARAM_SONAR_KALMAN_L2], "SONAR_KAL_L2"); global_data.param_access[PARAM_SONAR_KALMAN_L2] = READ_WRITE; + global_data.param[PARAM_USB_SEND_VIDEO] = 1; // send video over USB strcpy(global_data.param_name[PARAM_USB_SEND_VIDEO], "USB_SEND_VIDEO"); global_data.param_access[PARAM_USB_SEND_VIDEO] = READ_WRITE; @@ -186,9 +199,13 @@ void global_data_reset_param_defaults(void){ global_data.param_access[PARAM_USB_SEND_DEBUG] = READ_WRITE; global_data.param[PARAM_USB_SEND_FLOW_OUTL] = 1; // send flow outlier debug - strcpy(global_data.param_name[PARAM_USB_SEND_FLOW_OUTL], "USB_SEND_FLOW_O"); + strcpy(global_data.param_name[PARAM_USB_SEND_FLOW_OUTL], "USB_SEND_FLW_OL"); global_data.param_access[PARAM_USB_SEND_FLOW_OUTL] = READ_WRITE; + global_data.param[PARAM_USB_SEND_QUAL_0] = 1; + strcpy(global_data.param_name[PARAM_USB_SEND_QUAL_0], "USB_SEND_Q_0"); + global_data.param_access[PARAM_USB_SEND_QUAL_0] = READ_WRITE; + global_data.param[PARAM_VIDEO_ONLY] = 0; strcpy(global_data.param_name[PARAM_VIDEO_ONLY], "VIDEO_ONLY"); @@ -198,9 +215,10 @@ void global_data_reset_param_defaults(void){ strcpy(global_data.param_name[PARAM_VIDEO_RATE], "VIDEO_RATE"); global_data.param_access[PARAM_VIDEO_RATE] = READ_WRITE; - global_data.param[PARAM_MAX_FLOW_PIXEL] = FLOW_SEARCH_WINDOW_SIZE; - strcpy(global_data.param_name[PARAM_MAX_FLOW_PIXEL], "FLOW_MAX_PIX"); - global_data.param_access[PARAM_MAX_FLOW_PIXEL] = READ_ONLY; + + global_data.param[PARAM_FLOW_MAX_PIXEL] = FLOW_SEARCH_WINDOW_SIZE; + strcpy(global_data.param_name[PARAM_FLOW_MAX_PIXEL], "FLOW_MAX_PIX"); + global_data.param_access[PARAM_FLOW_MAX_PIXEL] = READ_ONLY; // global_data.param[PARAM_FLOW_VALUE_THRESHOLD] = 8 * 8 * 20; global_data.param[PARAM_FLOW_VALUE_THRESHOLD] = 5000; // threshold is irrelevant with this value @@ -212,54 +230,18 @@ void global_data_reset_param_defaults(void){ strcpy(global_data.param_name[PARAM_FLOW_FEATURE_THRESHOLD], "FLOW_F_THLD"); global_data.param_access[PARAM_FLOW_FEATURE_THRESHOLD] = READ_WRITE; - global_data.param[PARAM_FLOW_HIST_FILTER] = 0; -// global_data.param[PARAM_FLOW_HIST_FILTER] = 1; - strcpy(global_data.param_name[PARAM_FLOW_HIST_FILTER], "FLOW_HIST_FIL"); - global_data.param_access[PARAM_FLOW_HIST_FILTER] = READ_WRITE; - -// global_data.param[PARAM_FLOW_GYRO_COMPENSATION] = 0; global_data.param[PARAM_FLOW_GYRO_COMPENSATION] = 1; strcpy(global_data.param_name[PARAM_FLOW_GYRO_COMPENSATION], "FLOW_GYRO_COM"); global_data.param_access[PARAM_FLOW_GYRO_COMPENSATION] = READ_WRITE; - global_data.param[PARAM_FLOW_LP_FILTERED] = 0; - strcpy(global_data.param_name[PARAM_FLOW_LP_FILTERED], "FLOW_LP_FIL"); - global_data.param_access[PARAM_FLOW_LP_FILTERED] = READ_WRITE; - - global_data.param[PARAM_FLOW_WEIGHT_NEW] = 0.3f; - strcpy(global_data.param_name[PARAM_FLOW_WEIGHT_NEW], "FLOW_W_NEW"); - global_data.param_access[PARAM_FLOW_WEIGHT_NEW] = READ_WRITE; - global_data.param[PARAM_FLOW_SERIAL_THROTTLE_FACTOR] = 10.0f; strcpy(global_data.param_name[PARAM_FLOW_SERIAL_THROTTLE_FACTOR], "FLOW_THROTT"); global_data.param_access[PARAM_FLOW_SERIAL_THROTTLE_FACTOR] = READ_WRITE; - global_data.param[PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_0] = 1.0f; - strcpy(global_data.param_name[PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_0], "MIN_OUT_THR_A0"); - global_data.param_access[PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_0] = READ_WRITE; - - global_data.param[PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_1] = 0.2f; - strcpy(global_data.param_name[PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_1], "MIN_OUT_THR_A1"); - global_data.param_access[PARAM_MINIMUM_OUTLIER_THRESHOLD_AlGO_1] = READ_WRITE; - - global_data.param[PARAM_OUTLIER_THRESHOLD] = 0.15f; - strcpy(global_data.param_name[PARAM_OUTLIER_THRESHOLD], "OUTLIER_THR"); - global_data.param_access[PARAM_OUTLIER_THRESHOLD] = READ_WRITE; - - global_data.param[PARAM_USB_SEND_QUAL_0] = 1; - strcpy(global_data.param_name[PARAM_USB_SEND_QUAL_0], "SEND_QUAL_0"); - global_data.param_access[PARAM_USB_SEND_QUAL_0] = READ_WRITE; - - global_data.param[PARAM_MIN_VALID_RATIO] = 50; - strcpy(global_data.param_name[PARAM_MIN_VALID_RATIO], "MIN_VALID_RATIO"); - global_data.param_access[PARAM_MIN_VALID_RATIO] = READ_WRITE; global_data.param[DEBUG_VARIABLE] = 1; strcpy(global_data.param_name[DEBUG_VARIABLE], "DEBUG"); global_data.param_access[DEBUG_VARIABLE] = READ_WRITE; - - - } /** From 65946f80c26aafed5309adfed4dc3ac69032f351 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Mon, 13 Jul 2015 16:31:37 +0200 Subject: [PATCH 028/120] Remove now obsolete quality_measurement from code. --- Makefile | 1 - inc/quality_measurement.h | 55 ------------ src/main.c | 1 - src/quality_measurement.c | 176 -------------------------------------- 4 files changed, 233 deletions(-) delete mode 100644 inc/quality_measurement.h delete mode 100644 src/quality_measurement.c diff --git a/Makefile b/Makefile index 9e2151f..32de070 100644 --- a/Makefile +++ b/Makefile @@ -25,7 +25,6 @@ SRCS = src/main.c \ src/settings.c \ src/communication.c \ src/filter.c \ - src/quality_measurement.c \ src/flow.c \ src/dcmi.c \ src/mt9v034.c \ diff --git a/inc/quality_measurement.h b/inc/quality_measurement.h deleted file mode 100644 index b2c74d6..0000000 --- a/inc/quality_measurement.h +++ /dev/null @@ -1,55 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#ifndef QUALITY_H_ -#define QUALITY_H_ - -#include -typedef struct _qual_output { - uint8_t qual; - float mean_x; - float mean_y; - float median_x; - float median_y; - float var; - float covar; - float qual_iqr; -} qual_output; - -/** - * @brief Computes the quality of the flow algorithm using the calculated pixel flow - * @note It uses the space after the image in the image buffer to work on the image. - */ -qual_output quality_new_measurement(float pixel_flow_x, float pixel_flow_y, float dt, float qual_iqr); - -#endif /* QUALITY_H_ */ diff --git a/src/main.c b/src/main.c index 1895f2e..a855069 100644 --- a/src/main.c +++ b/src/main.c @@ -50,7 +50,6 @@ #include "led.h" #include "filter.h" #include "result_accumulator.h" -#include "quality_measurement.h" #include "flow.h" #include "timer.h" #include "dcmi.h" diff --git a/src/quality_measurement.c b/src/quality_measurement.c deleted file mode 100644 index 0e495af..0000000 --- a/src/quality_measurement.c +++ /dev/null @@ -1,176 +0,0 @@ - -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Petri Tanskanen - * Lorenz Meier - * Samuel Zihlmann - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include -#include -#include -#include -#include -#include - -#include "settings.h" - -#define __INLINE inline -#define __ASM asm -#include "core_cm4_simd.h" - -#include "quality_measurement.h" - -#define QUEUE_SIZE 80 - -typedef struct _qual_datapoint { - float x; - float y; - float x_orig; - float y_orig; - float qual_iqr; -} qual_datapoint; - -static qual_datapoint in = {}; -static qual_datapoint arr[QUEUE_SIZE] = {}; -static int head = 0; //starting index of the queue - -static qual_datapoint input_flow_filter_state = {}; - -static qual_output qual_output_point = {}; - -static void enqueue(qual_datapoint *q, int *head, qual_datapoint data) { - q[(*head)] = data; - (*head)++; - if((*head) >= QUEUE_SIZE) - (*head) = 0; -} - -int floatcompare(const void* elem1, const void* elem2) -{ - if(*(const float*)elem1 < *(const float*)elem2) - return -1; - return *(const float*)elem1 > *(const float*)elem2; -} - -static struct _qual_output quality_calculate(){ - float sum_flow_x = 0; - float sum_flow_y = 0; - float sum2_flow_x = 0; - float sum2_flow_y = 0; - float mean_qual_iqr = 0; - float sum_qual_iqr = 0; - float x_array[QUEUE_SIZE] = {}; - float y_array[QUEUE_SIZE] = {}; - - for(int i = 0; i < QUEUE_SIZE; i++) - { - x_array[i] = arr[i].x; - y_array[i] = arr[i].y; - } - - //calculate median of flow - qsort(x_array, QUEUE_SIZE, sizeof (float), floatcompare); - qsort(y_array, QUEUE_SIZE, sizeof (float), floatcompare); - if(QUEUE_SIZE % 2 == 0) - { - qual_output_point.median_x = (x_array[QUEUE_SIZE / 2 - 1] + x_array[QUEUE_SIZE / 2]) / 2; - qual_output_point.median_y = (x_array[QUEUE_SIZE / 2 - 1] + x_array[QUEUE_SIZE / 2]) / 2; - } - else - { - qual_output_point.median_x = x_array[(QUEUE_SIZE - 1) / 2]; - qual_output_point.median_y = x_array[(QUEUE_SIZE - 1) / 2]; - } - - //calculate mean - for(int i = 0; i < QUEUE_SIZE; i++){ - sum_flow_x += arr[i].x; - sum_flow_y += arr[i].y; - sum_qual_iqr += arr[i].qual_iqr; - } - qual_output_point.mean_x = sum_flow_x/QUEUE_SIZE; - qual_output_point.mean_y = sum_flow_y/QUEUE_SIZE; - mean_qual_iqr = sum_qual_iqr/QUEUE_SIZE; - - //calculate variance - for(int i = 0; i < QUEUE_SIZE; i++){ - sum2_flow_x += (arr[i].x - qual_output_point.mean_x) * (arr[i].x - qual_output_point.mean_x); - sum2_flow_y += (arr[i].y - qual_output_point.mean_y) * (arr[i].y - qual_output_point.mean_y); - } - qual_output_point.var = ((sum2_flow_x + sum2_flow_y) / QUEUE_SIZE) / (1.0 + sqrt(in.x_orig * in.x_orig + - in.y_orig * in.y_orig)); - - //calculate covariance between flow_y and flow_x - float total = 0.0f; - for(int i = 0; i < QUEUE_SIZE; i++){ - total += (arr[i].x - qual_output_point.mean_x) * (arr[i].y - qual_output_point.mean_y); - } - qual_output_point.covar = total/QUEUE_SIZE; - - qual_output_point.qual = (int)sum_flow_y/QUEUE_SIZE; - qual_output_point.qual_iqr = in.qual_iqr; - - return qual_output_point; -} - -#define FILTER_TAU 4910 - -qual_output quality_new_measurement(float pixel_flow_x, float pixel_flow_y, float dt, float qual_iqr) { - - // high pass filter with exponential filter: - in.x_orig = pixel_flow_x; - in.y_orig = pixel_flow_y; - if(global_data.param[PARAM_QUALITY_FILTER] > 0) - { - float alpha = 1.0 - exp(- dt / FILTER_TAU); - input_flow_filter_state.x = alpha*(pixel_flow_x/dt) + (1.0 - alpha)*input_flow_filter_state.x; - input_flow_filter_state.y = alpha*(pixel_flow_y/dt) + (1.0 - alpha)*input_flow_filter_state.y; - in.x = pixel_flow_x - input_flow_filter_state.x; - in.y = pixel_flow_y - input_flow_filter_state.y; - in.qual_iqr = qual_iqr; - enqueue(arr, &head, in); - } - else - { - in.x = pixel_flow_x/dt; - in.y = pixel_flow_y/dt; - in.qual_iqr = qual_iqr; - enqueue(arr, &head, in); - } - - // queue into circular buffer: - return quality_calculate(); -} - - From 9eb6112384206918d601ee247bddf9d57476f1ec Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Mon, 13 Jul 2015 16:50:37 +0200 Subject: [PATCH 029/120] implemented changing video downlink rate. --- inc/timer.h | 8 +++++++- src/main.c | 3 +++ src/stm32f4xx_it.c | 2 +- src/timer.c | 20 +++++++++++++++++++- 4 files changed, 30 insertions(+), 3 deletions(-) diff --git a/inc/timer.h b/inc/timer.h index 4e61b84..734ecb0 100644 --- a/inc/timer.h +++ b/inc/timer.h @@ -50,6 +50,12 @@ void timer_init(void); */ void timer_register(void (*timer_fn)(void), uint32_t period_ms); +/** Updates the period of an timer that has already been registered. + * @param timer_fn The timer function that has been registered. + * @param period_ms The new period of the timer in milliseconds. + */ +int timer_update(void (*timer_fn)(void), uint32_t period_ms); + /** Checks any pending timers and calls the respective timer functions. */ void timer_check(void); @@ -77,6 +83,6 @@ uint32_t get_time_delta_us(uint32_t start); * @param None * @retval None */ -void timer_update(void); +void timer_interrupt_fn(void); #endif /* TIMER_H_ */ diff --git a/src/main.c b/src/main.c index a855069..6a1105d 100644 --- a/src/main.c +++ b/src/main.c @@ -121,6 +121,9 @@ void system_receive_fn(void) { } void send_video_fn(void) { + /* update the rate */ + timer_update(send_video_fn, global_data.param[PARAM_VIDEO_RATE]); + /* transmit raw 8-bit image */ if (global_data.param[PARAM_USB_SEND_VIDEO]) { diff --git a/src/stm32f4xx_it.c b/src/stm32f4xx_it.c index fd5a928..8e2eb69 100644 --- a/src/stm32f4xx_it.c +++ b/src/stm32f4xx_it.c @@ -136,7 +136,7 @@ void PendSV_Handler(void) */ void SysTick_Handler(void) { - timer_update(); + timer_interrupt_fn(); } /******************************************************************************/ diff --git a/src/timer.c b/src/timer.c index 3544e75..b72ab83 100644 --- a/src/timer.c +++ b/src/timer.c @@ -70,7 +70,7 @@ static timer_info timer[NTIMERS]; * @param None * @retval None */ -void timer_update(void) +void timer_interrupt_fn(void) { boot_time_ms++; /* we do it like this to have correct roll-over behaviour */ @@ -131,6 +131,24 @@ void timer_register(void (*timer_fn)(void), uint32_t period_ms) { timer[idx].flags = TIMER_FLAG_ENABLED; } +int timer_update(void (*timer_fn)(void), uint32_t period_ms) { + /* find the timer: */ + int idx; + for (idx = 0; idx < NTIMERS; idx++) { + if ((timer[idx].flags & TIMER_FLAG_ENABLED) != 0 && timer[idx].timer_fn == timer_fn) { + break; + } + } + if (idx >= NTIMERS) { + return -1; + } + /* update the period */ + if (timer[idx].period != period_ms) { + timer[idx].period = period_ms; + } + return 0; +} + void timer_init(void) { /* init clock */ From 7a02bb12a59ff944c5bd85992648ca62580f8d8a Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Mon, 13 Jul 2015 17:13:49 +0200 Subject: [PATCH 030/120] make VIDEO_ONLY mode work again. --- src/main.c | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main.c b/src/main.c index 6a1105d..7db7a3d 100644 --- a/src/main.c +++ b/src/main.c @@ -123,9 +123,9 @@ void system_receive_fn(void) { void send_video_fn(void) { /* update the rate */ timer_update(send_video_fn, global_data.param[PARAM_VIDEO_RATE]); - + /* transmit raw 8-bit image */ - if (global_data.param[PARAM_USB_SEND_VIDEO]) + if (global_data.param[PARAM_USB_SEND_VIDEO] && !global_data.param[PARAM_VIDEO_ONLY]) { /* get size of image to send */ uint16_t image_size_send; @@ -285,9 +285,11 @@ int main(void) if(buffer_reset_needed) { buffer_reset_needed = 0; - /* get two new fresh (full) images: (or 8 small images ..) */ - dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 4); - dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 4); + if(!global_data.param[PARAM_VIDEO_ONLY]) { + /* get two new fresh images: */ + dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1); + dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1); + } continue; } From bef8bb39f362129652e3fd79fc08f300ecb1e587 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Tue, 14 Jul 2015 07:51:21 +0200 Subject: [PATCH 031/120] include the new source files in CMakeLists.txt --- CMakeLists.txt | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d5c0df8..7864648 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -184,6 +184,7 @@ set(PX4FLOW_HDRS inc/communication.h inc/dcmi.h inc/debug.h + inc/filter.h inc/flow.h inc/gyro.h inc/i2c_frame.h @@ -192,11 +193,13 @@ set(PX4FLOW_HDRS inc/main.h inc/mavlink_bridge_header.h inc/mt9v034.h + inc/result_accumulator.h inc/settings.h inc/sonar.h inc/sonar_mode_filter.h inc/stm32f4xx_conf.h inc/stm32f4xx_it.h + inc/timer.h inc/usart.h inc/usb_conf.h inc/usbd_cdc_vcp.h @@ -209,6 +212,7 @@ set(PX4FLOW_SRC src/communication.c src/dcmi.c src/debug.c + src/filter.c src/flow.c src/gyro.c src/i2c.c @@ -216,19 +220,19 @@ set(PX4FLOW_SRC src/main.c src/mt9v034.c src/reset.c + src/result_accumulator.c src/settings.c src/sonar.c src/sonar_mode_filter.c src/stm32f4xx_it.c src/system_stm32f4xx.c + src/timer.c src/usart.c src/usb_bsp.c src/usbd_cdc_vcp.c src/usbd_desc.c src/usbd_usr.c src/utils.c - src/system_stm32f4xx.c - src/stm32f4xx_it.c ) # building From 63a7aac78d20de9f2e4abf7050e5d18b35c16755 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Tue, 14 Jul 2015 11:33:57 +0200 Subject: [PATCH 032/120] add (some) missing register definitions and change naming convention to the datasheet naming. fixed writing to wrong register --- inc/mt9v034.h | 32 +++++++++++++++++++++++--------- src/mt9v034.c | 23 +++++++++++------------ 2 files changed, 34 insertions(+), 21 deletions(-) diff --git a/inc/mt9v034.h b/inc/mt9v034.h index c3c16b5..c646cd7 100644 --- a/inc/mt9v034.h +++ b/inc/mt9v034.h @@ -74,6 +74,7 @@ #define MTV_V3_CTRL_REG_A 0x33 #define MTV_V4_CTRL_REG_A 0x34 #define MTV_ANALOG_GAIN_CTRL_REG_A 0x35 +#define MTV_BLC_VALUE_REG_A 0x48 /* Context B */ #define MTV_COLUMN_START_REG_B 0xC9 @@ -101,22 +102,35 @@ #define MTV_CHIP_CONTROL_REG 0x07 #define MTV_SOFT_RESET_REG 0x0C -#define MTV_HDR_ENABLE_REG 0x0F +#define MTV_SENSOR_TYPE_CTRL_REG 0x0F +#define MTV_LED_OUT_CTRL_REG 0x1B #define MTV_ADC_RES_CTRL_REG 0x1C +#define MTV_VREF_ADC_CTRL_REG 0x2C +#define MTV_FRAME_DARK_AVG_REG 0x42 +#define MTV_FRAME_DARK_AVG_THR_REG 0x46 +#define MTV_BLC_CTRL_REG 0x47 +#define MTV_BLC_VALUE_STEP_SIZE_REG 0x4C #define MTV_ROW_NOISE_CORR_CTRL_REG 0x70 +#define MTV_ROW_NOISE_CONST_REG 0x71 +#define MTV_PIXEL_FRAME_LINE_CTRL_REG 0x72 #define MTV_TEST_PATTERN_REG 0x7F #define MTV_TILED_DIGITAL_GAIN_REG 0x80 #define MTV_AGC_AEC_DESIRED_BIN_REG 0xA5 -#define MTV_MAX_GAIN_REG 0xAB -#define MTV_MIN_EXPOSURE_REG 0xAC // datasheet min coarse shutter width -#define MTV_MAX_EXPOSURE_REG 0xAD // datasheet max coarse shutter width +#define MTV_AEC_UPDATE_FREQ_REG 0xA6 +#define MTV_AEC_LPF_REG 0xA8 +#define MTV_AGC_UPDATE_FREQ_REG 0xA9 +#define MTV_AGC_LPF_REG 0xAA +#define MTV_ANALOG_MAX_GAIN_REG 0xAB +#define MTV_MIN_COARSE_SW_REG 0xAC +#define MTV_MAX_COARSE_SW_REG 0xAD +#define MTV_AEC_AGC_BIN_DIFF_THR_REG 0xAE #define MTV_AEC_AGC_ENABLE_REG 0xAF #define MTV_AGC_AEC_PIXEL_COUNT_REG 0xB0 -#define MTV_AEC_UPDATE_REG 0xA6 -#define MTV_AEC_LOWPASS_REG 0xA8 -#define MTV_AGC_UPDATE_REG 0xA9 -#define MTV_AGC_LOWPASS_REG 0xAA -#define MTV_DIGITAL_TEST_REG 0x7F + +#define MTV_AGC_GAIN_OUT_REG 0xBA +#define MTV_AEC_EXPOSURE_REG 0xBB +#define MTV_AGC_AEC_CUR_BIN_REG 0xBC + /* * Resolution: diff --git a/src/mt9v034.c b/src/mt9v034.c index 989a4dd..472fc22 100644 --- a/src/mt9v034.c +++ b/src/mt9v034.c @@ -180,8 +180,7 @@ void mt9v034_context_configuration(void) mt9v034_WriteReg16(MTV_COARSE_SW_1_REG_A, coarse_sw1); mt9v034_WriteReg16(MTV_COARSE_SW_2_REG_A, coarse_sw2); mt9v034_WriteReg16(MTV_COARSE_SW_CTRL_REG_A, shutter_width_ctrl); - mt9v034_WriteReg16(MTV_V2_CTRL_REG_A, total_shutter_width); - + mt9v034_WriteReg16(MTV_COARSE_SW_TOTAL_REG_A, total_shutter_width); /* Context B */ mt9v034_WriteReg16(MTV_WINDOW_WIDTH_REG_B, new_width_context_b); @@ -194,25 +193,25 @@ void mt9v034_context_configuration(void) mt9v034_WriteReg16(MTV_COARSE_SW_1_REG_B, coarse_sw1); mt9v034_WriteReg16(MTV_COARSE_SW_2_REG_B, coarse_sw2); mt9v034_WriteReg16(MTV_COARSE_SW_CTRL_REG_B, shutter_width_ctrl); - mt9v034_WriteReg16(MTV_V2_CTRL_REG_B, total_shutter_width); + mt9v034_WriteReg16(MTV_COARSE_SW_TOTAL_REG_B, total_shutter_width); /* General Settings */ mt9v034_WriteReg16(MTV_ROW_NOISE_CORR_CTRL_REG, row_noise_correction); mt9v034_WriteReg16(MTV_AEC_AGC_ENABLE_REG, aec_agc_enabled); // disable AEC/AGC for both contexts - mt9v034_WriteReg16(MTV_HDR_ENABLE_REG, hdr_enabled); // disable HDR on both contexts - mt9v034_WriteReg16(MTV_MIN_EXPOSURE_REG, min_exposure); - mt9v034_WriteReg16(MTV_MAX_EXPOSURE_REG, max_exposure); - mt9v034_WriteReg16(MTV_MAX_GAIN_REG, new_max_gain); + mt9v034_WriteReg16(MTV_SENSOR_TYPE_CTRL_REG, hdr_enabled); // disable HDR on both contexts + mt9v034_WriteReg16(MTV_MIN_COARSE_SW_REG, min_exposure); + mt9v034_WriteReg16(MTV_MAX_COARSE_SW_REG, max_exposure); + mt9v034_WriteReg16(MTV_ANALOG_MAX_GAIN_REG, new_max_gain); mt9v034_WriteReg16(MTV_AGC_AEC_PIXEL_COUNT_REG, pixel_count); mt9v034_WriteReg16(MTV_AGC_AEC_DESIRED_BIN_REG, desired_brightness); mt9v034_WriteReg16(MTV_ADC_RES_CTRL_REG, resolution_ctrl); // here is the way to regulate darkness :) - mt9v034_WriteReg16(MTV_DIGITAL_TEST_REG, test_data);//enable test pattern + mt9v034_WriteReg16(MTV_TEST_PATTERN_REG, test_data);//enable test pattern - mt9v034_WriteReg16(MTV_AEC_UPDATE_REG,aec_update_freq); - mt9v034_WriteReg16(MTV_AEC_LOWPASS_REG,aec_low_pass); - mt9v034_WriteReg16(MTV_AGC_UPDATE_REG,agc_update_freq); - mt9v034_WriteReg16(MTV_AGC_LOWPASS_REG,agc_low_pass); + mt9v034_WriteReg16(MTV_AEC_UPDATE_FREQ_REG,aec_update_freq); + mt9v034_WriteReg16(MTV_AEC_LPF_REG,aec_low_pass); + mt9v034_WriteReg16(MTV_AGC_UPDATE_FREQ_REG,agc_update_freq); + mt9v034_WriteReg16(MTV_AGC_LPF_REG,agc_low_pass); /* Reset */ mt9v034_WriteReg16(MTV_SOFT_RESET_REG, 0x01); From 1cef37d8efeee135a42155d249f6bb0ee547a783 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Tue, 14 Jul 2015 17:41:59 +0200 Subject: [PATCH 033/120] fixed result accumulator to comply with expected scaling of i2c frames --- src/i2c.c | 6 +++--- src/result_accumulator.c | 10 +++++----- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/i2c.c b/src/i2c.c index 962a3ad..c488576 100644 --- a/src/i2c.c +++ b/src/i2c.c @@ -275,12 +275,12 @@ void update_TX_buffer(float dt, float x_rate, float y_rate, float z_rate, int16_ /* write the i2c_integral_frame */ f_integral.frame_count_since_last_readout = output_i2c.valid_frames; - f_integral.gyro_x_rate_integral = output_i2c.integrated_gyro_x; //mrad*10 - f_integral.gyro_y_rate_integral = output_i2c.integrated_gyro_y; //mrad*10 + f_integral.gyro_x_rate_integral = output_i2c.integrated_gyro_x; //mrad*10 + f_integral.gyro_y_rate_integral = output_i2c.integrated_gyro_y; //mrad*10 f_integral.gyro_z_rate_integral = output_i2c.integrated_gyro_z; //mrad*10 f_integral.pixel_flow_x_integral = output_i2c.rad_flow_x; //mrad*10 f_integral.pixel_flow_y_integral = output_i2c.rad_flow_y; //mrad*10 - f_integral.integration_timespan = output_i2c.integration_time; //microseconds + f_integral.integration_timespan = output_i2c.integration_time; //microseconds f_integral.ground_distance = output_i2c.ground_distance; //mmeters f_integral.sonar_timestamp = output_i2c.time_delta_distance_us; //microseconds f_integral.qual = output_i2c.quality; //0-255 linear quality measurement 0=bad, 255=best diff --git a/src/result_accumulator.c b/src/result_accumulator.c index 82400b7..36c5d6b 100644 --- a/src/result_accumulator.c +++ b/src/result_accumulator.c @@ -169,11 +169,11 @@ void result_accumulator_calculate_output_flow_i2c(result_accumulator_ctx *ctx, u out->frame_count = ctx->frame_count; out->valid_frames = ctx->valid_data_count; out->integration_time = ctx->valid_time * 1000000.0f; - out->rad_flow_x = floor(ctx->rad_flow_x_accu * 100.0f + 0.5f); - out->rad_flow_y = floor(ctx->rad_flow_y_accu * 100.0f + 0.5f); - out->integrated_gyro_x = floor(ctx->gyro_x_accu * 100.0f + 0.5f); - out->integrated_gyro_y = floor(ctx->gyro_y_accu * 100.0f + 0.5f); - out->integrated_gyro_z = floor(ctx->gyro_z_accu * 100.0f + 0.5f); + out->rad_flow_x = floor(ctx->rad_flow_x_accu * 10000.0f + 0.5f); + out->rad_flow_y = floor(ctx->rad_flow_y_accu * 10000.0f + 0.5f); + out->integrated_gyro_x = floor(ctx->gyro_x_accu * 10000.0f + 0.5f); + out->integrated_gyro_y = floor(ctx->gyro_y_accu * 10000.0f + 0.5f); + out->integrated_gyro_z = floor(ctx->gyro_z_accu * 10000.0f + 0.5f); out->gyro_x = floor(ctx->gyro_x_accu * (100.0f / ctx->valid_time) + 0.5f); out->gyro_y = floor(ctx->gyro_y_accu * (100.0f / ctx->valid_time) + 0.5f); out->gyro_z = floor(ctx->gyro_z_accu * (100.0f / ctx->valid_time) + 0.5f); From 66a25548a9f4c97512b879c23317698cdd67a853 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Wed, 15 Jul 2015 11:48:02 +0200 Subject: [PATCH 034/120] introduced new parameter KLT_GYRO_ASSIST and turned gyro compensation off by default. --- inc/settings.h | 1 + src/flow.c | 2 +- src/settings.c | 6 +++++- 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/inc/settings.h b/inc/settings.h index 8b109dc..b988a44 100644 --- a/inc/settings.h +++ b/inc/settings.h @@ -120,6 +120,7 @@ enum global_param_id_t PARAM_KLT_DET_VALUE_MIN, PARAM_KLT_MAX_ITERS, + PARAM_KLT_GYRO_ASSIST, PARAM_GYRO_SENSITIVITY_DPS, PARAM_GYRO_COMPENSATION_THRESHOLD, diff --git a/src/flow.c b/src/flow.c index f2a73c4..c8a89dc 100644 --- a/src/flow.c +++ b/src/flow.c @@ -586,7 +586,7 @@ uint16_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat for (int x = 0; x < NUM_BLOCK_KLT; x++, i += pixStep) { uint16_t idx = y*NUM_BLOCK_KLT+x; - if (global_data.param[PARAM_FLOW_GYRO_COMPENSATION]) { + if (global_data.param[PARAM_KLT_GYRO_ASSIST]) { /* use the gyro measurement to guess the initial position in the new image */ us[idx] = i + x_rate; //position in new image at level 0 vs[idx] = j + y_rate; diff --git a/src/settings.c b/src/settings.c index e0b3ab4..244d5d5 100644 --- a/src/settings.c +++ b/src/settings.c @@ -156,6 +156,10 @@ void global_data_reset_param_defaults(void){ strcpy(global_data.param_name[PARAM_KLT_DET_VALUE_MIN], "KLT_DET_MIN"); global_data.param_access[PARAM_KLT_DET_VALUE_MIN] = READ_WRITE; + global_data.param[PARAM_KLT_GYRO_ASSIST] = 1; + strcpy(global_data.param_name[PARAM_KLT_GYRO_ASSIST], "KLT_GYRO_ASSIST"); + global_data.param_access[PARAM_KLT_GYRO_ASSIST] = READ_WRITE; + global_data.param[PARAM_GYRO_SENSITIVITY_DPS] = 250; strcpy(global_data.param_name[PARAM_GYRO_SENSITIVITY_DPS], "GYRO_SENS_DPS"); @@ -230,7 +234,7 @@ void global_data_reset_param_defaults(void){ strcpy(global_data.param_name[PARAM_FLOW_FEATURE_THRESHOLD], "FLOW_F_THLD"); global_data.param_access[PARAM_FLOW_FEATURE_THRESHOLD] = READ_WRITE; - global_data.param[PARAM_FLOW_GYRO_COMPENSATION] = 1; + global_data.param[PARAM_FLOW_GYRO_COMPENSATION] = 0; strcpy(global_data.param_name[PARAM_FLOW_GYRO_COMPENSATION], "FLOW_GYRO_COM"); global_data.param_access[PARAM_FLOW_GYRO_COMPENSATION] = READ_WRITE; From ce75ccea5229cba650267bf99506cbaef1a45e8a Mon Sep 17 00:00:00 2001 From: Tom Date: Tue, 28 Jul 2015 17:10:52 -0700 Subject: [PATCH 035/120] Lidar driver in place of the sonar driver --- Makefile | 4 +- ...r_mode_filter.h => distance_mode_filter.h} | 8 +- inc/i2c_frame.h | 4 +- inc/lidar.h | 59 +++ ...r_mode_filter.c => distance_mode_filter.c} | 34 +- src/i2c.c | 16 +- src/lidar.c | 363 ++++++++++++++++++ src/main.c | 63 +-- src/sonar.c | 4 +- unittests/Makefile | 2 +- unittests/tests.c | 4 +- 11 files changed, 493 insertions(+), 68 deletions(-) rename inc/{sonar_mode_filter.h => distance_mode_filter.h} (91%) create mode 100644 inc/lidar.h rename src/{sonar_mode_filter.c => distance_mode_filter.c} (71%) create mode 100644 src/lidar.c diff --git a/Makefile b/Makefile index 1d6c15b..902ec34 100644 --- a/Makefile +++ b/Makefile @@ -33,9 +33,9 @@ SRCS = src/main.c \ src/usbd_cdc_vcp.c \ src/usbd_desc.c \ src/usbd_usr.c \ - src/i2c.c \ src/reset.c \ - src/sonar_mode_filter.c + src/lidar.c \ + src/distance_mode_filter.c SRCS += src/system_stm32f4xx.c src/stm32f4xx_it.c lib/startup_stm32f4xx.s SRCS += lib/STM32F4xx_StdPeriph_Driver/src/misc.c \ lib/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_rcc.c \ diff --git a/inc/sonar_mode_filter.h b/inc/distance_mode_filter.h similarity index 91% rename from inc/sonar_mode_filter.h rename to inc/distance_mode_filter.h index e22e972..0972ebf 100644 --- a/inc/sonar_mode_filter.h +++ b/inc/distance_mode_filter.h @@ -31,17 +31,17 @@ * ****************************************************************************/ -#ifndef SONAR_MODE_FILTER_H_ -#define SONAR_MODE_FILTER_H_ +#ifndef DISTANCE_MODE_FILTER_H_ +#define DISTANCE_MODE_FILTER_H_ #ifdef __cplusplus extern "C" { #endif -float insert_sonar_value_and_get_mode_value(float insert); +float insert_distance_value_and_get_mode_value(float insert); #ifdef __cplusplus } #endif -#endif /* SONAR_MODE_FILTER_H_ */ +#endif /* DISTANCE_MODE_FILTER_H_ */ diff --git a/inc/i2c_frame.h b/inc/i2c_frame.h index b391b1f..343f4a7 100644 --- a/inc/i2c_frame.h +++ b/inc/i2c_frame.h @@ -55,7 +55,7 @@ typedef struct i2c_frame int16_t gyro_y_rate; int16_t gyro_z_rate; uint8_t gyro_range; - uint8_t sonar_timestamp; + uint8_t distance_timestamp; int16_t ground_distance; } i2c_frame; @@ -71,7 +71,7 @@ typedef struct i2c_integral_frame int16_t gyro_y_rate_integral; int16_t gyro_z_rate_integral; uint32_t integration_timespan; - uint32_t sonar_timestamp; + uint32_t distance_timestamp; uint16_t ground_distance; int16_t gyro_temperature; uint8_t qual; diff --git a/inc/lidar.h b/inc/lidar.h new file mode 100644 index 0000000..ba275a2 --- /dev/null +++ b/inc/lidar.h @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file lidar.h + * I2C master mode and lidar communications. + * @author Tom Cauchois + */ + +//FIXME: possible power savings? + +#ifndef LIDAR_H_ +#define LIDAR_H_ +#include +#include + +//Stated range of sensor +#define MINIMUM_DISTANCE 0.0f +#define MAXIMUM_DISTANCE 40.0f + +void i2c_init_master(void); + +void lidar_config(void); +void lidar_trigger(void); +void lidar_readback(void); +bool lidar_read(float* distance_filtered, float* distance_raw); +uint32_t get_lidar_measure_time(void); + +#endif //LIDAR_H_ diff --git a/src/sonar_mode_filter.c b/src/distance_mode_filter.c similarity index 71% rename from src/sonar_mode_filter.c rename to src/distance_mode_filter.c index 086d0db..a6ca913 100644 --- a/src/sonar_mode_filter.c +++ b/src/distance_mode_filter.c @@ -31,53 +31,53 @@ * ****************************************************************************/ -#include "sonar_mode_filter.h" +#include "distance_mode_filter.h" #include /** - * insert-only ring buffer of sonar data, needs to be of uneven size + * insert-only ring buffer of distance data, needs to be of uneven size * the initialization to zero will make the filter respond zero for the * first three inserted readinds, which is a decent startup-logic. */ -static float sonar_values[3] = { 0.0f }; +static float distance_values[3] = { 0.0f }; static unsigned insert_index = 0; -static void sonar_bubble_sort(float sonar_values[], unsigned n); +static void distance_bubble_sort(float distance_values[], unsigned n); -void sonar_bubble_sort(float sonar_values[], unsigned n) +void distance_bubble_sort(float distance_values[], unsigned n) { float t; for (unsigned i = 0; i < (n - 1); i++) { for (unsigned j = 0; j < (n - i - 1); j++) { - if (sonar_values[j] > sonar_values[j+1]) { + if (distance_values[j] > distance_values[j+1]) { /* swap two values */ - t = sonar_values[j]; - sonar_values[j] = sonar_values[j + 1]; - sonar_values[j + 1] = t; + t = distance_values[j]; + distance_values[j] = distance_values[j + 1]; + distance_values[j + 1] = t; } } } } -float insert_sonar_value_and_get_mode_value(float insert) +float insert_distance_value_and_get_mode_value(float insert) { - const unsigned sonar_count = sizeof(sonar_values) / sizeof(sonar_values[0]); + const unsigned distance_count = sizeof(distance_values) / sizeof(distance_values[0]); - sonar_values[insert_index] = insert; + distance_values[insert_index] = insert; insert_index++; - if (insert_index == sonar_count) { + if (insert_index == distance_count) { insert_index = 0; } /* sort and return mode */ /* copy ring buffer */ - float sonar_temp[sonar_count]; - memcpy(sonar_temp, sonar_values, sizeof(sonar_values)); + float distance_temp[distance_count]; + memcpy(distance_temp, distance_values, sizeof(distance_values)); - sonar_bubble_sort(sonar_temp, sonar_count); + distance_bubble_sort(distance_temp, distance_count); /* the center element represents the mode after sorting */ - return sonar_temp[sonar_count / 2]; + return distance_temp[distance_count / 2]; } diff --git a/src/i2c.c b/src/i2c.c index 61e6084..fa58882 100644 --- a/src/i2c.c +++ b/src/i2c.c @@ -47,7 +47,7 @@ #include "led.h" #include "i2c_frame.h" #include "gyro.h" -#include "sonar.h" +#include "lidar.h" #include "main.h" #include "mavlink_bridge_header.h" @@ -243,15 +243,15 @@ void update_TX_buffer(float pixel_flow_x, float pixel_flow_y, f.gyro_z_rate = gyro_z_rate * getGyroScalingFactor(); f.gyro_range = getGyroRange(); - uint32_t time_since_last_sonar_update; + uint32_t time_since_last_distance_update; - time_since_last_sonar_update = (get_boot_time_us() - - get_sonar_measure_time()); + time_since_last_distance_update = (get_boot_time_us() + - get_lidar_measure_time()); - if (time_since_last_sonar_update < 255 * 1000) { - f.sonar_timestamp = time_since_last_sonar_update / 1000; //convert to ms + if (time_since_last_distance_update < 255 * 1000) { + f.distance_timestamp = time_since_last_distance_update / 1000; //convert to ms } else { - f.sonar_timestamp = 255; + f.distance_timestamp = 255; } static float accumulated_flow_x = 0; @@ -313,7 +313,7 @@ void update_TX_buffer(float pixel_flow_x, float pixel_flow_y, f_integral.pixel_flow_y_integral = accumulated_flow_y * 10.0f; //mrad*10 f_integral.integration_timespan = integration_timespan; //microseconds f_integral.ground_distance = ground_distance * 1000; //mmeters - f_integral.sonar_timestamp = time_since_last_sonar_update; //microseconds + f_integral.distance_timestamp = time_since_last_distance_update; //microseconds f_integral.qual = (uint8_t) (accumulated_quality / accumulated_framecount); //0-255 linear quality measurement 0=bad, 255=best f_integral.gyro_temperature = gyro_temp;//Temperature * 100 in centi-degrees Celsius diff --git a/src/lidar.c b/src/lidar.c new file mode 100644 index 0000000..cda8dde --- /dev/null +++ b/src/lidar.c @@ -0,0 +1,363 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "lidar.h" +#include "usbd_cdc_vcp.h" +#include "main.h" +#include "utils.h" +#include "distance_mode_filter.h" + +#include "stm32f4xx.h" +#include "stm32f4xx_i2c.h" +#include "stm32f4xx_rcc.h" +#include "stm32f4xx_gpio.h" +#include "misc.h" + +#define LIDAR_ADDRESS 0x62 +#define REG_CONTROL 0x0 +#define REG_STATUS 0x1 +//for bias calibration +#define REG_OFFSET 0x13 +#define REG_HWVER 0x41 +#define REG_STATUS2 0x47 +#define REG_SWVER 0x4f +#define REG_DATA 0x8f +//REG_DATA is 2-byte read... + +//CONTROL +#define BITS_ACQUIRE_WITH_CORRECTION 0x04 +//STATUS +#define BITS_HEALTH 0x01 +#define BITS_INVALID 0x08 +//STATUS2 +#define BITS_BUSY 0x01 + +bool i2c_started = false; + +void i2c_init_master(void) +{ + //Reset I2C + I2C_DeInit(I2C1); + I2C_SoftwareResetCmd(I2C1, ENABLE); + I2C_SoftwareResetCmd(I2C1, DISABLE); + + //Turn on clocks + RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE); + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); + + //Route I2C out to B8 (SCL) and B9 (SDA) + GPIO_InitTypeDef gpio_init; + gpio_init.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_9; + gpio_init.GPIO_Mode = GPIO_Mode_AF; + gpio_init.GPIO_Speed = GPIO_Speed_50MHz; + gpio_init.GPIO_PuPd = GPIO_PuPd_UP; + gpio_init.GPIO_OType = GPIO_OType_OD; + GPIO_Init(GPIOB, &gpio_init); + GPIO_PinAFConfig(GPIOB, GPIO_PinSource8, GPIO_AF_I2C1); //SCL + GPIO_PinAFConfig(GPIOB, GPIO_PinSource9, GPIO_AF_I2C1); //SDA + + //NVIC + NVIC_InitTypeDef nvic_init; + nvic_init.NVIC_IRQChannel = I2C1_EV_IRQn; + nvic_init.NVIC_IRQChannelPreemptionPriority = 0; + nvic_init.NVIC_IRQChannelSubPriority = 0; + nvic_init.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&nvic_init); + nvic_init.NVIC_IRQChannel = I2C1_ER_IRQn; + NVIC_Init(&nvic_init); + + //I2C block setup + I2C_InitTypeDef i2c_init; + i2c_init.I2C_ClockSpeed = 100000; + i2c_init.I2C_Mode = I2C_Mode_I2C; + i2c_init.I2C_DutyCycle = I2C_DutyCycle_2; + i2c_init.I2C_OwnAddress1 = 0x0; + i2c_init.I2C_Ack = I2C_Ack_Disable; + i2c_init.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; + I2C_Init(I2C1, &i2c_init); + I2C_Cmd(I2C1, ENABLE); + + i2c_started = true; +} + +/* Returns false on error condition... also responsible for error recovery */ +bool i2c_wait_for_event(uint32_t evt) +{ + while(!I2C_CheckEvent(I2C1, evt)) { + if(I2C_GetFlagStatus(I2C1, I2C_FLAG_AF) == SET) { + I2C_ClearFlag(I2C1, I2C_FLAG_AF); + I2C_GenerateSTOP(I2C1, ENABLE); + return false; + } + } + return true; +} + +/* Returns false on NAK */ +bool i2c_write(uint8_t address, uint8_t *data, uint8_t length) +{ + if(!i2c_started) + return false; + while(I2C_GetFlagStatus(I2C1, I2C_FLAG_BUSY)); + I2C_GenerateSTART(I2C1, ENABLE); + if(!i2c_wait_for_event(I2C_EVENT_MASTER_MODE_SELECT)) return false; + I2C_Send7bitAddress(I2C1, address << 1, I2C_Direction_Transmitter); + if(!i2c_wait_for_event(I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)) return false; + while(length > 0) { + I2C_SendData(I2C1, *data); + if(!i2c_wait_for_event(I2C_EVENT_MASTER_BYTE_TRANSMITTED)) return false; + data++; + length--; + } + I2C_GenerateSTOP(I2C1, ENABLE); + return true; +} + +/* Returns false on NAK */ +bool i2c_read(uint8_t address, uint8_t *data, uint8_t length) +{ + if(!i2c_started) + return false; + while(I2C_GetFlagStatus(I2C1, I2C_FLAG_BUSY)); + I2C_GenerateSTART(I2C1, ENABLE); + if(!i2c_wait_for_event(I2C_EVENT_MASTER_MODE_SELECT)) return false; + I2C_Send7bitAddress(I2C1, address << 1, I2C_Direction_Receiver); + if(!i2c_wait_for_event(I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED)) return false; + while(length > 0) { + if(length == 1) { + I2C_AcknowledgeConfig(I2C1, DISABLE); + I2C_GenerateSTOP(I2C1, ENABLE); + } else { + I2C_AcknowledgeConfig(I2C1, ENABLE); + } + if(!i2c_wait_for_event(I2C_EVENT_MASTER_BYTE_RECEIVED)) return false; + *data = I2C_ReceiveData(I2C1); + data++; + length--; + } + return true; +} + +/* Data from the LIDAR */ + +float sample, sample_filter; +uint8_t swver = 0, hwver = 0; +uint32_t measure_time = 0; +bool sample_valid = false; + +void lidar_config(void) +{ + uint8_t out[1] = { REG_HWVER }; + uint8_t in[1] = { 0 }; + if(i2c_write(LIDAR_ADDRESS, out, 1) && i2c_read(LIDAR_ADDRESS, in, 1)) + hwver = in[0]; + out[0] = REG_SWVER; in[0] = 0; + if(i2c_write(LIDAR_ADDRESS, out, 1) && i2c_read(LIDAR_ADDRESS, in, 1)) + swver = in[0]; + + /* Run interrupt-based drivers after config */ + I2C_ITConfig(I2C1, I2C_IT_EVT, ENABLE); + I2C_ITConfig(I2C1, I2C_IT_ERR, ENABLE); + I2C_ITConfig(I2C1, I2C_IT_BUF, ENABLE); + +} + +/* In order to minimize busywait time, everything is done with interrupt callbacks after config. + * lidar_trigger should be called periodically, and lidar_readback should be called periodically at least + * 20ms after lidar_trigger is called. + * An attempt is made to provide a clean interface (used in e.g. lidar_trigger), so that all the + * state machine nastiness is encapsulated in I2C1_EV_IRQHandler... + * Async I2C stuff eventually ends up in lidar_process... + */ + +#define IDLE 0 +#define WRITE 1 +#define READ 2 +//idle -> WRITE -> idle +//idle -> READ -> idle +uint8_t ld_state = IDLE; +uint8_t ld_reg = 0; +uint8_t ld_buffer[2] = {0}; +uint8_t ld_length = 0; +uint8_t ld_xferred = 0; +uint8_t ld_address = 0; +uint32_t ld_nextev = 0; + +void lidar_trigger(void) +{ + if(!i2c_started) + return; + ld_state = WRITE; + ld_reg = REG_CONTROL; + ld_buffer[0] = BITS_ACQUIRE_WITH_CORRECTION; + ld_length = 1; + ld_xferred = 0; + ld_address = LIDAR_ADDRESS; + ld_nextev = I2C_EVENT_MASTER_MODE_SELECT; + I2C_GenerateSTART(I2C1, ENABLE); +} + +void lidar_readback(void) +{ + if(!i2c_started) + return; + ld_state = READ; + ld_reg = REG_DATA; + ld_length = 2; + ld_xferred = 0; + ld_address = LIDAR_ADDRESS; + ld_nextev = I2C_EVENT_MASTER_MODE_SELECT; + I2C_GenerateSTART(I2C1, ENABLE); +} + +void lidar_process(void); + +void I2C1_EV_IRQHandler(void) +{ + if(ld_nextev != 0 && I2C_CheckEvent(I2C1, ld_nextev)) { + switch(ld_nextev) { + //invariant: state != IDLE + case I2C_EVENT_MASTER_MODE_SELECT: + if(ld_state == WRITE || (ld_state == READ && ld_xferred == 0)) { + I2C_Send7bitAddress(I2C1, ld_address << 1, I2C_Direction_Transmitter); + ld_nextev = I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED; + } else if(ld_state == READ) { + I2C_Send7bitAddress(I2C1, ld_address << 1, I2C_Direction_Receiver); + ld_nextev = I2C_EVENT_MASTER_BYTE_RECEIVED; + if(ld_xferred == ld_length) { // last byte + I2C_AcknowledgeConfig(I2C1, DISABLE); + I2C_GenerateSTOP(I2C1, ENABLE); + } else { + I2C_AcknowledgeConfig(I2C1, ENABLE); + } + } else { + I2C_GenerateSTOP(I2C1, ENABLE), ld_state = IDLE, ld_nextev = 0; + } + break; + //invariant: state != IDLE, ld_xferred == 0 + case I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED: + if(ld_state != IDLE && ld_xferred == 0) { + I2C_SendData(I2C1, ld_reg), ld_xferred++; + ld_nextev = I2C_EVENT_MASTER_BYTE_TRANSMITTED; + } else { + I2C_GenerateSTOP(I2C1, ENABLE), ld_state = IDLE, ld_nextev = 0; + } + break; + + //invariant: state == WRITE, 0 < ld_xferred <= length+1 + //or: state == READ, ld_xferred == 1 + case I2C_EVENT_MASTER_BYTE_TRANSMITTED: + if(ld_state == WRITE && ld_xferred == (ld_length + 1)) { + I2C_GenerateSTOP(I2C1, ENABLE); + ld_nextev = 0, ld_state = IDLE; + } else if(ld_state == WRITE && ld_xferred > 0 && ld_xferred < (ld_length + 1)) { + I2C_SendData(I2C1, ld_buffer[ld_xferred-1]); + ld_xferred++; + } else if(ld_state == READ && ld_xferred == 1) { + I2C_GenerateSTOP(I2C1, ENABLE); + I2C_GenerateSTART(I2C1, ENABLE); + ld_nextev = I2C_EVENT_MASTER_MODE_SELECT; + } else { + I2C_GenerateSTOP(I2C1, ENABLE), ld_state = IDLE, ld_nextev = 0; + } + break; + + //invariant: state == READ, 0 < ld_xferred < length + 1 + case I2C_EVENT_MASTER_BYTE_RECEIVED: + if(ld_state == READ && ld_xferred == ld_length) { //last byte + ld_buffer[ld_xferred-1] = I2C_ReceiveData(I2C1); + lidar_process(); + ld_state = IDLE, ld_nextev = 0; + } else if(ld_state == READ && ld_xferred < ld_length && ld_xferred > 0) { + ld_buffer[ld_xferred-1] = I2C_ReceiveData(I2C1); + ld_xferred++; + } else { + I2C_GenerateSTOP(I2C1, ENABLE), ld_state = IDLE, ld_nextev = 0; + } + if(ld_nextev == I2C_EVENT_MASTER_BYTE_RECEIVED) { + if(ld_xferred == ld_length) { // last byte + I2C_AcknowledgeConfig(I2C1, DISABLE); + I2C_GenerateSTOP(I2C1, ENABLE); + } else { + I2C_AcknowledgeConfig(I2C1, ENABLE); + } + } + break; + default: + break; + } + } +} + +void I2C1_ER_IQRHandler(void) { + if((I2C_ReadRegister(I2C1, I2C_Register_SR1) & 0xFF00) != 0x00) { + I2C_GenerateSTOP(I2C1, ENABLE); + I2C1->SR1 &= 0x00FF; + ld_state = IDLE; + } +} + +void lidar_process(void) +{ + if(ld_reg == REG_DATA) { + int16_t sample16 = (((int16_t)ld_buffer[1]) << 8) | ld_buffer[0]; + if(sample16 <= 0) { // 0 or less means invalid data + sample_valid = false; + return; + } + sample16 = ((sample16 >> 8) & 0xFF) | ((sample16 << 8) & 0xFF00); + sample = (sample16 / 100.0f); /* convert cm->m */ + if(sample < MINIMUM_DISTANCE || sample > MAXIMUM_DISTANCE) { + sample_valid = false; + return; + } + sample_filter = insert_distance_value_and_get_mode_value(sample); + measure_time = get_boot_time_us(); + sample_valid = true; + } +} + +bool lidar_read(float* distance_filtered, float* distance_raw) +{ + if(sample_valid) { + *distance_raw = sample; + *distance_filtered = sample_filter; + return true; + } + return false; +} + +uint32_t get_lidar_measure_time(void) +{ + return measure_time; +} diff --git a/src/main.c b/src/main.c index a3fe07e..908c86f 100644 --- a/src/main.c +++ b/src/main.c @@ -54,7 +54,7 @@ #include "gyro.h" #include "i2c.h" #include "usart.h" -#include "sonar.h" +#include "lidar.h" #include "communication.h" #include "debug.h" #include "usbd_cdc_core.h" @@ -90,7 +90,7 @@ volatile uint32_t boot_time10_us = 0; #define TIMER_CIN 0 #define TIMER_LED 1 #define TIMER_DELAY 2 -#define TIMER_SONAR 3 +#define TIMER_DISTANCE 3 #define TIMER_SYSTEM_STATE 4 #define TIMER_RECEIVE 5 #define TIMER_PARAMS 6 @@ -98,7 +98,8 @@ volatile uint32_t boot_time10_us = 0; #define TIMER_LPOS 8 #define MS_TIMER_COUNT 100 /* steps in 10 microseconds ticks */ #define LED_TIMER_COUNT 500 /* steps in milliseconds ticks */ -#define SONAR_TIMER_COUNT 100 /* steps in milliseconds ticks */ +#define DISTANCE_TIMER_COUNT 100 /* steps in milliseconds ticks */ +#define DISTANCE_TIMER_HALF 50 #define SYSTEM_STATE_COUNT 1000/* steps in milliseconds ticks */ #define PARAMS_COUNT 100 /* steps in milliseconds ticks */ #define LPOS_TIMER_COUNT 100 /* steps in milliseconds ticks */ @@ -145,10 +146,14 @@ void timer_update_ms(void) timer[TIMER_LED] = LED_TIMER_COUNT; } - if (timer[TIMER_SONAR] == 0) + if (timer[TIMER_DISTANCE] == 0) { - sonar_trigger(); - timer[TIMER_SONAR] = SONAR_TIMER_COUNT; + lidar_readback(); + timer[TIMER_DISTANCE] = DISTANCE_TIMER_COUNT; + } + else if(timer[TIMER_DISTANCE] == DISTANCE_TIMER_HALF) + { + lidar_trigger(); } if (timer[TIMER_SYSTEM_STATE] == 0) @@ -280,17 +285,15 @@ int main(void) /* usart config*/ usart_init(); - /* i2c config*/ - i2c_init(); - - /* sonar config*/ - float sonar_distance_filtered = 0.0f; // distance in meter - float sonar_distance_raw = 0.0f; // distance in meter + /* i2c config*/ + i2c_init_master(); + lidar_config(); + float distance_filtered = 0.0f; // distance in meter + float distance_raw = 0.0f; // distance in meter bool distance_valid = false; - sonar_config(); /* reset/start timers */ - timer[TIMER_SONAR] = SONAR_TIMER_COUNT; + timer[TIMER_DISTANCE] = DISTANCE_TIMER_COUNT; timer[TIMER_SYSTEM_STATE] = SYSTEM_STATE_COUNT; timer[TIMER_RECEIVE] = SYSTEM_STATE_COUNT / 2; timer[TIMER_PARAMS] = PARAMS_COUNT; @@ -321,7 +324,7 @@ int main(void) static uint16_t accumulated_quality = 0; static uint32_t integration_timespan = 0; static uint32_t lasttime = 0; - uint32_t time_since_last_sonar_update= 0; + uint32_t time_since_last_distance_update= 0; /* main loop */ @@ -389,13 +392,13 @@ int main(void) /* 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 - /* get sonar data */ - distance_valid = sonar_read(&sonar_distance_filtered, &sonar_distance_raw); + /* get distance data */ + distance_valid = lidar_read(&distance_filtered, &distance_raw); /* reset to zero for invalid distances */ if (!distance_valid) { - sonar_distance_filtered = 0.0f; - sonar_distance_raw = 0.0f; + distance_filtered = 0.0f; + distance_raw = 0.0f; } /* compute optical flow */ @@ -419,10 +422,10 @@ int main(void) 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; + float new_velocity_x = - flow_compx * distance_filtered; + float new_velocity_y = - flow_compy * distance_filtered; - time_since_last_sonar_update = (get_boot_time_us()- get_sonar_measure_time()); + time_since_last_distance_update = (get_boot_time_us()- get_lidar_measure_time()); if (qual > 0) { @@ -479,23 +482,23 @@ int main(void) if(global_data.param[PARAM_SONAR_FILTERED]) { - ground_distance = sonar_distance_filtered; + ground_distance = distance_filtered; } else { - ground_distance = sonar_distance_raw; + ground_distance = distance_raw; } //update I2C transmitbuffer if(valid_frame_count>0) { - update_TX_buffer(pixel_flow_x, pixel_flow_y, velocity_x_sum/valid_frame_count, velocity_y_sum/valid_frame_count, qual, - ground_distance, x_rate, y_rate, z_rate, gyro_temp); + //update_TX_buffer(pixel_flow_x, pixel_flow_y, velocity_x_sum/valid_frame_count, velocity_y_sum/valid_frame_count, qual, + // ground_distance, x_rate, y_rate, z_rate, gyro_temp); } else { - update_TX_buffer(pixel_flow_x, pixel_flow_y, 0.0f, 0.0f, qual, - ground_distance, x_rate, y_rate, z_rate, gyro_temp); + //update_TX_buffer(pixel_flow_x, pixel_flow_y, 0.0f, 0.0f, qual, + // ground_distance, x_rate, y_rate, z_rate, gyro_temp); } //serial mavlink + usb mavlink output throttled @@ -534,7 +537,7 @@ int main(void) integration_timespan, accumulated_flow_x, accumulated_flow_y, accumulated_gyro_x, accumulated_gyro_y, accumulated_gyro_z, gyro_temp, accumulated_quality/accumulated_framecount, - time_since_last_sonar_update,ground_distance); + time_since_last_distance_update,ground_distance); /* send approximate local position estimate without heading */ if (global_data.param[PARAM_SYSTEM_SEND_LPOS]) @@ -569,7 +572,7 @@ int main(void) integration_timespan, accumulated_flow_x, accumulated_flow_y, accumulated_gyro_x, accumulated_gyro_y, accumulated_gyro_z, gyro_temp, accumulated_quality/accumulated_framecount, - time_since_last_sonar_update,ground_distance); + time_since_last_distance_update,ground_distance); } diff --git a/src/sonar.c b/src/sonar.c index e49e868..dc62bbe 100644 --- a/src/sonar.c +++ b/src/sonar.c @@ -48,7 +48,7 @@ #include "usart.h" #include "settings.h" #include "sonar.h" -#include "sonar_mode_filter.h" +#include "distance_mode_filter.h" #define SONAR_SCALE 1000.0f #define SONAR_MIN 0.12f /** 0.12m sonar minimum distance */ @@ -134,7 +134,7 @@ void UART4_IRQHandler(void) dt = ((float)(measure_time - last_measure_time)) / 1000000.0f; valid_data = temp; - sonar_mode = insert_sonar_value_and_get_mode_value(valid_data / SONAR_SCALE); + sonar_mode = insert_distance_value_and_get_mode_value(valid_data / SONAR_SCALE); new_value = 1; sonar_valid = true; } else { diff --git a/unittests/Makefile b/unittests/Makefile index d33b015..054fe81 100644 --- a/unittests/Makefile +++ b/unittests/Makefile @@ -4,7 +4,7 @@ CFLAGS=-I. -I../src -lm all: tests -TEST_FILES=../src/sonar_mode_filter.c \ +TEST_FILES=../src/distance_mode_filter.c \ tests.cpp tests: $(TEST_FILES) diff --git a/unittests/tests.c b/unittests/tests.c index 4dd7135..2edeb95 100644 --- a/unittests/tests.c +++ b/unittests/tests.c @@ -4,7 +4,7 @@ #include -#include "../inc/sonar_mode_filter.h" +#include "../inc/distance_mode_filter.h" int main(int argc, char *argv[]) { @@ -18,7 +18,7 @@ int main(int argc, char *argv[]) { float inf = in / 10.0f; - float out = insert_sonar_value_and_get_mode_value(inf); + float out = insert_distance_value_and_get_mode_value(inf); printf("in: %f\tout: %f\n", (double)inf, (double)out); } From 1a1a1d3247c48c8d045acd4fefde963890df9682 Mon Sep 17 00:00:00 2001 From: Tom Date: Wed, 29 Jul 2015 12:58:57 -0700 Subject: [PATCH 036/120] Up distance sample rate to 50Hz, 5-sample median filter --- src/distance_mode_filter.c | 4 ++-- src/main.c | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/distance_mode_filter.c b/src/distance_mode_filter.c index a6ca913..a2b7cf9 100644 --- a/src/distance_mode_filter.c +++ b/src/distance_mode_filter.c @@ -37,9 +37,9 @@ /** * insert-only ring buffer of distance data, needs to be of uneven size * the initialization to zero will make the filter respond zero for the - * first three inserted readinds, which is a decent startup-logic. + * first N inserted readinds, which is a decent startup-logic. */ -static float distance_values[3] = { 0.0f }; +static float distance_values[5] = { 0.0f }; static unsigned insert_index = 0; static void distance_bubble_sort(float distance_values[], unsigned n); diff --git a/src/main.c b/src/main.c index 908c86f..87f2401 100644 --- a/src/main.c +++ b/src/main.c @@ -98,8 +98,8 @@ volatile uint32_t boot_time10_us = 0; #define TIMER_LPOS 8 #define MS_TIMER_COUNT 100 /* steps in 10 microseconds ticks */ #define LED_TIMER_COUNT 500 /* steps in milliseconds ticks */ -#define DISTANCE_TIMER_COUNT 100 /* steps in milliseconds ticks */ -#define DISTANCE_TIMER_HALF 50 +#define DISTANCE_TIMER_COUNT 20 /* steps in milliseconds ticks */ +#define DISTANCE_TIMER_TRIGGER 18 #define SYSTEM_STATE_COUNT 1000/* steps in milliseconds ticks */ #define PARAMS_COUNT 100 /* steps in milliseconds ticks */ #define LPOS_TIMER_COUNT 100 /* steps in milliseconds ticks */ @@ -151,7 +151,7 @@ void timer_update_ms(void) lidar_readback(); timer[TIMER_DISTANCE] = DISTANCE_TIMER_COUNT; } - else if(timer[TIMER_DISTANCE] == DISTANCE_TIMER_HALF) + else if(timer[TIMER_DISTANCE] == DISTANCE_TIMER_TRIGGER) { lidar_trigger(); } From d3509fa6306828da82cc2d094d099d97225367ed Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Tue, 4 Aug 2015 13:49:39 +0200 Subject: [PATCH 037/120] initial prototype of the new camera driver. --- inc/camera.h | 150 +++++++++++++++++++++++++++++++++++++++++++++++++++ src/camera.c | 3 ++ 2 files changed, 153 insertions(+) create mode 100644 inc/camera.h create mode 100644 src/camera.c diff --git a/inc/camera.h b/inc/camera.h new file mode 100644 index 0000000..e688bfd --- /dev/null +++ b/inc/camera.h @@ -0,0 +1,150 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef CAMERA_H_ +#define CAMERA_H_ + +#include + +#define CAMERA_MAX_BUFFER_COUNT 5 + +struct _camera_sensor; +typedef struct _camera_sensor camera_sensor; + +struct _camera_transport; +typedef struct _camera_transport camera_transport; + +struct _camera_ctx; +typedef struct _camera_ctx camera_ctx; + +/** + * Struct holding image parameters for the camera. + */ +typedef struct _camera_img_param { + struct _size { + uint16_t x; ///< Image size in x direction. + uint16_t y; ///< Image size in y direction. + } size; ///< Image size. + uint8_t col_bin; ///< Column binning ratio. (x direction) + uint8_t row_bin; ///< Row binning ration. (y direction) +} camera_img_param; + +/** + * Struct holding information about an image and a pointer to the buffer itself. + */ +typedef struct _camera_image_buffer { + camera_img_param param; ///< The parameters of the image that is stored in the buffer. + uint32_t frame_number; ///< The frame number. This number increases with every frame that is captured from the camera module. + uint32_t timestamp; ///< Frame timestamp in microseconds. + void *buffer; ///< Pointer to the buffer itself. + size_t buffer_size; ///< Maximum size of the buffer. Note: this is not the actual size of the image in the buffer. + uint32_t meta; ///< Buffer meta data not used by the camera driver. This can be used to identify buffers. +} camera_image_buffer; + +/** + * Callback which is called when a snapshot capture has finished. + * @note This callback may be called from an interrupt handler. + * @param Pointer to buffer which contains the snapshot. + */ +typedef void (*camera_snapshot_done_cb)(camera_image_buffer *buf); + +/** + * Initializes the camera driver. + * @param ctx The context to use. + * @param sensor The sensor interface to use. + * @param transport The sensor data transport interface to use. + * @param buffers Array of initialized buffers to use for buffering the images in img_stream mode. + * In each camera_image_buffer the buffer and buffer_size members must be correctly set. + * The camera_img_stream_get_buffers function will return pointers to one of these buffers. + * The buffer can reside in the CCM of the microcontroller. + * @param buffer_count Number of buffers that are passed in buffers. + * Must be lower than or equal the configured CAMERA_MAX_BUFFER_COUNT. + * There must be at least one more buffer than what is requested in camera_img_stream_get_buffers. + * More buffers will reduce the latency when frames are skipped. + * @return Zero when successful. + */ +int camera_init(camera_ctx *ctx, camera_sensor *sensor, camera_transport *transport, + camera_image_buffer buffers[], size_t buffer_count); + +/** + * Schedules the new parameters to take effect as soon as possible. This function makes sure that + * parameter changes are commited to the sensor in a time window which will guarantee glitch free operation. + * @param ctx The context to use. + * @param img_param The new image parameters to use. Note that the image must still fit inside the buffers passed + * to camera_init. + * @return Zero on success. + */ +int camera_img_stream_schedule_param_change(camera_ctx *ctx, const camera_img_param *img_param); + +/** + * Gets the most recent images. If no more recent image is available this function returns immediatly without doing anything. + * @param ctx The context to use. + * @param buffers Pointer to an array of buffer pointers which are updated to point to the most recent buffers. + * The first buffer pointer will be updated to the most recent image followed by the next + * pointer which is updated to the next older image and so on. + * It is guaranteed to retrieve consecutive images. + * @param count Number of most recent images to retrieve. Must be lower than buffer_count - 1 which was + * passed to camera_init. + * @return 0 when a new most recent image has been retrieved. + * 1 if there was no new most recent image since the last call to this function. (In this case nothing has been updated) + * -1 on error. + * @note When this function is successful the buffers need to be returned to the camera driver before + * requesting new buffers. + */ +int camera_img_stream_get_buffers(camera_ctx *ctx, camera_image_buffer **buffers[], size_t count); + +/** + * Returns the buffers that have been retrieved by camera_img_stream_get_buffers back to the camera driver. + * This function must be called when image processing has been finished. + * @param ctx The context to use. + * @param buffers Pointer to an array of buffer pointers which will be returned to the camera driver. + * @param count Number of buffer pointers in the array. + */ +void camera_img_stream_return_buffers(camera_ctx *ctx, camera_image_buffer **buffers[], size_t count); + +/** + * Schedules a snapshot to be taken with the camera. The snapshot will be scheduled as soon as possible. + * Only one snapshot can be scheduled at a time. + * @param ctx The context to use. + * @param img_param The image parameters for the snapshot image. + * @param dst The destination buffer which should receive the image. + * The buffer and buffer_size members must be correctly set. + * @param cb Callback function which is called when the snapshot has been taken. + * Note that the callback is called from the interrupt context and should only do minimal stuff + * like setting a flag to notify the main loop. Calling camera_* functions is not allowed. + * @return Zero when snapshot has been successfully scheduled. + */ +int camera_snapshot_schedule(camera_ctx *ctx, const camera_img_param *img_param, camera_image_buffer *dst, camera_snapshot_done_cb cb); + + +#endif /* CAMERA_H_ */ \ No newline at end of file diff --git a/src/camera.c b/src/camera.c new file mode 100644 index 0000000..72e817b --- /dev/null +++ b/src/camera.c @@ -0,0 +1,3 @@ +#include "camera.h" + + From ef4eff26ecb543580e1a44a0e55c6de964d6e0b6 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Tue, 4 Aug 2015 13:50:16 +0200 Subject: [PATCH 038/120] fixed comments about the system clocks to match the actual system clocks that are used. --- src/system_stm32f4xx.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/system_stm32f4xx.c b/src/system_stm32f4xx.c index 9a937a4..224843e 100644 --- a/src/system_stm32f4xx.c +++ b/src/system_stm32f4xx.c @@ -46,9 +46,9 @@ *----------------------------------------------------------------------------- * System Clock source | PLL (HSE) *----------------------------------------------------------------------------- - * SYSCLK(Hz) | 168000000 + * SYSCLK(Hz) | 192000000 *----------------------------------------------------------------------------- - * HCLK(Hz) | 168000000 + * HCLK(Hz) | 192000000 *----------------------------------------------------------------------------- * AHB Prescaler | 1 *----------------------------------------------------------------------------- @@ -60,11 +60,11 @@ *----------------------------------------------------------------------------- * PLL_M | 24 *----------------------------------------------------------------------------- - * PLL_N | 336 + * PLL_N | 384 *----------------------------------------------------------------------------- * PLL_P | 2 *----------------------------------------------------------------------------- - * PLL_Q | 7 + * PLL_Q | 8 *----------------------------------------------------------------------------- * PLLI2S_N | NA *----------------------------------------------------------------------------- From ce49c1dfe02c317520d2e7ded068a2508b686e76 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Tue, 4 Aug 2015 15:36:03 +0200 Subject: [PATCH 039/120] updates to the documentation, added missing struct definitions. --- CMakeLists.txt | 2 ++ Makefile | 1 + inc/camera.h | 88 ++++++++++++++++++++++++++++++++++++++++++++++---- src/camera.c | 55 +++++++++++++++++++++++++++++++ 4 files changed, 140 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7864648..707baf2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -181,6 +181,7 @@ set(PERIPH_SRC ) set(PX4FLOW_HDRS + inc/camera.h inc/communication.h inc/dcmi.h inc/debug.h @@ -209,6 +210,7 @@ set(PX4FLOW_HDRS ) set(PX4FLOW_SRC + inc/camera.c src/communication.c src/dcmi.c src/debug.c diff --git a/Makefile b/Makefile index 32de070..05e057b 100644 --- a/Makefile +++ b/Makefile @@ -23,6 +23,7 @@ SRCS = src/main.c \ src/utils.c \ src/led.c \ src/settings.c \ + src/camera.c \ src/communication.c \ src/filter.c \ src/flow.c \ diff --git a/inc/camera.h b/inc/camera.h index e688bfd..38cf963 100644 --- a/inc/camera.h +++ b/inc/camera.h @@ -1,6 +1,7 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Simon Laube * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -38,11 +39,11 @@ #define CAMERA_MAX_BUFFER_COUNT 5 -struct _camera_sensor; -typedef struct _camera_sensor camera_sensor; +struct _camera_sensor_interface; +typedef struct _camera_sensor_interface camera_sensor_interface; -struct _camera_transport; -typedef struct _camera_transport camera_transport; +struct _camera_transport_interface; +typedef struct _camera_transport_interface camera_transport_interface; struct _camera_ctx; typedef struct _camera_ctx camera_ctx; @@ -54,7 +55,10 @@ typedef struct _camera_img_param { struct _size { uint16_t x; ///< Image size in x direction. uint16_t y; ///< Image size in y direction. - } size; ///< Image size. + } size; /**< Image size. + * The image size is constrained by the transfer size of the camera_transport_interface. + * The image size in bytes must be an exact multiple of the transfer size + * as well as at least two times the transfer size. */ uint8_t col_bin; ///< Column binning ratio. (x direction) uint8_t row_bin; ///< Row binning ration. (y direction) } camera_img_param; @@ -83,6 +87,7 @@ typedef void (*camera_snapshot_done_cb)(camera_image_buffer *buf); * @param ctx The context to use. * @param sensor The sensor interface to use. * @param transport The sensor data transport interface to use. + * @param img_param The initial image parameters to use for img_stream operation mode. * @param buffers Array of initialized buffers to use for buffering the images in img_stream mode. * In each camera_image_buffer the buffer and buffer_size members must be correctly set. * The camera_img_stream_get_buffers function will return pointers to one of these buffers. @@ -93,7 +98,8 @@ typedef void (*camera_snapshot_done_cb)(camera_image_buffer *buf); * More buffers will reduce the latency when frames are skipped. * @return Zero when successful. */ -int camera_init(camera_ctx *ctx, camera_sensor *sensor, camera_transport *transport, +int camera_init(camera_ctx *ctx, camera_sensor_interface *sensor, camera_transport_interface *transport, + const camera_img_param *img_param, camera_image_buffer buffers[], size_t buffer_count); /** @@ -146,5 +152,75 @@ void camera_img_stream_return_buffers(camera_ctx *ctx, camera_image_buffer **buf */ int camera_snapshot_schedule(camera_ctx *ctx, const camera_img_param *img_param, camera_image_buffer *dst, camera_snapshot_done_cb cb); +/** + * The camera driver context struct. + */ +struct _camera_ctx { + camera_sensor_interface *sensor; + camera_transport_interface *transport; +}; + +/** Camera sensor configuration interface. + */ +struct _camera_sensor_interface { + void *usr; ///< User pointer that should be passed to the following interface functions. + /** + * Initializes the sensor and the hardware of the microcontroller. + * @param usr User pointer from this struct. + * @param img_param The image parameters to use for initialization. + * @return 0 on success. + */ + int (*init)(void *usr, const camera_img_param *img_param); + /** + * Prepares the sensor to switch to new parameters. + * This function should perform most of the work that is needed to update the sensor with new parameters. + * @param usr User pointer from this struct. + * @param img_param The new image parameters. + * @return 0 on success. + */ + int (*prepare_update_param)(void *usr, const camera_img_param *img_param); + /** + * Switches the sensor to the new parameters that have been previously prepared. + * This function is called just after the camera module has started outputting a new frame. + * @param usr User pointer from this struct. + * @return >= 0 on success: the number of frames until the changes take effect. + * < 0 on error. + * @note This function may be called from an interrupt vector and should do as little work as necessary. + */ + int (*update_param)(void *usr); +}; + +/** + * Callback for notifying the camera driver about a completed transfer. + * @param usr The user data pointer that has been specified in the init function. (cb_usr) + * @param buffer The buffer that contains the data of the transfer was completed. + * @param size The size of the transfer. + */ +typedef void (*camera_transport_transfer_done_cb)(void *usr, void *buffer, size_t size); + +/** + * Callback for notifying the camera driver about a completed frame. + * @param usr The user data pointer that has been specified in the init function. (cb_usr) + */ +typedef void (*camera_transport_frame_done_cb)(void *usr); + +/** Camera image transport interface. + */ +struct _camera_transport_interface { + void *usr; ///< User pointer that should be passed to the following interface functions. + /** + * Initializes the sensor and the hardware of the microcontroller. + * @param usr User pointer from this struct. + * @param transfer_done_cb Callback which should be called when a transfer has been completed. + * @param frame_done_cb Callback which should be called when a frame has been completed. + * @param cb_usr Callback user data pointer which should be passed to the transfer_done_cb + * and frame_done_cb callbacks. + * @return 0 on success. + */ + int (*init)(void *usr, + camera_transport_transfer_done_cb transfer_done_cb, + camera_transport_frame_done_cb frame_done_cb, + void *cb_usr); +}; #endif /* CAMERA_H_ */ \ No newline at end of file diff --git a/src/camera.c b/src/camera.c index 72e817b..9f0cf12 100644 --- a/src/camera.c +++ b/src/camera.c @@ -1,3 +1,58 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Simon Laube + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + #include "camera.h" +int camera_init(camera_ctx *ctx, camera_sensor_interface *sensor, camera_transport_interface *transport, + const camera_img_param *img_param, + camera_image_buffer buffers[], size_t buffer_count) { + +} + +int camera_img_stream_schedule_param_change(camera_ctx *ctx, const camera_img_param *img_param) { + +} + +int camera_img_stream_get_buffers(camera_ctx *ctx, camera_image_buffer **buffers[], size_t count) { + +} + +void camera_img_stream_return_buffers(camera_ctx *ctx, camera_image_buffer **buffers[], size_t count) { + +} + +int camera_snapshot_schedule(camera_ctx *ctx, const camera_img_param *img_param, camera_image_buffer *dst, camera_snapshot_done_cb cb) { + +} From 678ca42bfc0115e2824a6e6f67c9fb902f5c62f3 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Tue, 4 Aug 2015 17:13:47 +0200 Subject: [PATCH 040/120] changes to the camera sensor and transport interfaces. started implementation of init function. --- inc/camera.h | 31 ++++++++++++++++++------------- src/camera.c | 24 ++++++++++++++++++++++++ 2 files changed, 42 insertions(+), 13 deletions(-) diff --git a/inc/camera.h b/inc/camera.h index 38cf963..925f903 100644 --- a/inc/camera.h +++ b/inc/camera.h @@ -92,6 +92,7 @@ typedef void (*camera_snapshot_done_cb)(camera_image_buffer *buf); * In each camera_image_buffer the buffer and buffer_size members must be correctly set. * The camera_img_stream_get_buffers function will return pointers to one of these buffers. * The buffer can reside in the CCM of the microcontroller. + * The structs are copied to an internal data-structure. * @param buffer_count Number of buffers that are passed in buffers. * Must be lower than or equal the configured CAMERA_MAX_BUFFER_COUNT. * There must be at least one more buffer than what is requested in camera_img_stream_get_buffers. @@ -152,14 +153,6 @@ void camera_img_stream_return_buffers(camera_ctx *ctx, camera_image_buffer **buf */ int camera_snapshot_schedule(camera_ctx *ctx, const camera_img_param *img_param, camera_image_buffer *dst, camera_snapshot_done_cb cb); -/** - * The camera driver context struct. - */ -struct _camera_ctx { - camera_sensor_interface *sensor; - camera_transport_interface *transport; -}; - /** Camera sensor configuration interface. */ struct _camera_sensor_interface { @@ -180,14 +173,18 @@ struct _camera_sensor_interface { */ int (*prepare_update_param)(void *usr, const camera_img_param *img_param); /** - * Switches the sensor to the new parameters that have been previously prepared. - * This function is called just after the camera module has started outputting a new frame. + * Called every frame just after readout has started (but not completed yet). + * This function may be used to switch the sensor to new parameters that have been previously prepared. * @param usr User pointer from this struct. - * @return >= 0 on success: the number of frames until the changes take effect. - * < 0 on error. * @note This function may be called from an interrupt vector and should do as little work as necessary. */ - int (*update_param)(void *usr); + void (*notify_readout_start)(void *usr); + /** + * Called to retrieve the image parameters of the current frame that is being output. + * This function is called after notify_readout_start to retrieve the + * parameters that were in effect at the time the image was taken. + */ + void (*get_current_param)(void *usr, camera_img_param *img_param); }; /** @@ -223,4 +220,12 @@ struct _camera_transport_interface { void *cb_usr); }; +/** + * The camera driver context struct. + */ +struct _camera_ctx { + camera_sensor_interface *sensor; + camera_transport_interface *transport; +}; + #endif /* CAMERA_H_ */ \ No newline at end of file diff --git a/src/camera.c b/src/camera.c index 9f0cf12..50235db 100644 --- a/src/camera.c +++ b/src/camera.c @@ -34,10 +34,25 @@ #include "camera.h" +#include + +void camera_transport_transfer_done_fn(void *usr, void *buffer, size_t size); +void camera_transport_frame_done_fn(void *usr); + int camera_init(camera_ctx *ctx, camera_sensor_interface *sensor, camera_transport_interface *transport, const camera_img_param *img_param, camera_image_buffer buffers[], size_t buffer_count) { + memset(ctx, 0, sizeof(camera_ctx)); + ctx->sensor = sensor; + ctx->transport = transport; + // initialize state: + // initialize hardware: + ctx->transport->init(ctx->transport->usr, + camera_transport_transfer_done_fn, + camera_transport_frame_done_fn, + ctx); + ctx->sensor->init(ctx->sensor->usr, img_param); } int camera_img_stream_schedule_param_change(camera_ctx *ctx, const camera_img_param *img_param) { @@ -56,3 +71,12 @@ int camera_snapshot_schedule(camera_ctx *ctx, const camera_img_param *img_param, } + + +void camera_transport_transfer_done_fn(void *usr, void *buffer, size_t size) { + +} + +void camera_transport_frame_done_fn(void *usr) { + +} From c42f6dff23a2d426b7c7048bd3ad69c7296524bc Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Tue, 4 Aug 2015 23:09:24 +0200 Subject: [PATCH 041/120] WIP. start to move code around into new functions that do the initialisation. --- inc/camera.h | 11 +- inc/mt9v034.h | 14 ++ src/dcmi.c | 46 ----- src/mt9v034.c | 451 ++++++++++++++++++++++++++++++++------------------ 4 files changed, 309 insertions(+), 213 deletions(-) diff --git a/inc/camera.h b/inc/camera.h index 925f903..7a8557e 100644 --- a/inc/camera.h +++ b/inc/camera.h @@ -59,8 +59,7 @@ typedef struct _camera_img_param { * The image size is constrained by the transfer size of the camera_transport_interface. * The image size in bytes must be an exact multiple of the transfer size * as well as at least two times the transfer size. */ - uint8_t col_bin; ///< Column binning ratio. (x direction) - uint8_t row_bin; ///< Row binning ration. (y direction) + uint8_t binning; ///< Column and row binning ratio. } camera_img_param; /** @@ -99,7 +98,7 @@ typedef void (*camera_snapshot_done_cb)(camera_image_buffer *buf); * More buffers will reduce the latency when frames are skipped. * @return Zero when successful. */ -int camera_init(camera_ctx *ctx, camera_sensor_interface *sensor, camera_transport_interface *transport, +int camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const camera_transport_interface *transport, const camera_img_param *img_param, camera_image_buffer buffers[], size_t buffer_count); @@ -205,6 +204,7 @@ typedef void (*camera_transport_frame_done_cb)(void *usr); */ struct _camera_transport_interface { void *usr; ///< User pointer that should be passed to the following interface functions. + size_t transfer_size; ///< Transfer size of this transport. /** * Initializes the sensor and the hardware of the microcontroller. * @param usr User pointer from this struct. @@ -224,8 +224,9 @@ struct _camera_transport_interface { * The camera driver context struct. */ struct _camera_ctx { - camera_sensor_interface *sensor; - camera_transport_interface *transport; + const camera_sensor_interface *sensor; + const camera_transport_interface *transport; + }; #endif /* CAMERA_H_ */ \ No newline at end of file diff --git a/inc/mt9v034.h b/inc/mt9v034.h index c646cd7..0a280b8 100644 --- a/inc/mt9v034.h +++ b/inc/mt9v034.h @@ -36,6 +36,20 @@ #include #include "settings.h" +#include "camera.h" + +/** + * Configures the maximum exposure time in number of image rows. + * Exposure should not affect frame time. + * Set to number of rows in the image. + */ +#define CONFIG_MAX_EXPOSURE_ROWS (64) +/** + * Configures the maximum analog gain. + * Range: 1x - 4x => 16 - 64. + * Higher gain means more noise. + */ +#define CONFIG_MAX_ANALOG_GAIN (64) /* Constants */ #define TIMEOUT_MAX 10000 diff --git a/src/dcmi.c b/src/dcmi.c index 8c69464..95337dd 100644 --- a/src/dcmi.c +++ b/src/dcmi.c @@ -478,9 +478,6 @@ void dcmi_hw_init(void) dcmi_image_buffer_8bit_3 [i] = 0; } - GPIO_InitTypeDef GPIO_InitStructure; - I2C_InitTypeDef I2C_InitStruct; - /*** Configures the DCMI GPIOs to interface with the OV2640 camera module ***/ /* Enable DCMI GPIOs clocks */ RCC_AHB1PeriphClockCmd( @@ -524,49 +521,6 @@ void dcmi_hw_init(void) | GPIO_Pin_5 | GPIO_Pin_6; GPIO_Init(GPIOE, &GPIO_InitStructure); - /* I2C2 clock enable */ - RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE); - - /* GPIOB clock enable */ - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); - - /* Connect I2C2 pins to AF4 */ - GPIO_PinAFConfig(GPIOB, GPIO_PinSource10, GPIO_AF_I2C2); - GPIO_PinAFConfig(GPIOB, GPIO_PinSource11, GPIO_AF_I2C2); - - /* Configure I2C2 GPIOs */ - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_11; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; - GPIO_InitStructure.GPIO_OType = GPIO_OType_OD; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; - GPIO_Init(GPIOB, &GPIO_InitStructure); - - /* I2C DeInit */ - I2C_DeInit(I2C2); - - /* Enable the I2C peripheral */ - I2C_Cmd(I2C2, ENABLE); - - /* Set the I2C structure parameters */ - I2C_InitStruct.I2C_Mode = I2C_Mode_I2C; - I2C_InitStruct.I2C_DutyCycle = I2C_DutyCycle_2; - I2C_InitStruct.I2C_OwnAddress1 = 0xFE; - I2C_InitStruct.I2C_Ack = I2C_Ack_Enable; - I2C_InitStruct.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; - I2C_InitStruct.I2C_ClockSpeed = 100000; - - /* Initialize the I2C peripheral w/ selected parameters */ - I2C_Init(I2C2, &I2C_InitStruct); - - /* Initialize GPIOs for EXPOSURE and STANDBY lines of the camera */ - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; - GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; - GPIO_Init(GPIOA, &GPIO_InitStructure); - GPIO_ResetBits(GPIOA, GPIO_Pin_2 | GPIO_Pin_3); } /** diff --git a/src/mt9v034.c b/src/mt9v034.c index 472fc22..59ebd16 100644 --- a/src/mt9v034.c +++ b/src/mt9v034.c @@ -5,6 +5,7 @@ * Dominik Honegger * Petri Tanskanen * Samuel Zihlmann + * Simon Laube * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,43 +36,257 @@ * ****************************************************************************/ -#include "stm32f4xx_dcmi.h" -#include "stm32f4xx_dma.h" #include "stm32f4xx_rcc.h" #include "stm32f4xx_i2c.h" +#include "stm32f4xx_gpio.h" #include "mt9v034.h" -/** - * @brief Configures the mt9v034 camera with two context (binning 4 and binning 2). - */ -void mt9v034_context_configuration(void) -{ - /* Chip Control - * - * bits | 15 | ... | 9 | 8 | 7 | 6 | 5 | 4 | 3 | 2 | 1 | 0 | - * ------------------------------------------------------------------- - * current values | 0 | ... | 0 | 1 | 1 | 0 | 0 | 0 | 1 | 0 | 0 | 0 | - * - * (0:2) Scan Mode (Progressive scan) - * (3:4) Sensor Operation Mode (Master mode) - * (5) Stereoscopy Mode (Disable) - * (6) Stereoscopic Master/Slave mode (not used) - * (7) Parallel Output Enable (Enable) - * (8) Simultaneous/Sequential Mode (Simultaneous mode) - * (9) Reserved - * - * (15)Context A (0) / Context B (1) - * - */ +#include + +typedef struct _mt9v034_sensor_ctx { + int cur_context; + camera_img_param cur_param; + camera_img_param exp_param; + camera_img_param context_a; + camera_img_param context_b; +} mt9v034_sensor_ctx; + +int mt9v034_init(void *usr, const camera_img_param *img_param); +int mt9v034_prepare_update_param(void *usr, const camera_img_param *img_param); +void mt9v034_notify_readout_start(void *usr); +void mt9v034_get_current_param(void *usr, camera_img_param *img_param); + +static int mt9v034_init_hw(mt9v034_sensor_ctx *ctx); +static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx); +static void mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, const camera_img_param *img_param); + +static mt9v034_sensor_ctx mt9v034_ctx; + +const camera_sensor_interface mt9v034_sensor_interface = { + .usr = &mt9v034_ctx, + .init = mt9v034_init, + .prepare_update_param = mt9v034_prepare_update_param, + .notify_readout_start = mt9v034_notify_readout_start, + .get_current_param = mt9v034_get_current_param +}; + + +int mt9v034_init(void *usr, const camera_img_param *img_param) { + mt9v034_sensor_ctx *ctx = (mt9v034_sensor_ctx *)usr; + memset(ctx, 0, sizeof(mt9v034_sensor_ctx)); + ctx->cur_context = 0; + if (mt9v034_init_hw(ctx) != 0) return -1; + mt9v034_configure_context(ctx, 0, img_param); + mt9v034_configure_context(ctx, 1, img_param); + mt9v034_configure_general(ctx); +} - uint16_t new_control; +int mt9v034_prepare_update_param(void *usr, const camera_img_param *img_param); - if (global_data.param[PARAM_VIDEO_ONLY]) +void mt9v034_notify_readout_start(void *usr) { + // TODO: decide when to switch context: + + mt9v034_WriteReg16(MTV_AGC_AEC_PIXEL_COUNT_REG, pixel_count); + + uint16_t new_control; + if () new_control = 0x8188; // Context B else new_control = 0x0188; // Context A - /* image dimentions */ + mt9v034_WriteReg16(MTV_CHIP_CONTROL_REG, new_control); +} + +void mt9v034_get_current_param(void *usr, camera_img_param *img_param); + + +static int mt9v034_init_hw(mt9v034_sensor_ctx *ctx) { + GPIO_InitTypeDef GPIO_InitStructure; + I2C_InitTypeDef I2C_InitStruct; + + /* I2C2 clock enable */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE); + + /* GPIOB clock enable */ + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); + + /* Connect I2C2 pins to AF4 */ + GPIO_PinAFConfig(GPIOB, GPIO_PinSource10, GPIO_AF_I2C2); + GPIO_PinAFConfig(GPIOB, GPIO_PinSource11, GPIO_AF_I2C2); + + /* Configure I2C2 GPIOs */ + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_11; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; + GPIO_InitStructure.GPIO_OType = GPIO_OType_OD; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + GPIO_Init(GPIOB, &GPIO_InitStructure); + + /* I2C DeInit */ + I2C_DeInit(I2C2); + + /* Enable the I2C peripheral */ + I2C_Cmd(I2C2, ENABLE); + + /* Set the I2C structure parameters */ + I2C_InitStruct.I2C_Mode = I2C_Mode_I2C; + I2C_InitStruct.I2C_DutyCycle = I2C_DutyCycle_2; + I2C_InitStruct.I2C_OwnAddress1 = 0xFE; + I2C_InitStruct.I2C_Ack = I2C_Ack_Enable; + I2C_InitStruct.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; + I2C_InitStruct.I2C_ClockSpeed = 100000; + + /* Initialize the I2C peripheral w/ selected parameters */ + I2C_Init(I2C2, &I2C_InitStruct); + + /* Initialize GPIOs for EXPOSURE and STANDBY lines of the camera */ + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + GPIO_Init(GPIOA, &GPIO_InitStructure); + GPIO_ResetBits(GPIOA, GPIO_Pin_2 | GPIO_Pin_3); + + /* test I2C: */ + uint16_t version = mt9v034_ReadReg16(MTV_CHIP_VERSION_REG); + if (version != 0x1324) return -1; + + return 0; +} + +static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx) { + /* + * Chip control register. + * [2:0] Scan Mode: 0 = Progressive scan. + * [4:3] Sensor operating mode: 1 = Master mode + * [5] Stereoscopy Mode: 0 = Disabled + * [6] Stereoscopic Master/Slave: 0 = Master + * [7] Parallel output: 1 = Enabled + * [8] Simultaneous mode: 1 = Enabled. Pixel and column readout take place in conjunction with exposure. + * [15] Context A / B select 0 = Context A registers are used. + */ + mt9v034_WriteReg16(MTV_CHIP_CONTROL_REG, 0x0188); + + /* settings that are the same for both contexts: */ + + /* + * ADC Companding Mode + * [1:0] ADC Mode Context A: 2 = 10-bit linear + * [9:8] ADC Mode Context B: 2 = 10-bit linear + */ + mt9v034_WriteReg16(MTV_ADC_RES_CTRL_REG, 0x0202); + + /* + * Row Noise Correction Control + * [0] Noise Correction Ctx A: 0 / 1 = Disable / Enable + * [1] Noise Use Black Level Avg Ctx A: 0 = Disable + * [8] Noise Correction Ctx B: 0 / 1 = Disable / Enable + * [9] Noise Use Black Level Avg Ctx B: 0 = Disable + */ + uint16_t row_noise_correction; + if(global_data.param[PARAM_IMAGE_ROW_NOISE_CORR] && !global_data.param[PARAM_IMAGE_TEST_PATTERN]) { + row_noise_correction = 0x0101; + } else { + row_noise_correction = 0x0000; + } + mt9v034_WriteReg16(MTV_ROW_NOISE_CORR_CTRL_REG, row_noise_correction); + + /* + * Minimum Coarse Shutter Width + * Set to minimum. (1) + */ + mt9v034_WriteReg16(MTV_MIN_COARSE_SW_REG, 0x0001); + /* + * Maximum Coarse Shutter Width + */ + mt9v034_WriteReg16(MTV_MAX_COARSE_SW_REG, CONFIG_MAX_EXPOSURE_ROWS); + /* + * Maximum Analog Gain + */ + mt9v034_WriteReg16(MTV_ANALOG_MAX_GAIN_REG, CONFIG_MAX_ANALOG_GAIN); + + /* + * AEC/AGC Desired Bin + * User-defined “desired bin” that gives a measure of how bright the image is intended to be. + * Range: 1 - 64. + */ + uint16_t desired_brightness; + if(global_data.param[PARAM_IMAGE_LOW_LIGHT]) { + desired_brightness = 58; + } else { + desired_brightness = 16; + } + mt9v034_WriteReg16(MTV_AGC_AEC_DESIRED_BIN_REG, desired_brightness); + /* + * AEC Update Frequency + * Number of frames to skip between changes in AEC + * Range: 0-15 + */ + mt9v034_WriteReg16(MTV_AEC_UPDATE_FREQ_REG, 2); + /* + * AEC Low Pass Filter + * Range: 0-2 + */ + mt9v034_WriteReg16(MTV_AEC_LPF_REG, 1); + /* + * AGC Output Update Frequency + * Number of frames to skip between changes in AGC + * Range: 0-15 + */ + mt9v034_WriteReg16(MTV_AGC_UPDATE_FREQ_REG, 2); + /* + * AGC Low Pass Filter + * Range: 0-2 + */ + mt9v034_WriteReg16(MTV_AGC_LPF_REG, 2); + /* + * AGC/AEC Enable + * [0] AEC Enable Ctx A: 1 = Enable + * [1] AGC Enable Ctx A: 1 = Enable + * [8] AEC Enable Ctx B: 1 = Enable + * [9] AGC Enable Ctx B: 1 = Enable + */ + mt9v034_WriteReg16(MTV_AEC_AGC_ENABLE_REG, 0x0303); + /* + * Sensor Type Control + * [0] HDR Enable Ctx A: 0 = Disable + * [1] Color/Mono Sensor Control: 0 = Monochrome + * [8] HDR Enable Ctx B: 0 = Disable + */ + mt9v034_WriteReg16(MTV_SENSOR_TYPE_CTRL_REG, 0x0000); + + /* + * Digital Test Pattern + * [9:0] TWI Test Data: 0 + * [10] Use TWI Test Data: 0 = Disable. + *[12:11] Gray Shade Test Pattern: 0 / 2 = None / Horizontal Shades. + * [13] Test Enable: 0 / 1 = Disable / Enable + * [14] Flip TWI Test Data: 0 = Disable. + */ + uint16_t test_data; + if(global_data.param[PARAM_IMAGE_TEST_PATTERN]) { + test_data = 0x3000; + } else { + test_data = 0x0000; + } + mt9v034_WriteReg16(MTV_TEST_PATTERN_REG, test_data);//enable test pattern + + /* Reset */ + mt9v034_WriteReg16(MTV_SOFT_RESET_REG, 0x01); + + /* + * NOTES: + * Old code unexpectedly used: + * - 12 to 10bit companding mode on 64x64 pixel image. + * - Enabled AGC & AEC only on 64x64pixel image. + * - Enabled HDR mode on big image. + * - Used 64 x 64 = 4096 Pixels for AGC & AEC + */ +} + +static void mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, const camera_img_param *img_param) { + /* image dimensions */ uint16_t new_width_context_a = global_data.param[PARAM_IMAGE_WIDTH] * 4; // windowing off, row + col bin reduce size uint16_t new_height_context_a = global_data.param[PARAM_IMAGE_HEIGHT] * 4; uint16_t new_width_context_b = FULL_IMAGE_ROW_SIZE * 4; // windowing off, row + col bin reduce size @@ -83,7 +298,6 @@ void mt9v034_context_configuration(void) uint16_t new_hor_blanking_context_b = MAX_IMAGE_WIDTH - new_width_context_b + MINIMUM_HORIZONTAL_BLANKING; uint16_t new_ver_blanking_context_b = 10; - /* Read Mode * * bits | ... | 10 | 9 | 8 | 7 | 6 | 5 | 4 | 3 | 2 | 1 | 0 | @@ -106,131 +320,44 @@ void mt9v034_context_configuration(void) */ uint16_t min_exposure = 0x0001; // default uint16_t max_exposure = 0x01E0; // default - uint16_t new_max_gain = 64; // VALID RANGE: 16-64 (default) - uint16_t pixel_count = 4096; //64x64 take all pixels to estimate exposure time // VALID RANGE: 1-65535 - uint16_t desired_brightness = 58; // default - uint16_t resolution_ctrl = 0x0203; // default - uint16_t hdr_enabled = 0x0100; // default - uint16_t aec_agc_enabled = 0x0003; // default uint16_t coarse_sw1 = 0x01BB; // default from context A uint16_t coarse_sw2 = 0x01D9; // default from context A uint16_t shutter_width_ctrl = 0x0164; // default from context A uint16_t total_shutter_width = 0x01E0; // default from context A - uint16_t aec_update_freq = 0x02; // default Number of frames to skip between changes in AEC VALID RANGE: 0-15 - uint16_t aec_low_pass = 0x01; // default VALID RANGE: 0-2 - uint16_t agc_update_freq = 0x02; // default Number of frames to skip between changes in AGC VALID RANGE: 0-15 - uint16_t agc_low_pass = 0x02; // default VALID RANGE: 0-2 - - - if(global_data.param[PARAM_IMAGE_LOW_LIGHT]) - { - min_exposure = 0x0001; - max_exposure = 0x0040; - desired_brightness = 58; // VALID RANGE: 8-64 - resolution_ctrl = 0x0202;//10 bit linear - hdr_enabled = 0x0000; // off - aec_agc_enabled = 0x0303; // on - coarse_sw1 = 0x01BB; // default from context A - coarse_sw2 = 0x01D9; // default from context A - shutter_width_ctrl = 0x0164; // default from context A - total_shutter_width = 0x01E0; // default from context A - } - else - { - min_exposure = 0x0001; - max_exposure = 0x0040; - desired_brightness = 16; // VALID RANGE: 8-64 - resolution_ctrl = 0x0202;//10bit linear - hdr_enabled = 0x0000; // off - aec_agc_enabled = 0x0303; // on - coarse_sw1 = 0x01BB; // default from context A - coarse_sw2 = 0x01D9; // default from context A - shutter_width_ctrl = 0x0164; // default from context A - total_shutter_width = 0x01E0; // default from context A - } - - uint16_t row_noise_correction = 0x0000; // default - uint16_t test_data = 0x0000; // default - - if(global_data.param[PARAM_IMAGE_ROW_NOISE_CORR]&&!global_data.param[PARAM_IMAGE_TEST_PATTERN]) - row_noise_correction = 0x0101; - else - row_noise_correction = 0x0000; - - if(global_data.param[PARAM_IMAGE_TEST_PATTERN]) - test_data = 0x3000; //enable vertical gray shade pattern - else - test_data = 0x0000; - uint16_t version = mt9v034_ReadReg16(MTV_CHIP_VERSION_REG); - - if (version == 0x1324) - { - mt9v034_WriteReg16(MTV_CHIP_CONTROL_REG, new_control); - - /* Context A */ - mt9v034_WriteReg16(MTV_WINDOW_WIDTH_REG_A, new_width_context_a); - mt9v034_WriteReg16(MTV_WINDOW_HEIGHT_REG_A, new_height_context_a); - mt9v034_WriteReg16(MTV_HOR_BLANKING_REG_A, new_hor_blanking_context_a); - mt9v034_WriteReg16(MTV_VER_BLANKING_REG_A, new_ver_blanking_context_a); - mt9v034_WriteReg16(MTV_READ_MODE_REG_A, new_readmode_context_a); - mt9v034_WriteReg16(MTV_COLUMN_START_REG_A, (MAX_IMAGE_WIDTH - new_width_context_a) / 2 + MINIMUM_COLUMN_START); // Set column/row start point for lower resolutions (center window) - mt9v034_WriteReg16(MTV_ROW_START_REG_A, (MAX_IMAGE_HEIGHT - new_height_context_a) / 2 + MINIMUM_ROW_START); - mt9v034_WriteReg16(MTV_COARSE_SW_1_REG_A, coarse_sw1); - mt9v034_WriteReg16(MTV_COARSE_SW_2_REG_A, coarse_sw2); - mt9v034_WriteReg16(MTV_COARSE_SW_CTRL_REG_A, shutter_width_ctrl); - mt9v034_WriteReg16(MTV_COARSE_SW_TOTAL_REG_A, total_shutter_width); - - /* Context B */ - mt9v034_WriteReg16(MTV_WINDOW_WIDTH_REG_B, new_width_context_b); - mt9v034_WriteReg16(MTV_WINDOW_HEIGHT_REG_B, new_height_context_b); - mt9v034_WriteReg16(MTV_HOR_BLANKING_REG_B, new_hor_blanking_context_b); - mt9v034_WriteReg16(MTV_VER_BLANKING_REG_B, new_ver_blanking_context_b); - mt9v034_WriteReg16(MTV_READ_MODE_REG_B, new_readmode_context_b); - mt9v034_WriteReg16(MTV_COLUMN_START_REG_B, MINIMUM_COLUMN_START); // default - mt9v034_WriteReg16(MTV_ROW_START_REG_B, MINIMUM_ROW_START); - mt9v034_WriteReg16(MTV_COARSE_SW_1_REG_B, coarse_sw1); - mt9v034_WriteReg16(MTV_COARSE_SW_2_REG_B, coarse_sw2); - mt9v034_WriteReg16(MTV_COARSE_SW_CTRL_REG_B, shutter_width_ctrl); - mt9v034_WriteReg16(MTV_COARSE_SW_TOTAL_REG_B, total_shutter_width); - - /* General Settings */ - mt9v034_WriteReg16(MTV_ROW_NOISE_CORR_CTRL_REG, row_noise_correction); - mt9v034_WriteReg16(MTV_AEC_AGC_ENABLE_REG, aec_agc_enabled); // disable AEC/AGC for both contexts - mt9v034_WriteReg16(MTV_SENSOR_TYPE_CTRL_REG, hdr_enabled); // disable HDR on both contexts - mt9v034_WriteReg16(MTV_MIN_COARSE_SW_REG, min_exposure); - mt9v034_WriteReg16(MTV_MAX_COARSE_SW_REG, max_exposure); - mt9v034_WriteReg16(MTV_ANALOG_MAX_GAIN_REG, new_max_gain); - mt9v034_WriteReg16(MTV_AGC_AEC_PIXEL_COUNT_REG, pixel_count); - mt9v034_WriteReg16(MTV_AGC_AEC_DESIRED_BIN_REG, desired_brightness); - mt9v034_WriteReg16(MTV_ADC_RES_CTRL_REG, resolution_ctrl); // here is the way to regulate darkness :) - - mt9v034_WriteReg16(MTV_TEST_PATTERN_REG, test_data);//enable test pattern - - mt9v034_WriteReg16(MTV_AEC_UPDATE_FREQ_REG,aec_update_freq); - mt9v034_WriteReg16(MTV_AEC_LPF_REG,aec_low_pass); - mt9v034_WriteReg16(MTV_AGC_UPDATE_FREQ_REG,agc_update_freq); - mt9v034_WriteReg16(MTV_AGC_LPF_REG,agc_low_pass); - - /* Reset */ - mt9v034_WriteReg16(MTV_SOFT_RESET_REG, 0x01); - } - -} - -/** - * @brief Changes sensor context based on settings - */ -void mt9v034_set_context() -{ - uint16_t new_control; - if (global_data.param[PARAM_VIDEO_ONLY]) - new_control = 0x8188; // Context B - else - new_control = 0x0188; // Context A - - mt9v034_WriteReg16(MTV_CHIP_CONTROL_REG, new_control); + min_exposure = 0x0001; + max_exposure = 0x0040; + coarse_sw1 = 0x01BB; // default from context A + coarse_sw2 = 0x01D9; // default from context A + shutter_width_ctrl = 0x0164; // default from context A + total_shutter_width = 0x01E0; // default from context A + + /* Context A */ + mt9v034_WriteReg16(MTV_WINDOW_WIDTH_REG_A, new_width_context_a); + mt9v034_WriteReg16(MTV_WINDOW_HEIGHT_REG_A, new_height_context_a); + mt9v034_WriteReg16(MTV_HOR_BLANKING_REG_A, new_hor_blanking_context_a); + mt9v034_WriteReg16(MTV_VER_BLANKING_REG_A, new_ver_blanking_context_a); + mt9v034_WriteReg16(MTV_READ_MODE_REG_A, new_readmode_context_a); + mt9v034_WriteReg16(MTV_COLUMN_START_REG_A, (MAX_IMAGE_WIDTH - new_width_context_a) / 2 + MINIMUM_COLUMN_START); // Set column/row start point for lower resolutions (center window) + mt9v034_WriteReg16(MTV_ROW_START_REG_A, (MAX_IMAGE_HEIGHT - new_height_context_a) / 2 + MINIMUM_ROW_START); + mt9v034_WriteReg16(MTV_COARSE_SW_1_REG_A, coarse_sw1); + mt9v034_WriteReg16(MTV_COARSE_SW_2_REG_A, coarse_sw2); + mt9v034_WriteReg16(MTV_COARSE_SW_CTRL_REG_A, shutter_width_ctrl); + mt9v034_WriteReg16(MTV_COARSE_SW_TOTAL_REG_A, total_shutter_width); + + /* Context B */ + mt9v034_WriteReg16(MTV_WINDOW_WIDTH_REG_B, new_width_context_b); + mt9v034_WriteReg16(MTV_WINDOW_HEIGHT_REG_B, new_height_context_b); + mt9v034_WriteReg16(MTV_HOR_BLANKING_REG_B, new_hor_blanking_context_b); + mt9v034_WriteReg16(MTV_VER_BLANKING_REG_B, new_ver_blanking_context_b); + mt9v034_WriteReg16(MTV_READ_MODE_REG_B, new_readmode_context_b); + mt9v034_WriteReg16(MTV_COLUMN_START_REG_B, MINIMUM_COLUMN_START); // default + mt9v034_WriteReg16(MTV_ROW_START_REG_B, MINIMUM_ROW_START); + mt9v034_WriteReg16(MTV_COARSE_SW_1_REG_B, coarse_sw1); + mt9v034_WriteReg16(MTV_COARSE_SW_2_REG_B, coarse_sw2); + mt9v034_WriteReg16(MTV_COARSE_SW_CTRL_REG_B, shutter_width_ctrl); + mt9v034_WriteReg16(MTV_COARSE_SW_TOTAL_REG_B, total_shutter_width); } /** @@ -251,18 +378,18 @@ uint8_t mt9v034_WriteReg(uint16_t Addr, uint8_t Data) timeout = TIMEOUT_MAX; /* Initialize timeout value */ while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_MODE_SELECT)) { - /* If the timeout delay is exeeded, exit with error code */ + /* If the timeout delay is exceeded, exit with error code */ if ((timeout--) == 0) return 0xFF; } - /* Send DCMI selcted device slave Address for write */ + /* Send DCMI selected device slave Address for write */ I2C_Send7bitAddress(I2C2, mt9v034_DEVICE_WRITE_ADDRESS, I2C_Direction_Transmitter); /* Test on I2C2 EV6 and clear it */ timeout = TIMEOUT_MAX; /* Initialize timeout value */ while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)) { - /* If the timeout delay is exeeded, exit with error code */ + /* If the timeout delay is exceeded, exit with error code */ if ((timeout--) == 0) return 0xFF; } @@ -273,7 +400,7 @@ uint8_t mt9v034_WriteReg(uint16_t Addr, uint8_t Data) timeout = TIMEOUT_MAX; /* Initialize timeout value */ while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED)) { - /* If the timeout delay is exeeded, exit with error code */ + /* If the timeout delay is exceeded, exit with error code */ if ((timeout--) == 0) return 0xFF; } @@ -284,7 +411,7 @@ uint8_t mt9v034_WriteReg(uint16_t Addr, uint8_t Data) timeout = TIMEOUT_MAX; /* Initialize timeout value */ while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED)) { - /* If the timeout delay is exeeded, exit with error code */ + /* If the timeout delay is exceeded, exit with error code */ if ((timeout--) == 0) return 0xFF; } @@ -309,7 +436,7 @@ uint8_t mt9v034_WriteReg16(uint16_t address, uint16_t Data) * @brief Reads a byte from a specific Camera register * @param Addr: mt9v034 register address. * @retval data read from the specific register or 0xFF if timeout condition - * occured. + * occurred. */ uint8_t mt9v034_ReadReg(uint16_t Addr) { @@ -323,18 +450,18 @@ uint8_t mt9v034_ReadReg(uint16_t Addr) timeout = TIMEOUT_MAX; /* Initialize timeout value */ while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_MODE_SELECT)) { - /* If the timeout delay is exeeded, exit with error code */ + /* If the timeout delay is exceeded, exit with error code */ if ((timeout--) == 0) return 0xFF; } - /* Send DCMI selcted device slave Address for write */ + /* Send DCMI selected device slave Address for write */ I2C_Send7bitAddress(I2C2, mt9v034_DEVICE_READ_ADDRESS, I2C_Direction_Transmitter); /* Test on I2C2 EV6 and clear it */ timeout = TIMEOUT_MAX; /* Initialize timeout value */ while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)) { - /* If the timeout delay is exeeded, exit with error code */ + /* If the timeout delay is exceeded, exit with error code */ if ((timeout--) == 0) return 0xFF; } @@ -345,11 +472,11 @@ uint8_t mt9v034_ReadReg(uint16_t Addr) timeout = TIMEOUT_MAX; /* Initialize timeout value */ while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED)) { - /* If the timeout delay is exeeded, exit with error code */ + /* If the timeout delay is exceeded, exit with error code */ if ((timeout--) == 0) return 0xFF; } - /* Clear AF flag if arised */ + /* Clear AF flag if set */ I2C2->SR1 |= (uint16_t)0x0400; /* Generate the Start Condition */ @@ -359,18 +486,18 @@ uint8_t mt9v034_ReadReg(uint16_t Addr) timeout = TIMEOUT_MAX; /* Initialize timeout value */ while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_MODE_SELECT)) { - /* If the timeout delay is exeeded, exit with error code */ + /* If the timeout delay is exceeded, exit with error code */ if ((timeout--) == 0) return 0xFF; } - /* Send DCMI selcted device slave Address for write */ + /* Send DCMI selected device slave Address for write */ I2C_Send7bitAddress(I2C2, mt9v034_DEVICE_READ_ADDRESS, I2C_Direction_Receiver); /* Test on I2C2 EV6 and clear it */ timeout = TIMEOUT_MAX; /* Initialize timeout value */ while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED)) { - /* If the timeout delay is exeeded, exit with error code */ + /* If the timeout delay is exceeded, exit with error code */ if ((timeout--) == 0) return 0xFF; } @@ -381,7 +508,7 @@ uint8_t mt9v034_ReadReg(uint16_t Addr) timeout = TIMEOUT_MAX; /* Initialize timeout value */ while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_RECEIVED)) { - /* If the timeout delay is exeeded, exit with error code */ + /* If the timeout delay is exceeded, exit with error code */ if ((timeout--) == 0) return 0xFF; } From a197670452d010387315623ff7d634c196a2b4ad Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Wed, 5 Aug 2015 09:56:16 +0200 Subject: [PATCH 042/120] use bool for success/fail return status. --- inc/camera.h | 35 ++++++++++++++++++----------------- src/camera.c | 10 +++++----- src/mt9v034.c | 28 +++++++++++++++------------- 3 files changed, 38 insertions(+), 35 deletions(-) diff --git a/inc/camera.h b/inc/camera.h index 7a8557e..4857da6 100644 --- a/inc/camera.h +++ b/inc/camera.h @@ -36,6 +36,7 @@ #define CAMERA_H_ #include +#include #define CAMERA_MAX_BUFFER_COUNT 5 @@ -96,11 +97,11 @@ typedef void (*camera_snapshot_done_cb)(camera_image_buffer *buf); * Must be lower than or equal the configured CAMERA_MAX_BUFFER_COUNT. * There must be at least one more buffer than what is requested in camera_img_stream_get_buffers. * More buffers will reduce the latency when frames are skipped. - * @return Zero when successful. + * @return True when successful. */ -int camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const camera_transport_interface *transport, - const camera_img_param *img_param, - camera_image_buffer buffers[], size_t buffer_count); +bool camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const camera_transport_interface *transport, + const camera_img_param *img_param, + camera_image_buffer buffers[], size_t buffer_count); /** * Schedules the new parameters to take effect as soon as possible. This function makes sure that @@ -108,9 +109,9 @@ int camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const ca * @param ctx The context to use. * @param img_param The new image parameters to use. Note that the image must still fit inside the buffers passed * to camera_init. - * @return Zero on success. + * @return True on success. */ -int camera_img_stream_schedule_param_change(camera_ctx *ctx, const camera_img_param *img_param); +bool camera_img_stream_schedule_param_change(camera_ctx *ctx, const camera_img_param *img_param); /** * Gets the most recent images. If no more recent image is available this function returns immediatly without doing anything. @@ -148,9 +149,9 @@ void camera_img_stream_return_buffers(camera_ctx *ctx, camera_image_buffer **buf * @param cb Callback function which is called when the snapshot has been taken. * Note that the callback is called from the interrupt context and should only do minimal stuff * like setting a flag to notify the main loop. Calling camera_* functions is not allowed. - * @return Zero when snapshot has been successfully scheduled. + * @return True when snapshot has been successfully scheduled. */ -int camera_snapshot_schedule(camera_ctx *ctx, const camera_img_param *img_param, camera_image_buffer *dst, camera_snapshot_done_cb cb); +bool camera_snapshot_schedule(camera_ctx *ctx, const camera_img_param *img_param, camera_image_buffer *dst, camera_snapshot_done_cb cb); /** Camera sensor configuration interface. */ @@ -160,17 +161,17 @@ struct _camera_sensor_interface { * Initializes the sensor and the hardware of the microcontroller. * @param usr User pointer from this struct. * @param img_param The image parameters to use for initialization. - * @return 0 on success. + * @return true on success. */ - int (*init)(void *usr, const camera_img_param *img_param); + bool (*init)(void *usr, const camera_img_param *img_param); /** * Prepares the sensor to switch to new parameters. * This function should perform most of the work that is needed to update the sensor with new parameters. * @param usr User pointer from this struct. * @param img_param The new image parameters. - * @return 0 on success. + * @return true on success. */ - int (*prepare_update_param)(void *usr, const camera_img_param *img_param); + bool (*prepare_update_param)(void *usr, const camera_img_param *img_param); /** * Called every frame just after readout has started (but not completed yet). * This function may be used to switch the sensor to new parameters that have been previously prepared. @@ -212,12 +213,12 @@ struct _camera_transport_interface { * @param frame_done_cb Callback which should be called when a frame has been completed. * @param cb_usr Callback user data pointer which should be passed to the transfer_done_cb * and frame_done_cb callbacks. - * @return 0 on success. + * @return True on success. */ - int (*init)(void *usr, - camera_transport_transfer_done_cb transfer_done_cb, - camera_transport_frame_done_cb frame_done_cb, - void *cb_usr); + bool (*init)(void *usr, + camera_transport_transfer_done_cb transfer_done_cb, + camera_transport_frame_done_cb frame_done_cb, + void *cb_usr); }; /** diff --git a/src/camera.c b/src/camera.c index 50235db..5a0bfe8 100644 --- a/src/camera.c +++ b/src/camera.c @@ -39,9 +39,9 @@ void camera_transport_transfer_done_fn(void *usr, void *buffer, size_t size); void camera_transport_frame_done_fn(void *usr); -int camera_init(camera_ctx *ctx, camera_sensor_interface *sensor, camera_transport_interface *transport, - const camera_img_param *img_param, - camera_image_buffer buffers[], size_t buffer_count) { +bool camera_init(camera_ctx *ctx, camera_sensor_interface *sensor, camera_transport_interface *transport, + const camera_img_param *img_param, + camera_image_buffer buffers[], size_t buffer_count) { memset(ctx, 0, sizeof(camera_ctx)); ctx->sensor = sensor; ctx->transport = transport; @@ -55,7 +55,7 @@ int camera_init(camera_ctx *ctx, camera_sensor_interface *sensor, camera_transpo ctx->sensor->init(ctx->sensor->usr, img_param); } -int camera_img_stream_schedule_param_change(camera_ctx *ctx, const camera_img_param *img_param) { +bool camera_img_stream_schedule_param_change(camera_ctx *ctx, const camera_img_param *img_param) { } @@ -67,7 +67,7 @@ void camera_img_stream_return_buffers(camera_ctx *ctx, camera_image_buffer **buf } -int camera_snapshot_schedule(camera_ctx *ctx, const camera_img_param *img_param, camera_image_buffer *dst, camera_snapshot_done_cb cb) { +bool camera_snapshot_schedule(camera_ctx *ctx, const camera_img_param *img_param, camera_image_buffer *dst, camera_snapshot_done_cb cb) { } diff --git a/src/mt9v034.c b/src/mt9v034.c index 59ebd16..6f55bd9 100644 --- a/src/mt9v034.c +++ b/src/mt9v034.c @@ -51,14 +51,14 @@ typedef struct _mt9v034_sensor_ctx { camera_img_param context_b; } mt9v034_sensor_ctx; -int mt9v034_init(void *usr, const camera_img_param *img_param); -int mt9v034_prepare_update_param(void *usr, const camera_img_param *img_param); +bool mt9v034_init(void *usr, const camera_img_param *img_param); +bool mt9v034_prepare_update_param(void *usr, const camera_img_param *img_param); void mt9v034_notify_readout_start(void *usr); void mt9v034_get_current_param(void *usr, camera_img_param *img_param); -static int mt9v034_init_hw(mt9v034_sensor_ctx *ctx); +static bool mt9v034_init_hw(mt9v034_sensor_ctx *ctx); static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx); -static void mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, const camera_img_param *img_param); +static void mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, const camera_img_param *img_param, bool full_refresh); static mt9v034_sensor_ctx mt9v034_ctx; @@ -71,17 +71,19 @@ const camera_sensor_interface mt9v034_sensor_interface = { }; -int mt9v034_init(void *usr, const camera_img_param *img_param) { +bool mt9v034_init(void *usr, const camera_img_param *img_param) { mt9v034_sensor_ctx *ctx = (mt9v034_sensor_ctx *)usr; memset(ctx, 0, sizeof(mt9v034_sensor_ctx)); ctx->cur_context = 0; - if (mt9v034_init_hw(ctx) != 0) return -1; - mt9v034_configure_context(ctx, 0, img_param); - mt9v034_configure_context(ctx, 1, img_param); + if (!mt9v034_init_hw(ctx)) return false; + mt9v034_configure_context(ctx, 0, img_param, true); + mt9v034_configure_context(ctx, 1, img_param, true); mt9v034_configure_general(ctx); + + return true; } -int mt9v034_prepare_update_param(void *usr, const camera_img_param *img_param); +bool mt9v034_prepare_update_param(void *usr, const camera_img_param *img_param); void mt9v034_notify_readout_start(void *usr) { // TODO: decide when to switch context: @@ -100,7 +102,7 @@ void mt9v034_notify_readout_start(void *usr) { void mt9v034_get_current_param(void *usr, camera_img_param *img_param); -static int mt9v034_init_hw(mt9v034_sensor_ctx *ctx) { +static bool mt9v034_init_hw(mt9v034_sensor_ctx *ctx) { GPIO_InitTypeDef GPIO_InitStructure; I2C_InitTypeDef I2C_InitStruct; @@ -150,9 +152,9 @@ static int mt9v034_init_hw(mt9v034_sensor_ctx *ctx) { /* test I2C: */ uint16_t version = mt9v034_ReadReg16(MTV_CHIP_VERSION_REG); - if (version != 0x1324) return -1; + if (version != 0x1324) return false; - return 0; + return true; } static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx) { @@ -285,7 +287,7 @@ static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx) { */ } -static void mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, const camera_img_param *img_param) { +static void mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, const camera_img_param *img_param, bool full_refresh) { /* image dimensions */ uint16_t new_width_context_a = global_data.param[PARAM_IMAGE_WIDTH] * 4; // windowing off, row + col bin reduce size uint16_t new_height_context_a = global_data.param[PARAM_IMAGE_HEIGHT] * 4; From 9d3ef04666684ebcfe68d346a465e0dcdd1a28bf Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Wed, 5 Aug 2015 11:27:38 +0200 Subject: [PATCH 043/120] finished cleaning up camera sensor configuration code. updated code with detailed documentation from the datasheet. --- inc/mt9v034.h | 5 - src/mt9v034.c | 662 +++++++++++++++++++++++++++++--------------------- 2 files changed, 379 insertions(+), 288 deletions(-) diff --git a/inc/mt9v034.h b/inc/mt9v034.h index 0a280b8..ca25032 100644 --- a/inc/mt9v034.h +++ b/inc/mt9v034.h @@ -168,9 +168,4 @@ void mt9v034_context_configuration(void); */ void mt9v034_set_context(void); -uint16_t mt9v034_ReadReg16(uint8_t address); -uint8_t mt9v034_WriteReg16(uint16_t address, uint16_t Data); -uint8_t mt9v034_ReadReg(uint16_t Addr); -uint8_t mt9v034_WriteReg(uint16_t Addr, uint8_t Data); - #endif /* MT9V34_H_ */ diff --git a/src/mt9v034.c b/src/mt9v034.c index 6f55bd9..4452f7f 100644 --- a/src/mt9v034.c +++ b/src/mt9v034.c @@ -47,8 +47,7 @@ typedef struct _mt9v034_sensor_ctx { int cur_context; camera_img_param cur_param; camera_img_param exp_param; - camera_img_param context_a; - camera_img_param context_b; + camera_img_param context_p[2]; } mt9v034_sensor_ctx; bool mt9v034_init(void *usr, const camera_img_param *img_param); @@ -58,7 +57,31 @@ void mt9v034_get_current_param(void *usr, camera_img_param *img_param); static bool mt9v034_init_hw(mt9v034_sensor_ctx *ctx); static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx); -static void mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, const camera_img_param *img_param, bool full_refresh); +static bool mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, const camera_img_param *img_param, bool full_refresh); + +/** + * @brief Reads from a specific Camera register + */ +static uint16_t mt9v034_ReadReg16(uint8_t address); +/** + * @brief Writes to a specific Camera register + */ +static uint8_t mt9v034_WriteReg16(uint16_t address, uint16_t Data); +/** + * @brief Reads a byte from a specific Camera register + * @param Addr: mt9v034 register address. + * @retval data read from the specific register or 0xFF if timeout condition + * occurred. + */ +static uint8_t mt9v034_ReadReg(uint16_t Addr); +/** + * @brief Writes a byte at a specific Camera register + * @param Addr: mt9v034 register address. + * @param Data: Data to be written to the specific register + * @retval 0x00 if write operation is OK. + * 0xFF if timeout condition occured (device not connected or bus error). + */ +static uint8_t mt9v034_WriteReg(uint16_t Addr, uint8_t Data); static mt9v034_sensor_ctx mt9v034_ctx; @@ -102,61 +125,6 @@ void mt9v034_notify_readout_start(void *usr) { void mt9v034_get_current_param(void *usr, camera_img_param *img_param); -static bool mt9v034_init_hw(mt9v034_sensor_ctx *ctx) { - GPIO_InitTypeDef GPIO_InitStructure; - I2C_InitTypeDef I2C_InitStruct; - - /* I2C2 clock enable */ - RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE); - - /* GPIOB clock enable */ - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); - - /* Connect I2C2 pins to AF4 */ - GPIO_PinAFConfig(GPIOB, GPIO_PinSource10, GPIO_AF_I2C2); - GPIO_PinAFConfig(GPIOB, GPIO_PinSource11, GPIO_AF_I2C2); - - /* Configure I2C2 GPIOs */ - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_11; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; - GPIO_InitStructure.GPIO_OType = GPIO_OType_OD; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; - GPIO_Init(GPIOB, &GPIO_InitStructure); - - /* I2C DeInit */ - I2C_DeInit(I2C2); - - /* Enable the I2C peripheral */ - I2C_Cmd(I2C2, ENABLE); - - /* Set the I2C structure parameters */ - I2C_InitStruct.I2C_Mode = I2C_Mode_I2C; - I2C_InitStruct.I2C_DutyCycle = I2C_DutyCycle_2; - I2C_InitStruct.I2C_OwnAddress1 = 0xFE; - I2C_InitStruct.I2C_Ack = I2C_Ack_Enable; - I2C_InitStruct.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; - I2C_InitStruct.I2C_ClockSpeed = 100000; - - /* Initialize the I2C peripheral w/ selected parameters */ - I2C_Init(I2C2, &I2C_InitStruct); - - /* Initialize GPIOs for EXPOSURE and STANDBY lines of the camera */ - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; - GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; - GPIO_Init(GPIOA, &GPIO_InitStructure); - GPIO_ResetBits(GPIOA, GPIO_Pin_2 | GPIO_Pin_3); - - /* test I2C: */ - uint16_t version = mt9v034_ReadReg16(MTV_CHIP_VERSION_REG); - if (version != 0x1324) return false; - - return true; -} - static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx) { /* * Chip control register. @@ -284,251 +252,379 @@ static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx) { * - Enabled AGC & AEC only on 64x64pixel image. * - Enabled HDR mode on big image. * - Used 64 x 64 = 4096 Pixels for AGC & AEC + * - row / col bin 4 on 64x64 + * - row / col bin 2 on big image */ } -static void mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, const camera_img_param *img_param, bool full_refresh) { +static bool mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, const camera_img_param *img_param, bool full_refresh) { + bool update_size = full_refresh; + bool update_binning = full_refresh; + if (context_idx < 0 || context_idx >= 2) return false; + camera_img_param *ctx_param = &ctx->context_p[context_idx]; + /* check which sets of parameters we need to update: */ + if (!full_refresh) { + if (ctx_param->size.x != img_param->size.x || + ctx_param->size.y != img_param->size.y) { + update_size = true; + } + if (ctx_param->binning != img_param->binning) { + update_binning = true; + } + } + + /* + * Coarse Shutter Width 1 + * The row number in which the first knee occurs. + * Default: 0x01BB + */ + uint16_t coarse_sw1 = 0x01BB; + /* + * Coarse Shutter Width 2 + * The row number in which the second knee occurs. + * Default: 0x01D9 + */ + uint16_t coarse_sw2 = 0x01D9; + /* + * Shutter Width Control + * When Exposure Knee Point Auto Adjust is enabled, + * then one-half to the power of this value indicates the + * ratio of duration time t2, when saturation control + * gate is adjusted to level V2, to total coarse + * integration. + * [3:0] T2 Ratio: 4 -> Tint_tot * (1/2) ^ n + * [7:4] T3 Ratio: 6 -> Tint_tot * (1/2) ^ n + * [8] Exposure Knee Auto Adj: 1 = Enabled. + * [9] Single Knee Enable: 0 = Disabled. + * + * * Tint_tot = Coarse Shutter Width Total + * + * NOTE: update vertical blanking if changing parameters here! + */ + uint16_t coarse_sw_ctrl = 0x0164; + /* + * Coarse Shutter Width Total + * Total integration time in number of rows. This value is used only when AEC is disabled. + * Default: 0x01E0 + */ + uint16_t coarse_sw_total = 0x01E0; + /* image dimensions */ - uint16_t new_width_context_a = global_data.param[PARAM_IMAGE_WIDTH] * 4; // windowing off, row + col bin reduce size - uint16_t new_height_context_a = global_data.param[PARAM_IMAGE_HEIGHT] * 4; - uint16_t new_width_context_b = FULL_IMAGE_ROW_SIZE * 4; // windowing off, row + col bin reduce size - uint16_t new_height_context_b = FULL_IMAGE_COLUMN_SIZE * 4; - - /* blanking settings */ - uint16_t new_hor_blanking_context_a = 350 + MINIMUM_HORIZONTAL_BLANKING;// 350 is minimum value without distortions - uint16_t new_ver_blanking_context_a = 10; // this value is the first without image errors (dark lines) - uint16_t new_hor_blanking_context_b = MAX_IMAGE_WIDTH - new_width_context_b + MINIMUM_HORIZONTAL_BLANKING; - uint16_t new_ver_blanking_context_b = 10; - - /* Read Mode - * - * bits | ... | 10 | 9 | 8 | 7 | 6 | 5 | 4 | 3 | 2 | 1 | 0 | - * ------------------------------------------------------------------- - * current values | ... | 0 | 1 | 1 | 0 | 0 | 0 | 0 | 1 | 0 | 1 | 0 | - * - * (1:0) Row Bin - * (3:2) Column Bin - * (9:8) Reserved - * + uint16_t width = img_param->size.x * img_param->binning; + uint16_t height = img_param->size.y * img_param->binning; + if (width < 1 || width > 752) return false; + if (height < 1 || height > 480) return false; + + uint16_t col_start = 1 + (752 - width) / 2; + uint16_t row_start = 4 + (480 - height) / 2; + + /* + * Horizontal Blanking + * Number of blank columns in a row. + * Minimum horizontal blanking is 61 for normal mode, + * 71 for column bin 2 mode, and 91 for column bin 4 mode. + * + * The MT9V034 uses column parallel analog-digital converters, thus short row timing is not possible. + * The minimum total row time is 690 columns (horizontal width + horizontal blanking). The minimum + * horizontal blanking is 61( / 71 / 91). When the window width is set below 627, horizontal blanking + * must be increased. + * + * In old code for bin 4 and 64 pixels it was 350 + 91. + */ + uint16_t hor_blanking = 61; + switch (img_param->binning) { + case 1: + /* init to minimum: */ + hor_blanking = 61; + /* increase if window is smaller: */ + if (width < 627) { + hor_blanking += 627 - width; + } + break; + case 2: + /* init to minimum: */ + hor_blanking = 71; + /* increase if window is smaller: */ + if (width < 627) { + hor_blanking += 627 - width; + } + break; + case 4: + /* init to minimum: */ + hor_blanking = 91; + /* increase if window is smaller: */ + if (width < 627) { + hor_blanking += 627 - width; + } + break; + } + + /* + * Vertical Blanking + * Number of blank rows in a frame. V-Blank value must meet the following minimums: + * Linear Mode: + * V-Blank (min) = SW_total - R0x08 + 7 + * R0x08 (Coarse Shutter Width 1) + * If manual exposure then SW_total = R0x0B. (Coarse Shutter Width Total) + * If auto-exposure mode then SW_total = R0xAD. (Maximum Coarse Shutter Width) + * + * If Auto-Knee Point enabled, then V-Blank (min) = (t2 + t3 + 7). + * t2 = Tint_tot * (1/2) ^ T2 Ratio -> 4 + * t3 = Tint_tot * (1/2) ^ T3 Ratio -> 1 */ - uint16_t new_readmode_context_a = 0x30A ; // row + col bin 4 enable, (9:8) default - uint16_t new_readmode_context_b = 0x305 ; // row bin 2 col bin 2 enable, (9:8) default + uint16_t ver_blanking = 4 + 1 + 7; /* - * Settings for both context: - * - * Exposure time should not affect frame time - * so we set max on 64 (lines) = 0x40 + * Read Mode + * [1:0] Row Bin: 0 / 1 / 2 = Bin 1 / Bin 2 / Bin 4 + * [3:2] Col Bin: 0 / 1 / 2 = Bin 1 / Bin 2 / Bin 4 + * [4] Row flip: 0 = Disable. + * [5] Column flip: 0 = Disable. + * [6] Show Dark Rows: 0 = Disable. + * [7] Show Dark Columns: 0 = Disable. + * [9:8] Reserved: 3 = Must be 3. */ - uint16_t min_exposure = 0x0001; // default - uint16_t max_exposure = 0x01E0; // default - - uint16_t coarse_sw1 = 0x01BB; // default from context A - uint16_t coarse_sw2 = 0x01D9; // default from context A - uint16_t shutter_width_ctrl = 0x0164; // default from context A - uint16_t total_shutter_width = 0x01E0; // default from context A - - min_exposure = 0x0001; - max_exposure = 0x0040; - coarse_sw1 = 0x01BB; // default from context A - coarse_sw2 = 0x01D9; // default from context A - shutter_width_ctrl = 0x0164; // default from context A - total_shutter_width = 0x01E0; // default from context A + uint16_t readmode = 0x0300; + switch (img_param->binning) { + case 1: readmode |= 0x0000; break; + case 2: readmode |= 0x0005; break; + case 4: readmode |= 0x000A; break; + default: return false; + } - /* Context A */ - mt9v034_WriteReg16(MTV_WINDOW_WIDTH_REG_A, new_width_context_a); - mt9v034_WriteReg16(MTV_WINDOW_HEIGHT_REG_A, new_height_context_a); - mt9v034_WriteReg16(MTV_HOR_BLANKING_REG_A, new_hor_blanking_context_a); - mt9v034_WriteReg16(MTV_VER_BLANKING_REG_A, new_ver_blanking_context_a); - mt9v034_WriteReg16(MTV_READ_MODE_REG_A, new_readmode_context_a); - mt9v034_WriteReg16(MTV_COLUMN_START_REG_A, (MAX_IMAGE_WIDTH - new_width_context_a) / 2 + MINIMUM_COLUMN_START); // Set column/row start point for lower resolutions (center window) - mt9v034_WriteReg16(MTV_ROW_START_REG_A, (MAX_IMAGE_HEIGHT - new_height_context_a) / 2 + MINIMUM_ROW_START); - mt9v034_WriteReg16(MTV_COARSE_SW_1_REG_A, coarse_sw1); - mt9v034_WriteReg16(MTV_COARSE_SW_2_REG_A, coarse_sw2); - mt9v034_WriteReg16(MTV_COARSE_SW_CTRL_REG_A, shutter_width_ctrl); - mt9v034_WriteReg16(MTV_COARSE_SW_TOTAL_REG_A, total_shutter_width); - - /* Context B */ - mt9v034_WriteReg16(MTV_WINDOW_WIDTH_REG_B, new_width_context_b); - mt9v034_WriteReg16(MTV_WINDOW_HEIGHT_REG_B, new_height_context_b); - mt9v034_WriteReg16(MTV_HOR_BLANKING_REG_B, new_hor_blanking_context_b); - mt9v034_WriteReg16(MTV_VER_BLANKING_REG_B, new_ver_blanking_context_b); - mt9v034_WriteReg16(MTV_READ_MODE_REG_B, new_readmode_context_b); - mt9v034_WriteReg16(MTV_COLUMN_START_REG_B, MINIMUM_COLUMN_START); // default - mt9v034_WriteReg16(MTV_ROW_START_REG_B, MINIMUM_ROW_START); - mt9v034_WriteReg16(MTV_COARSE_SW_1_REG_B, coarse_sw1); - mt9v034_WriteReg16(MTV_COARSE_SW_2_REG_B, coarse_sw2); - mt9v034_WriteReg16(MTV_COARSE_SW_CTRL_REG_B, shutter_width_ctrl); - mt9v034_WriteReg16(MTV_COARSE_SW_TOTAL_REG_B, total_shutter_width); + switch (context_idx) { + case 0: + if (update_size || update_binning) { + mt9v034_WriteReg16(MTV_WINDOW_WIDTH_REG_A, width); + mt9v034_WriteReg16(MTV_WINDOW_HEIGHT_REG_A, height); + mt9v034_WriteReg16(MTV_HOR_BLANKING_REG_A, hor_blanking); + mt9v034_WriteReg16(MTV_VER_BLANKING_REG_A, ver_blanking); + mt9v034_WriteReg16(MTV_READ_MODE_REG_A, readmode); + mt9v034_WriteReg16(MTV_COLUMN_START_REG_A, col_start); + mt9v034_WriteReg16(MTV_ROW_START_REG_A, row_start); + } + if (full_refresh) { + mt9v034_WriteReg16(MTV_COARSE_SW_1_REG_A, coarse_sw1); + mt9v034_WriteReg16(MTV_COARSE_SW_2_REG_A, coarse_sw2); + mt9v034_WriteReg16(MTV_COARSE_SW_CTRL_REG_A, coarse_sw_ctrl); + mt9v034_WriteReg16(MTV_COARSE_SW_TOTAL_REG_A, coarse_sw_total); + } + break; + case 1: + if (update_size || update_binning) { + mt9v034_WriteReg16(MTV_WINDOW_WIDTH_REG_B, width); + mt9v034_WriteReg16(MTV_WINDOW_HEIGHT_REG_B, height); + mt9v034_WriteReg16(MTV_HOR_BLANKING_REG_B, hor_blanking); + mt9v034_WriteReg16(MTV_VER_BLANKING_REG_B, ver_blanking); + mt9v034_WriteReg16(MTV_READ_MODE_REG_B, readmode); + mt9v034_WriteReg16(MTV_COLUMN_START_REG_B, col_start); + mt9v034_WriteReg16(MTV_ROW_START_REG_B, row_start); + } + if (full_refresh) { + mt9v034_WriteReg16(MTV_COARSE_SW_1_REG_B, coarse_sw1); + mt9v034_WriteReg16(MTV_COARSE_SW_2_REG_B, coarse_sw2); + mt9v034_WriteReg16(MTV_COARSE_SW_CTRL_REG_B, coarse_sw_ctrl); + mt9v034_WriteReg16(MTV_COARSE_SW_TOTAL_REG_B, coarse_sw_total); + } + break; + } + + return true; } -/** - * @brief Writes a byte at a specific Camera register - * @param Addr: mt9v034 register address. - * @param Data: Data to be written to the specific register - * @retval 0x00 if write operation is OK. - * 0xFF if timeout condition occured (device not connected or bus error). - */ -uint8_t mt9v034_WriteReg(uint16_t Addr, uint8_t Data) -{ - uint32_t timeout = TIMEOUT_MAX; - - /* Generate the Start Condition */ - I2C_GenerateSTART(I2C2, ENABLE); - - /* Test on I2C2 EV5 and clear it */ - timeout = TIMEOUT_MAX; /* Initialize timeout value */ - while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_MODE_SELECT)) - { - /* If the timeout delay is exceeded, exit with error code */ - if ((timeout--) == 0) return 0xFF; - } - - /* Send DCMI selected device slave Address for write */ - I2C_Send7bitAddress(I2C2, mt9v034_DEVICE_WRITE_ADDRESS, I2C_Direction_Transmitter); - - /* Test on I2C2 EV6 and clear it */ - timeout = TIMEOUT_MAX; /* Initialize timeout value */ - while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)) - { - /* If the timeout delay is exceeded, exit with error code */ - if ((timeout--) == 0) return 0xFF; - } - - /* Send I2C2 location address LSB */ - I2C_SendData(I2C2, (uint8_t)(Addr)); - - /* Test on I2C2 EV8 and clear it */ - timeout = TIMEOUT_MAX; /* Initialize timeout value */ - while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED)) - { - /* If the timeout delay is exceeded, exit with error code */ - if ((timeout--) == 0) return 0xFF; - } - - /* Send Data */ - I2C_SendData(I2C2, Data); - - /* Test on I2C2 EV8 and clear it */ - timeout = TIMEOUT_MAX; /* Initialize timeout value */ - while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED)) - { - /* If the timeout delay is exceeded, exit with error code */ - if ((timeout--) == 0) return 0xFF; - } - - /* Send I2C2 STOP Condition */ - I2C_GenerateSTOP(I2C2, ENABLE); - - /* If operation is OK, return 0 */ - return 0; +static bool mt9v034_init_hw(mt9v034_sensor_ctx *ctx) { + GPIO_InitTypeDef GPIO_InitStructure; + I2C_InitTypeDef I2C_InitStruct; + + /* I2C2 clock enable */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE); + + /* GPIOB clock enable */ + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); + + /* Connect I2C2 pins to AF4 */ + GPIO_PinAFConfig(GPIOB, GPIO_PinSource10, GPIO_AF_I2C2); + GPIO_PinAFConfig(GPIOB, GPIO_PinSource11, GPIO_AF_I2C2); + + /* Configure I2C2 GPIOs */ + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_11; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; + GPIO_InitStructure.GPIO_OType = GPIO_OType_OD; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + GPIO_Init(GPIOB, &GPIO_InitStructure); + + /* I2C DeInit */ + I2C_DeInit(I2C2); + + /* Enable the I2C peripheral */ + I2C_Cmd(I2C2, ENABLE); + + /* Set the I2C structure parameters */ + I2C_InitStruct.I2C_Mode = I2C_Mode_I2C; + I2C_InitStruct.I2C_DutyCycle = I2C_DutyCycle_2; + I2C_InitStruct.I2C_OwnAddress1 = 0xFE; + I2C_InitStruct.I2C_Ack = I2C_Ack_Enable; + I2C_InitStruct.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; + I2C_InitStruct.I2C_ClockSpeed = 100000; + + /* Initialize the I2C peripheral w/ selected parameters */ + I2C_Init(I2C2, &I2C_InitStruct); + + /* Initialize GPIOs for EXPOSURE and STANDBY lines of the camera */ + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + GPIO_Init(GPIOA, &GPIO_InitStructure); + GPIO_ResetBits(GPIOA, GPIO_Pin_2 | GPIO_Pin_3); + + /* test I2C: */ + uint16_t version = mt9v034_ReadReg16(MTV_CHIP_VERSION_REG); + if (version != 0x1324) return false; + + return true; } -/** - * @brief Writes to a specific Camera register - */ -uint8_t mt9v034_WriteReg16(uint16_t address, uint16_t Data) -{ +static uint8_t mt9v034_WriteReg(uint16_t Addr, uint8_t Data) { + uint32_t timeout = TIMEOUT_MAX; + + /* Generate the Start Condition */ + I2C_GenerateSTART(I2C2, ENABLE); + + /* Test on I2C2 EV5 and clear it */ + timeout = TIMEOUT_MAX; /* Initialize timeout value */ + while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_MODE_SELECT)) + { + /* If the timeout delay is exceeded, exit with error code */ + if ((timeout--) == 0) return 0xFF; + } + + /* Send DCMI selected device slave Address for write */ + I2C_Send7bitAddress(I2C2, mt9v034_DEVICE_WRITE_ADDRESS, I2C_Direction_Transmitter); + + /* Test on I2C2 EV6 and clear it */ + timeout = TIMEOUT_MAX; /* Initialize timeout value */ + while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)) + { + /* If the timeout delay is exceeded, exit with error code */ + if ((timeout--) == 0) return 0xFF; + } + + /* Send I2C2 location address LSB */ + I2C_SendData(I2C2, (uint8_t)(Addr)); + + /* Test on I2C2 EV8 and clear it */ + timeout = TIMEOUT_MAX; /* Initialize timeout value */ + while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED)) + { + /* If the timeout delay is exceeded, exit with error code */ + if ((timeout--) == 0) return 0xFF; + } + + /* Send Data */ + I2C_SendData(I2C2, Data); + + /* Test on I2C2 EV8 and clear it */ + timeout = TIMEOUT_MAX; /* Initialize timeout value */ + while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED)) + { + /* If the timeout delay is exceeded, exit with error code */ + if ((timeout--) == 0) return 0xFF; + } + + /* Send I2C2 STOP Condition */ + I2C_GenerateSTOP(I2C2, ENABLE); + + /* If operation is OK, return 0 */ + return 0; +} + +static uint8_t mt9v034_WriteReg16(uint16_t address, uint16_t Data) { uint8_t result = mt9v034_WriteReg(address, (uint8_t)( Data >> 8)); // write upper byte result |= mt9v034_WriteReg(0xF0, (uint8_t) Data); // write lower byte return result; } -/** - * @brief Reads a byte from a specific Camera register - * @param Addr: mt9v034 register address. - * @retval data read from the specific register or 0xFF if timeout condition - * occurred. - */ -uint8_t mt9v034_ReadReg(uint16_t Addr) -{ - uint32_t timeout = TIMEOUT_MAX; - uint8_t Data = 0; - - /* Generate the Start Condition */ - I2C_GenerateSTART(I2C2, ENABLE); - - /* Test on I2C2 EV5 and clear it */ - timeout = TIMEOUT_MAX; /* Initialize timeout value */ - while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_MODE_SELECT)) - { - /* If the timeout delay is exceeded, exit with error code */ - if ((timeout--) == 0) return 0xFF; - } - - /* Send DCMI selected device slave Address for write */ - I2C_Send7bitAddress(I2C2, mt9v034_DEVICE_READ_ADDRESS, I2C_Direction_Transmitter); - - /* Test on I2C2 EV6 and clear it */ - timeout = TIMEOUT_MAX; /* Initialize timeout value */ - while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)) - { - /* If the timeout delay is exceeded, exit with error code */ - if ((timeout--) == 0) return 0xFF; - } - - /* Send I2C2 location address LSB */ - I2C_SendData(I2C2, (uint8_t)(Addr)); - - /* Test on I2C2 EV8 and clear it */ - timeout = TIMEOUT_MAX; /* Initialize timeout value */ - while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED)) - { - /* If the timeout delay is exceeded, exit with error code */ - if ((timeout--) == 0) return 0xFF; - } - - /* Clear AF flag if set */ - I2C2->SR1 |= (uint16_t)0x0400; - - /* Generate the Start Condition */ - I2C_GenerateSTART(I2C2, ENABLE); - - /* Test on I2C2 EV6 and clear it */ - timeout = TIMEOUT_MAX; /* Initialize timeout value */ - while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_MODE_SELECT)) - { - /* If the timeout delay is exceeded, exit with error code */ - if ((timeout--) == 0) return 0xFF; - } - - /* Send DCMI selected device slave Address for write */ - I2C_Send7bitAddress(I2C2, mt9v034_DEVICE_READ_ADDRESS, I2C_Direction_Receiver); - - /* Test on I2C2 EV6 and clear it */ - timeout = TIMEOUT_MAX; /* Initialize timeout value */ - while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED)) - { - /* If the timeout delay is exceeded, exit with error code */ - if ((timeout--) == 0) return 0xFF; - } - - /* Prepare an NACK for the next data received */ - I2C_AcknowledgeConfig(I2C2, DISABLE); - - /* Test on I2C2 EV7 and clear it */ - timeout = TIMEOUT_MAX; /* Initialize timeout value */ - while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_RECEIVED)) - { - /* If the timeout delay is exceeded, exit with error code */ - if ((timeout--) == 0) return 0xFF; - } - - /* Prepare Stop after receiving data */ - I2C_GenerateSTOP(I2C2, ENABLE); - - /* Receive the Data */ - Data = I2C_ReceiveData(I2C2); - - /* return the read data */ - return Data; +static uint8_t mt9v034_ReadReg(uint16_t Addr) { + uint32_t timeout = TIMEOUT_MAX; + uint8_t Data = 0; + + /* Generate the Start Condition */ + I2C_GenerateSTART(I2C2, ENABLE); + + /* Test on I2C2 EV5 and clear it */ + timeout = TIMEOUT_MAX; /* Initialize timeout value */ + while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_MODE_SELECT)) { + /* If the timeout delay is exceeded, exit with error code */ + if ((timeout--) == 0) return 0xFF; + } + + /* Send DCMI selected device slave Address for write */ + I2C_Send7bitAddress(I2C2, mt9v034_DEVICE_READ_ADDRESS, I2C_Direction_Transmitter); + + /* Test on I2C2 EV6 and clear it */ + timeout = TIMEOUT_MAX; /* Initialize timeout value */ + while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)) { + /* If the timeout delay is exceeded, exit with error code */ + if ((timeout--) == 0) return 0xFF; + } + + /* Send I2C2 location address LSB */ + I2C_SendData(I2C2, (uint8_t)(Addr)); + + /* Test on I2C2 EV8 and clear it */ + timeout = TIMEOUT_MAX; /* Initialize timeout value */ + while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED)) { + /* If the timeout delay is exceeded, exit with error code */ + if ((timeout--) == 0) return 0xFF; + } + + /* Clear AF flag if set */ + I2C2->SR1 |= (uint16_t)0x0400; + + /* Generate the Start Condition */ + I2C_GenerateSTART(I2C2, ENABLE); + + /* Test on I2C2 EV6 and clear it */ + timeout = TIMEOUT_MAX; /* Initialize timeout value */ + while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_MODE_SELECT)) { + /* If the timeout delay is exceeded, exit with error code */ + if ((timeout--) == 0) return 0xFF; + } + + /* Send DCMI selected device slave Address for write */ + I2C_Send7bitAddress(I2C2, mt9v034_DEVICE_READ_ADDRESS, I2C_Direction_Receiver); + + /* Test on I2C2 EV6 and clear it */ + timeout = TIMEOUT_MAX; /* Initialize timeout value */ + while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED)) { + /* If the timeout delay is exceeded, exit with error code */ + if ((timeout--) == 0) return 0xFF; + } + + /* Prepare an NACK for the next data received */ + I2C_AcknowledgeConfig(I2C2, DISABLE); + + /* Test on I2C2 EV7 and clear it */ + timeout = TIMEOUT_MAX; /* Initialize timeout value */ + while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_RECEIVED)) { + /* If the timeout delay is exceeded, exit with error code */ + if ((timeout--) == 0) return 0xFF; + } + + /* Prepare Stop after receiving data */ + I2C_GenerateSTOP(I2C2, ENABLE); + + /* Receive the Data */ + Data = I2C_ReceiveData(I2C2); + + /* return the read data */ + return Data; } -/** - * @brief Reads from a specific Camera register - */ -uint16_t mt9v034_ReadReg16(uint8_t address) -{ +static uint16_t mt9v034_ReadReg16(uint8_t address) { uint16_t result = mt9v034_ReadReg(address) << 8; // read upper byte result |= mt9v034_ReadReg(0xF0); // read lower byte return result; From ef308b57597a1ccfcb330be832b705e6e1bebd91 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Wed, 5 Aug 2015 13:08:20 +0200 Subject: [PATCH 044/120] finished camera sensor interface for mt9v034. - implemented context switch logic - implemented image parameter simulation --- src/mt9v034.c | 97 +++++++++++++++++++++++++++++++++++++++------------ 1 file changed, 74 insertions(+), 23 deletions(-) diff --git a/src/mt9v034.c b/src/mt9v034.c index 4452f7f..1545c88 100644 --- a/src/mt9v034.c +++ b/src/mt9v034.c @@ -43,12 +43,8 @@ #include -typedef struct _mt9v034_sensor_ctx { - int cur_context; - camera_img_param cur_param; - camera_img_param exp_param; - camera_img_param context_p[2]; -} mt9v034_sensor_ctx; +struct _mt9v034_sensor_ctx; +typedef struct _mt9v034_sensor_ctx mt9v034_sensor_ctx; bool mt9v034_init(void *usr, const camera_img_param *img_param); bool mt9v034_prepare_update_param(void *usr, const camera_img_param *img_param); @@ -79,7 +75,7 @@ static uint8_t mt9v034_ReadReg(uint16_t Addr); * @param Addr: mt9v034 register address. * @param Data: Data to be written to the specific register * @retval 0x00 if write operation is OK. - * 0xFF if timeout condition occured (device not connected or bus error). + * 0xFF if timeout condition occurred (device not connected or bus error). */ static uint8_t mt9v034_WriteReg(uint16_t Addr, uint8_t Data); @@ -93,37 +89,88 @@ const camera_sensor_interface mt9v034_sensor_interface = { .get_current_param = mt9v034_get_current_param }; +struct _mt9v034_sensor_ctx { + volatile int cur_context; ///< The current camera sensor context that has been activated. + volatile bool do_switch_context;///< When true the mt9v034_notify_readout_start function will switch the camera sensor context. + volatile int desired_context; ///< The desired context when do_switch_context is true. + uint16_t chip_control_reg; ///< The current chip control register value. + camera_img_param cur_param; ///< The parameters of the frame that is beeing read out. + camera_img_param exp_param; ///< Because exposure parameters take one more frame time to propagate to the output this holds the exposure settings. + camera_img_param context_p[2]; +}; bool mt9v034_init(void *usr, const camera_img_param *img_param) { mt9v034_sensor_ctx *ctx = (mt9v034_sensor_ctx *)usr; memset(ctx, 0, sizeof(mt9v034_sensor_ctx)); ctx->cur_context = 0; + ctx->desired_context = 0; + ctx->do_switch_context = false; if (!mt9v034_init_hw(ctx)) return false; mt9v034_configure_context(ctx, 0, img_param, true); mt9v034_configure_context(ctx, 1, img_param, true); mt9v034_configure_general(ctx); - + /* initialize the cur and exp param: */ + ctx->cur_param = *img_param; + ctx->exp_param = *img_param; + /* do a forced context update next. */ + ctx->do_switch_context = true; return true; } -bool mt9v034_prepare_update_param(void *usr, const camera_img_param *img_param); +bool mt9v034_prepare_update_param(void *usr, const camera_img_param *img_param) { + mt9v034_sensor_ctx *ctx = (mt9v034_sensor_ctx *)usr; + /* deassert pending context switch command: */ + ctx->do_switch_context = false; + /* determine which context to use to do the parameter update: */ + int cur_ctx_idx = ctx->cur_context; + int new_ctx_idx = !cur_ctx_idx; + if (!mt9v034_configure_context(ctx, new_ctx_idx, img_param, false)) { + // invalid parameters. + // check if there was a pending context switch when we started: + if (cur_ctx_idx != ctx->desired_context) { + ctx->do_switch_context = true; + } + return false; + } + /* setup context switching */ + ctx->desired_context = new_ctx_idx; + ctx->do_switch_context = true; + return true; +} void mt9v034_notify_readout_start(void *usr) { - // TODO: decide when to switch context: - - mt9v034_WriteReg16(MTV_AGC_AEC_PIXEL_COUNT_REG, pixel_count); - - uint16_t new_control; - if () - new_control = 0x8188; // Context B - else - new_control = 0x0188; // Context A - - mt9v034_WriteReg16(MTV_CHIP_CONTROL_REG, new_control); + mt9v034_sensor_ctx *ctx = (mt9v034_sensor_ctx *)usr; + camera_img_param *cur_ctx_param = &ctx->context_p[ctx->cur_context]; + /* update the sensor parameter simulation: */ + /* transfer exposure settings from exp_param into cur_param */ + // There are no exposure settings yet. + /* transfer exposure settings of currently active context into exp_param */ + // There are no exposure settings yet. + /* transfer other settings of currently active context into cur_param */ + ctx->cur_param.size = cur_ctx_param->size; + ctx->cur_param.binning = cur_ctx_param->binning; + /* handle context switching: */ + if (ctx->do_switch_context) { + /* update chip control register bit 15: */ + int desired_ctx_idx = ctx->desired_context; + camera_img_param *ctx_param = &ctx->context_p[desired_ctx_idx]; + switch(desired_ctx_idx) { + case 0: ctx->chip_control_reg &= 0x7FFF; break; + case 1: ctx->chip_control_reg |= 0x8000; break; + } + mt9v034_WriteReg16(MTV_CHIP_CONTROL_REG, ctx->chip_control_reg); + /* update pixel count for AGC and AEC: */ + mt9v034_WriteReg16(MTV_AGC_AEC_PIXEL_COUNT_REG, ctx_param->size.x * ctx_param->size.y); + /* done. */ + ctx->do_switch_context = false; + ctx->cur_context = desired_ctx_idx; + } } -void mt9v034_get_current_param(void *usr, camera_img_param *img_param); - +void mt9v034_get_current_param(void *usr, camera_img_param *img_param) { + mt9v034_sensor_ctx *ctx = (mt9v034_sensor_ctx *)usr; + *img_param = *ctx->cur_param; +} static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx) { /* @@ -136,7 +183,8 @@ static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx) { * [8] Simultaneous mode: 1 = Enabled. Pixel and column readout take place in conjunction with exposure. * [15] Context A / B select 0 = Context A registers are used. */ - mt9v034_WriteReg16(MTV_CHIP_CONTROL_REG, 0x0188); + ctx->chip_control_reg = 0x0188; + mt9v034_WriteReg16(MTV_CHIP_CONTROL_REG, ctx->chip_control_reg); /* settings that are the same for both contexts: */ @@ -429,6 +477,9 @@ static bool mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, break; } + /* update the current settings: */ + *ctx_param = *img_param; + return true; } From 31b9aa606657ded934e6ab7ec98eeb2da8a32cf2 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Wed, 5 Aug 2015 14:34:32 +0200 Subject: [PATCH 045/120] implemented image streaming logic. updated some of the documentation. --- inc/camera.h | 57 ++++++++++++++++++--- src/camera.c | 136 ++++++++++++++++++++++++++++++++++++++++++++------ src/mt9v034.c | 11 +++- 3 files changed, 179 insertions(+), 25 deletions(-) diff --git a/inc/camera.h b/inc/camera.h index 4857da6..b9f0a80 100644 --- a/inc/camera.h +++ b/inc/camera.h @@ -90,7 +90,7 @@ typedef void (*camera_snapshot_done_cb)(camera_image_buffer *buf); * @param img_param The initial image parameters to use for img_stream operation mode. * @param buffers Array of initialized buffers to use for buffering the images in img_stream mode. * In each camera_image_buffer the buffer and buffer_size members must be correctly set. - * The camera_img_stream_get_buffers function will return pointers to one of these buffers. + * The camera_img_stream_get_buffers function will return pointers to one of these (copied) buffers. * The buffer can reside in the CCM of the microcontroller. * The structs are copied to an internal data-structure. * @param buffer_count Number of buffers that are passed in buffers. @@ -122,11 +122,12 @@ bool camera_img_stream_schedule_param_change(camera_ctx *ctx, const camera_img_p * It is guaranteed to retrieve consecutive images. * @param count Number of most recent images to retrieve. Must be lower than buffer_count - 1 which was * passed to camera_init. - * @return 0 when a new most recent image has been retrieved. - * 1 if there was no new most recent image since the last call to this function. (In this case nothing has been updated) + * @return 0 when a new set of count most recent images has been retrieved. + * 1 if there was no new most recent image since the last call to this function + * or it is not possible to return count consecutive frames. * -1 on error. - * @note When this function is successful the buffers need to be returned to the camera driver before - * requesting new buffers. + * @note When this function is successful (return value 0) the buffers need to be returned to the camera driver before + * requesting new buffers. (use camera_img_stream_return_buffers) */ int camera_img_stream_get_buffers(camera_ctx *ctx, camera_image_buffer **buffers[], size_t count); @@ -146,6 +147,7 @@ void camera_img_stream_return_buffers(camera_ctx *ctx, camera_image_buffer **buf * @param img_param The image parameters for the snapshot image. * @param dst The destination buffer which should receive the image. * The buffer and buffer_size members must be correctly set. + * The pointed variable must remain valid until the snapshot capture is done. * @param cb Callback function which is called when the snapshot has been taken. * Note that the callback is called from the interrupt context and should only do minimal stuff * like setting a flag to notify the main loop. Calling camera_* functions is not allowed. @@ -193,7 +195,7 @@ struct _camera_sensor_interface { * @param buffer The buffer that contains the data of the transfer was completed. * @param size The size of the transfer. */ -typedef void (*camera_transport_transfer_done_cb)(void *usr, void *buffer, size_t size); +typedef void (*camera_transport_transfer_done_cb)(void *usr, const void *buffer, size_t size); /** * Callback for notifying the camera driver about a completed frame. @@ -225,9 +227,48 @@ struct _camera_transport_interface { * The camera driver context struct. */ struct _camera_ctx { - const camera_sensor_interface *sensor; - const camera_transport_interface *transport; + /* assets of the camera driver */ + const camera_sensor_interface *sensor; ///< Sensor interface. + const camera_transport_interface *transport; ///< Transport interface. + + /* image streaming buffer and parameters */ + + camera_img_param img_stream_param; ///< The parameters of the image streaming mode. + camera_image_buffer buffers[CAMERA_MAX_BUFFER_COUNT]; ///< The image buffers for image stream mode. + uint8_t avail_bufs[CAMERA_MAX_BUFFER_COUNT]; ///< Indexes to the buffers that are available. Ordered in the MRU order. + uint8_t avail_buf_count; ///< Number of buffer indexes in the avail_bufs array. + volatile bool new_frame_arrived; ///< Flag which is set by the interrupt handler to notify that a new frame has arrived. + + /* image snapshot buffer and parameters */ + + camera_img_param snapshot_param; ///< The parameters of the snapshot mode. + camera_image_buffer *snapshot_buffer; ///< Pointer to buffer which receives the snapshot. NULL when no snapshot is pending. + + /* retrieving buffers */ + + uint32_t last_read_frame_index; ///< The frame index of the last frame that has been read in streaming mode + + /* frame acquisition */ + + uint32_t cur_frame_index; + bool receiving_frame; + camera_image_buffer *target_buffer; + int target_buffer_index; + uint32_t cur_frame_size; + uint32_t cur_frame_pos; + + /* sequencing */ + + volatile bool seq_snapshot_active; /**< Flag that is asserted as long as a snapshot is not finished. + * While asserted the camera_img_stream_schedule_param_change function + * should not update the sensor. It should write the new parameters to the img_stream_param variable. */ + volatile bool seq_updating_img_stream; /**< Flag that must be set when snapshot is active and a write to the img_stream_param + * variable takes place. This is used by the interrupt to avoid switching the sensor back to normal + * mode while a parameter update takes place. */ + volatile bool seq_write_img_stream_param_yourself; /**< This flag is set by the interrupt handler when seq_snapshot_active and seq_updating_img_stream + * both where active while the interrupt handler attempted to switch the sensor back to streaming mode. + * in this case the parameter update function should do the parameter update of the sensor by itself. */ }; #endif /* CAMERA_H_ */ \ No newline at end of file diff --git a/src/camera.c b/src/camera.c index 5a0bfe8..7d69b30 100644 --- a/src/camera.c +++ b/src/camera.c @@ -33,10 +33,11 @@ ****************************************************************************/ #include "camera.h" +#include "timer.h" #include -void camera_transport_transfer_done_fn(void *usr, void *buffer, size_t size); +void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t size); void camera_transport_frame_done_fn(void *usr); bool camera_init(camera_ctx *ctx, camera_sensor_interface *sensor, camera_transport_interface *transport, @@ -45,18 +46,130 @@ bool camera_init(camera_ctx *ctx, camera_sensor_interface *sensor, camera_transp memset(ctx, 0, sizeof(camera_ctx)); ctx->sensor = sensor; ctx->transport = transport; + // check parameter: + if (buffer_count > CAMERA_MAX_BUFFER_COUNT && buffer_count < 2) { + return false; + } + uint32_t img_size = (uint32_t)img_param->size.x * (uint32_t)img_param->size.y; + if (img_size % ctx->transport->transfer_size != 0 || img_size / ctx->transport->transfer_size < 2) { + // invalid size parameter! + return false; + } // initialize state: + ctx->img_stream_param = *img_param; + int i; + for (i = 0; i < buffer_count; ++i) { + // check the buffer: + if (buffers[i].buffer_size < img_size || buffers[i].buffer == NULL) { + return false; + } + // init the buffer: + ctx->buffers[i] = buffers[i]; + ctx->buffers[i].frame_number = 0; + ctx->buffers[i].timestamp = get_boot_time_us(); + ctx->buffers[i].param = *img_param; + memset(ctx->buffers[i].buffer, 0, img_size); + // init the avail_bufs array: + ctx->avail_bufs[i] = i; + } + ctx->avail_buf_count = buffer_count; + ctx->new_frame_arrived = false; + ctx->snapshot_buffer = NULL; + + ctx->last_read_frame_index = 0; + + ctx->cur_frame_index = buffer_count + 1; + ctx->receiving_frame = false; + ctx->target_buffer = NULL; + + ctx->seq_snapshot_active = false; + ctx->seq_updating_img_stream = false; + ctx->seq_write_img_stream_param_yourself = false; // initialize hardware: - ctx->transport->init(ctx->transport->usr, - camera_transport_transfer_done_fn, - camera_transport_frame_done_fn, - ctx); - ctx->sensor->init(ctx->sensor->usr, img_param); + if (!ctx->transport->init(ctx->transport->usr, + camera_transport_transfer_done_fn, + camera_transport_frame_done_fn, + ctx)) { + return false; + } + if (!ctx->sensor->init(ctx->sensor->usr, img_param)) { + return false; + } + return true; +} + +static void uint32_t_memcpy(uint32_t *dst, const uint32_t *src, size_t count) { + int i; + for (i = 0; i < count; ++i) { + dst[i] = src[i]; + } +} + +void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t size) { + camera_ctx *ctx = (camera_ctx *)usr; + if (ctx->receiving_frame) { + // check if we have a target buffer: + if (ctx->target_buffer != NULL) { + uint32_t_memcpy((uint32_t *)((uint8_t *)ctx->target_buffer->buffer + ctx->cur_frame_pos), + (const uint32_t *)buffer, size / 4); + } + // update current position: + ctx->cur_frame_pos += size; + // check if we are done: + if (ctx->cur_frame_pos >= ctx->cur_frame_size) { + // frame done! + if (ctx->target_buffer_index >= 0) { + // put back into available buffers (in the front) + memmove(ctx->avail_bufs + 1, ctx->avail_bufs, ctx->avail_buf_count * sizeof(ctx->avail_bufs[0])); + ctx->avail_bufs[0] = ctx->target_buffer_index; + ctx->avail_buf_count += 1; + // notify camera_img_stream_get_buffers function. + ctx->new_frame_arrived = true; + } + // reset state: + ctx->target_buffer = NULL; + ctx->receiving_frame = false; + } + } else { + // no frame currently as the target frame. it must be the beginning of a new frame then! + // empty the DMA buffer as quickly as possible: + size_t size_w = size / 4; + uint32_t buf[size_w]; + uint32_t_memcpy(buf, (const uint32_t *)buffer, size_w); + // update the sensor: (this might trigger some I2C transfers) + ctx->sensor->notify_readout_start(ctx->sensor->usr); + // get current sensor parameter: + camera_img_param cparam; + ctx->sensor->get_current_param(ctx->sensor->usr, &cparam); + // update the receiving variables: + ctx->cur_frame_index += 1; + ctx->receiving_frame = true; + ctx->target_buffer = NULL; + ctx->cur_frame_size = (uint32_t)cparam.size.x * (uint32_t)cparam.size.y; + ctx->cur_frame_pos = size; + ctx->target_buffer_index = -1; + // check that the size parameters match: + if (cparam.size.x == ctx->img_stream_param.size.x || + cparam.size.y == ctx->img_stream_param.size.y) { + // get least recently used buffer from the available buffers: + ctx->target_buffer_index = ctx->avail_bufs[ctx->avail_buf_count - 1]; + ctx->avail_buf_count -= 1; + ctx->target_buffer = &ctx->buffers[ctx->target_buffer_index]; + // initialize it: + ctx->target_buffer->timestamp = get_boot_time_us(); + ctx->target_buffer->frame_number = ctx->cur_frame_index; + ctx->target_buffer->param = cparam; + // write data to it: (at position 0) + uint32_t_memcpy((uint32_t *)ctx->target_buffer->buffer, buf, size_w); + } + } +} + +void camera_transport_frame_done_fn(void *usr) { } bool camera_img_stream_schedule_param_change(camera_ctx *ctx, const camera_img_param *img_param) { - } int camera_img_stream_get_buffers(camera_ctx *ctx, camera_image_buffer **buffers[], size_t count) { @@ -68,15 +181,6 @@ void camera_img_stream_return_buffers(camera_ctx *ctx, camera_image_buffer **buf } bool camera_snapshot_schedule(camera_ctx *ctx, const camera_img_param *img_param, camera_image_buffer *dst, camera_snapshot_done_cb cb) { - } - -void camera_transport_transfer_done_fn(void *usr, void *buffer, size_t size) { - -} - -void camera_transport_frame_done_fn(void *usr) { - -} diff --git a/src/mt9v034.c b/src/mt9v034.c index 1545c88..7029f8e 100644 --- a/src/mt9v034.c +++ b/src/mt9v034.c @@ -90,13 +90,22 @@ const camera_sensor_interface mt9v034_sensor_interface = { }; struct _mt9v034_sensor_ctx { + /* context switching */ + volatile int cur_context; ///< The current camera sensor context that has been activated. volatile bool do_switch_context;///< When true the mt9v034_notify_readout_start function will switch the camera sensor context. volatile int desired_context; ///< The desired context when do_switch_context is true. + + camera_img_param context_p[2]; ///< The parameters of the camera sensor register sets. (for context A and B) + + /* mirrored registers of the sensor */ + uint16_t chip_control_reg; ///< The current chip control register value. + + /* current frame parameter simulation model */ + camera_img_param cur_param; ///< The parameters of the frame that is beeing read out. camera_img_param exp_param; ///< Because exposure parameters take one more frame time to propagate to the output this holds the exposure settings. - camera_img_param context_p[2]; }; bool mt9v034_init(void *usr, const camera_img_param *img_param) { From b037596518c626be68230df80d6c2ad295734b5e Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Wed, 5 Aug 2015 16:20:56 +0200 Subject: [PATCH 046/120] finished camera_transport_interface for the DCMI module. code is now almost ready to test. --- inc/camera.h | 2 + inc/dcmi.h | 36 +---- inc/mt9v034.h | 39 +---- src/camera.c | 2 +- src/dcmi.c | 422 ++++++++++---------------------------------------- src/main.c | 97 +++--------- src/mt9v034.c | 27 ++-- 7 files changed, 135 insertions(+), 490 deletions(-) diff --git a/inc/camera.h b/inc/camera.h index b9f0a80..3676cde 100644 --- a/inc/camera.h +++ b/inc/camera.h @@ -75,6 +75,8 @@ typedef struct _camera_image_buffer { uint32_t meta; ///< Buffer meta data not used by the camera driver. This can be used to identify buffers. } camera_image_buffer; +#define BuildCameraImageBuffer(memory_variable) ((camera_image_buffer){.buffer = &memory_variable, .buffer_size = sizeof(memory_variable)}) + /** * Callback which is called when a snapshot capture has finished. * @note This callback may be called from an interrupt handler. diff --git a/inc/dcmi.h b/inc/dcmi.h index 303b6c2..0cf0017 100644 --- a/inc/dcmi.h +++ b/inc/dcmi.h @@ -35,44 +35,16 @@ #define DCMI_H_ #include -#include "mt9v034.h" +#include "camera.h" -#define DCMI_DR_ADDRESS 0x50050028 - -/** - * @brief Copy image to fast RAM address - */ -int dma_copy_image_buffers(uint8_t ** current_image, uint8_t ** previous_image, uint16_t buffer_size, uint8_t image_step); - -/** - * @brief Send calibration image with MAVLINK over USB - */ -void send_calibration_image(uint8_t ** image_buffer_fast_1, uint8_t ** image_buffer_fast_2); +#define CONFIG_DCMI_DMA_BUFFER_SIZE (1024u) -/** - * @brief Initialize DCMI DMA and enable image capturing - */ -void enable_image_capture(void); - -/* Init Functions */ -void dcmi_clock_init(void); -void dcmi_hw_init(void); -void dcmi_dma_init(uint16_t buffer_size); -void dcmi_it_init(void); -void dma_it_init(void); +const camera_transport_interface *dcmi_get_transport_interface(); /* Interrupt Handlers */ void DCMI_IRQHandler(void); void DMA2_Stream1_IRQHandler(void); -void dcmi_dma_enable(void); -void dcmi_dma_disable(void); -void dma_reconfigure(void); -void dcmi_restart_calibration_routine(void); -void dma_swap_buffers(void); - -uint32_t get_time_between_images(void); -uint32_t get_frame_counter(void); -void reset_frame_counter(void); +#define DCMI_DR_ADDRESS 0x50050028 #endif /* DCMI_H_ */ diff --git a/inc/mt9v034.h b/inc/mt9v034.h index ca25032..b418260 100644 --- a/inc/mt9v034.h +++ b/inc/mt9v034.h @@ -38,6 +38,12 @@ #include "settings.h" #include "camera.h" +/** + * Returns the sensor interface that can be passed to the camera driver. + * @return The initialized sensor interface struct. + */ +const camera_sensor_interface *mt9v034_get_sensor_interface(); + /** * Configures the maximum exposure time in number of image rows. * Exposure should not affect frame time. @@ -54,16 +60,6 @@ /* Constants */ #define TIMEOUT_MAX 10000 -#define BINNING_ROW_A 4 -#define BINNING_COLUMN_A 4 -#define BINNING_ROW_B 2 -#define BINNING_COLUMN_B 2 -#define MINIMUM_HORIZONTAL_BLANKING 91 // see datasheet -#define MAX_IMAGE_HEIGHT 480 -#define MAX_IMAGE_WIDTH 752 -#define MINIMUM_COLUMN_START 1 -#define MINIMUM_ROW_START 4 - /* Camera I2C registers */ #define mt9v034_DEVICE_WRITE_ADDRESS 0xB8 #define mt9v034_DEVICE_READ_ADDRESS 0xB9 @@ -145,27 +141,4 @@ #define MTV_AEC_EXPOSURE_REG 0xBB #define MTV_AGC_AEC_CUR_BIN_REG 0xBC - -/* - * Resolution: - * ROW_SIZE * BINNING_ROW <= MAX_IMAGE_WIDTH - * COLUMN_SIZE * BINNING_COLUMN <= MAX_IMAGE_HEIGHT - */ - -#define FULL_IMAGE_SIZE (188*120) -#define FULL_IMAGE_ROW_SIZE (188) -#define FULL_IMAGE_COLUMN_SIZE (120) - -/* Functions */ - -/** - * @brief Configures the mt9v034 camera with two context (binning 4 and binning 2). - */ -void mt9v034_context_configuration(void); - -/** - * @brief Changes sensor context based on settings - */ -void mt9v034_set_context(void); - #endif /* MT9V34_H_ */ diff --git a/src/camera.c b/src/camera.c index 7d69b30..fc9dc7e 100644 --- a/src/camera.c +++ b/src/camera.c @@ -40,7 +40,7 @@ void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t size); void camera_transport_frame_done_fn(void *usr); -bool camera_init(camera_ctx *ctx, camera_sensor_interface *sensor, camera_transport_interface *transport, +bool camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const camera_transport_interface *transport, const camera_img_param *img_param, camera_image_buffer buffers[], size_t buffer_count) { memset(ctx, 0, sizeof(camera_ctx)); diff --git a/src/dcmi.c b/src/dcmi.c index 95337dd..58e046e 100644 --- a/src/dcmi.c +++ b/src/dcmi.c @@ -49,313 +49,108 @@ #include "misc.h" #include "stm32f4xx.h" -/* counters */ -volatile uint8_t image_counter = 0; -volatile uint32_t frame_counter; -volatile uint32_t time_last_frame = 0; -volatile uint32_t cycle_time = 0; -volatile uint32_t time_between_next_images; -volatile uint8_t dcmi_calibration_counter = 0; - -/* state variables */ -volatile uint8_t dcmi_image_buffer_memory0 = 1; -volatile uint8_t dcmi_image_buffer_memory1 = 2; -volatile uint8_t dcmi_image_buffer_unused = 3; -volatile uint8_t calibration_used; -volatile uint8_t calibration_unused; -volatile uint8_t calibration_mem0; -volatile uint8_t calibration_mem1; - -/* image buffers */ -uint8_t dcmi_image_buffer_8bit_1[FULL_IMAGE_SIZE]; -uint8_t dcmi_image_buffer_8bit_2[FULL_IMAGE_SIZE]; -uint8_t dcmi_image_buffer_8bit_3[FULL_IMAGE_SIZE]; - -uint32_t time_between_images; +struct _dcmi_transport_ctx; +typedef struct _dcmi_transport_ctx dcmi_transport_ctx; -/** - * @brief Initialize DCMI DMA and enable image capturing - */ -void enable_image_capture(void) -{ - dcmi_clock_init(); - dcmi_hw_init(); - dcmi_dma_init(global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]); - mt9v034_context_configuration(); - dcmi_dma_enable(); -} +/* DMA buffers */ +uint8_t dcmi_dma_buffer_1[CONFIG_DCMI_DMA_BUFFER_SIZE]; +uint8_t dcmi_dma_buffer_2[CONFIG_DCMI_DMA_BUFFER_SIZE]; /** - * @brief DMA reconfiguration after changing image window + * @brief HW initialization of DCMI clock */ -void dma_reconfigure(void) -{ - dcmi_dma_disable(); - - if(global_data.param[PARAM_VIDEO_ONLY]) - dcmi_dma_init(FULL_IMAGE_SIZE); - else - dcmi_dma_init(global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]); - - dcmi_dma_enable(); -} +static void dcmi_clock_init(); /** - * @brief Calibration image collection routine restart + * @brief HW initialization DCMI */ -void dcmi_restart_calibration_routine(void) -{ - /* wait until we have all 4 parts of image */ - while(frame_counter < 4){} - frame_counter = 0; - dcmi_dma_enable(); -} - +static void dcmi_hw_init(void); /** - * @brief Interrupt handler of DCMI + * @brief Configures DCMI and DMA to capture image from the camera. */ -void DCMI_IRQHandler(void) -{ - if (DCMI_GetITStatus(DCMI_IT_FRAME) != RESET) - { - DCMI_ClearITPendingBit(DCMI_IT_FRAME); - } - - return; -} - +static void dcmi_dma_init(); /** - * @brief Interrupt handler of DCMI DMA stream + * @brief Enable DCMI and DMA stream */ -void DMA2_Stream1_IRQHandler(void) -{ - /* transfer completed */ - if (DMA_GetITStatus(DMA2_Stream1, DMA_IT_TCIF1) != RESET) - { - DMA_ClearITPendingBit(DMA2_Stream1, DMA_IT_TCIF1); - frame_counter++; - - if(global_data.param[PARAM_VIDEO_ONLY]) - { - if (frame_counter >= 4) - { - dcmi_dma_disable(); - calibration_used = DMA_GetCurrentMemoryTarget(DMA2_Stream1); - calibration_unused = dcmi_image_buffer_unused; - calibration_mem0 = dcmi_image_buffer_memory0; - calibration_mem1 = dcmi_image_buffer_memory1; - } - } - - return; - } - - /* transfer half completed - * - * We use three buffers and switch the buffers if dma transfer - * is in half state. - */ - if (DMA_GetITStatus(DMA2_Stream1, DMA_IT_HTIF1) != RESET) - { - DMA_ClearITPendingBit(DMA2_Stream1, DMA_IT_HTIF1); - } - - dma_swap_buffers(); -} - +static void dcmi_dma_enable(); /** - * @brief Swap DMA image buffer addresses + * @brief Initialize/Enable DCMI Interrupt */ -void dma_swap_buffers(void) -{ - /* check which buffer is in use */ - if (DMA_GetCurrentMemoryTarget(DMA2_Stream1)) - { - /* swap dcmi image buffer */ - if (dcmi_image_buffer_unused == 1) - DMA_MemoryTargetConfig(DMA2_Stream1, (uint32_t) dcmi_image_buffer_8bit_1, DMA_Memory_0); - else if (dcmi_image_buffer_unused == 2) - DMA_MemoryTargetConfig(DMA2_Stream1, (uint32_t) dcmi_image_buffer_8bit_2, DMA_Memory_0); - else - DMA_MemoryTargetConfig(DMA2_Stream1, (uint32_t) dcmi_image_buffer_8bit_3, DMA_Memory_0); - - int tmp_buffer = dcmi_image_buffer_memory0; - dcmi_image_buffer_memory0 = dcmi_image_buffer_unused; - dcmi_image_buffer_unused = tmp_buffer; - } - else - { - /* swap dcmi image buffer */ - if (dcmi_image_buffer_unused == 1) - DMA_MemoryTargetConfig(DMA2_Stream1, (uint32_t) dcmi_image_buffer_8bit_1, DMA_Memory_1); - else if (dcmi_image_buffer_unused == 2) - DMA_MemoryTargetConfig(DMA2_Stream1, (uint32_t) dcmi_image_buffer_8bit_2, DMA_Memory_1); - else - DMA_MemoryTargetConfig(DMA2_Stream1, (uint32_t) dcmi_image_buffer_8bit_3, DMA_Memory_1); - - int tmp_buffer = dcmi_image_buffer_memory1; - dcmi_image_buffer_memory1 = dcmi_image_buffer_unused; - dcmi_image_buffer_unused = tmp_buffer; - } - - /* set next time_between_images */ - cycle_time = get_time_delta_us(time_last_frame); - time_last_frame += cycle_time; +static void dcmi_dma_it_init(); - if(image_counter) // image was not fetched jet - { - time_between_next_images = time_between_next_images + cycle_time; - } - else - { - time_between_next_images = cycle_time; - } +struct _dcmi_transport_ctx { + camera_transport_transfer_done_cb transfer_done_cb; + camera_transport_frame_done_cb frame_done_cb; + void *cb_usr; +}; - /* set new image true and increment frame counter*/ - image_counter += 1; +static dcmi_transport_ctx dcmi_ctx; - return; -} +const camera_transport_interface dcmi_transport_interface = { + .usr = &dcmi_ctx, + .transfer_size = CONFIG_DCMI_DMA_BUFFER_SIZE, + .init = dcmi_init, +}; -uint32_t get_time_between_images(void){ - return time_between_images; +const camera_transport_interface *dcmi_get_transport_interface() { + return &dcmi_transport_interface; } -uint32_t get_frame_counter(void){ - return frame_counter; +bool dcmi_init(void *usr, + camera_transport_transfer_done_cb transfer_done_cb, + camera_transport_frame_done_cb frame_done_cb, + void *cb_usr) { + dcmi_transport_ctx *ctx = (dcmi_transport_ctx *)usr; + memset(ctx, 0, sizeof(dcmi_transport_ctx)); + /* init assets: */ + ctx->transfer_done_cb = transfer_done_cb; + ctx->frame_done_cb = frame_done_cb; + ctx->cb_usr = cb_usr; + /* initialize hardware: */ + dcmi_clock_init(); + dcmi_hw_init(); + dcmi_dma_init(); + dcmi_dma_enable(); + dcmi_dma_it_init(); } /** - * @brief Copy image to fast RAM address - * - * @param current_image Current image buffer - * @param previous_image Previous image buffer - * @param image_size Image size of the image to copy - * @param image_step Image to wait for (if 1 no waiting) - * @return the number of skipped images. + * @brief Interrupt handler of DCMI */ -int dma_copy_image_buffers(uint8_t ** current_image, uint8_t ** previous_image, uint16_t image_size, uint8_t image_step){ - - /* swap image buffers */ - uint8_t * tmp_image = *current_image; - *current_image = *previous_image; - *previous_image = tmp_image; - - /* wait for new image if needed */ - while(image_counter < image_step) {} - int img_ctr_val = image_counter; - image_counter -= img_ctr_val; - - /* time between images */ - time_between_images = time_between_next_images; - - /* copy image */ - if (dcmi_image_buffer_unused == 1) - { - for (uint16_t pixel = 0; pixel < image_size; pixel++) - (*current_image)[pixel] = (uint8_t)(dcmi_image_buffer_8bit_1[pixel]); - } - else if (dcmi_image_buffer_unused == 2) - { - for (uint16_t pixel = 0; pixel < image_size; pixel++) - (*current_image)[pixel] = (uint8_t)(dcmi_image_buffer_8bit_2[pixel]); - } - else - { - for (uint16_t pixel = 0; pixel < image_size; pixel++) - (*current_image)[pixel] = (uint8_t)(dcmi_image_buffer_8bit_3[pixel]); +void DCMI_IRQHandler(void) { + if (DCMI_GetITStatus(DCMI_IT_FRAME) != RESET) { + DCMI_ClearITPendingBit(DCMI_IT_FRAME); + /* get context: */ + dcmi_transport_ctx *ctx = &dcmi_ctx; + /* typedef void (*camera_transport_frame_done_cb)(void *usr); */ + ctx->frame_done_cb(ctx->cb_usr); } - return img_ctr_val - image_step; } /** - * @brief Send calibration image with MAVLINK over USB - * - * @param image_buffer_fast_1 Image buffer in fast RAM - * @param image_buffer_fast_2 Image buffer in fast RAM + * @brief Interrupt handler of DCMI DMA stream */ -void send_calibration_image(uint8_t ** image_buffer_fast_1, uint8_t ** image_buffer_fast_2) { - - /* transmit raw 8-bit image */ - /* TODO image is too large for this transmission protocol (too much packets), but it works */ - mavlink_msg_data_transmission_handshake_send( - MAVLINK_COMM_2, - MAVLINK_DATA_STREAM_IMG_RAW8U, - FULL_IMAGE_SIZE * 4, - FULL_IMAGE_ROW_SIZE * 2, - FULL_IMAGE_COLUMN_SIZE * 2, - FULL_IMAGE_SIZE * 4 / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1, - MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN, - 100); - - uint16_t frame = 0; - uint8_t image = 0; - uint8_t frame_buffer[MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN]; - - for (int i = 0; i < FULL_IMAGE_SIZE * 4; i++) +void DMA2_Stream1_IRQHandler(void) +{ + /* transfer completed */ + if (DMA_GetITStatus(DMA2_Stream1, DMA_IT_TCIF1) != RESET) { - - if (i % FULL_IMAGE_SIZE == 0 && i != 0) - { - image++; - } - - if (i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN == 0 && i != 0) - { - mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, frame_buffer); - frame++; - delay(2); - } - - if (image == 0 ) - { - frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = (uint8_t)(*image_buffer_fast_1)[i % FULL_IMAGE_SIZE]; - } - else if (image == 1 ) - { - frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = (uint8_t)(*image_buffer_fast_2)[i % FULL_IMAGE_SIZE]; - } - else if (image == 2) - { - if (calibration_unused == 1) - frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_1[i % FULL_IMAGE_SIZE]; - else if (calibration_unused == 2) - frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_2[i % FULL_IMAGE_SIZE]; - else - frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_3[i % FULL_IMAGE_SIZE]; - } - else - { - if (calibration_used) - { - if (calibration_mem0 == 1) - frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_1[i % FULL_IMAGE_SIZE]; - else if (calibration_mem0 == 2) - frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_2[i % FULL_IMAGE_SIZE]; - else - frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_3[i % FULL_IMAGE_SIZE]; - } - else - { - if (calibration_mem1 == 1) - frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_1[i % FULL_IMAGE_SIZE]; - else if (calibration_mem1 == 2) - frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_2[i % FULL_IMAGE_SIZE]; - else - frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_3[i % FULL_IMAGE_SIZE]; - } + DMA_ClearITPendingBit(DMA2_Stream1, DMA_IT_TCIF1); + /* get the buffer that has been completed: */ + void *buffer; + if (DMA_GetCurrentMemoryTarget(DMA2_Stream1)) { + buffer = dcmi_dma_buffer_1; + } else { + buffer = dcmi_dma_buffer_2; } + /* get context: */ + dcmi_transport_ctx *ctx = &dcmi_ctx; + /* typedef void (*camera_transport_transfer_done_cb)(void *usr, const void *buffer, size_t size); */ + ctx->transfer_done_cb(ctx->cb_usr, buffer, CONFIG_DCMI_DMA_BUFFER_SIZE); } - - mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, frame_buffer); - } -/** - * @brief Initialize/Enable DCMI Interrupt - */ -void dcmi_it_init() -{ +static void dcmi_dma_it_init() { NVIC_InitTypeDef NVIC_InitStructure; /* Enable the DCMI global Interrupt */ @@ -365,16 +160,8 @@ void dcmi_it_init() NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); - DCMI_ITConfig(DCMI_IT_FRAME,ENABLE); -} - -/** - * @brief Initialize/Enable DMA Interrupt - */ -void dma_it_init() -{ - NVIC_InitTypeDef NVIC_InitStructure; - + DCMI_ITConfig(DCMI_IT_FRAME, ENABLE); + /* Enable the DMA global Interrupt */ NVIC_InitStructure.NVIC_IRQChannel = DMA2_Stream1_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 5; @@ -382,43 +169,17 @@ void dma_it_init() NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); - DMA_ITConfig(DMA2_Stream1, DMA_IT_HT, ENABLE); // half transfer interrupt DMA_ITConfig(DMA2_Stream1, DMA_IT_TC, ENABLE); // transfer complete interrupt } -/** - * @brief Enable DCMI DMA stream - */ -void dcmi_dma_enable() -{ +static void dcmi_dma_enable() { /* Enable DMA2 stream 1 and DCMI interface then start image capture */ DMA_Cmd(DMA2_Stream1, ENABLE); DCMI_Cmd(ENABLE); DCMI_CaptureCmd(ENABLE); - dma_it_init(); -} - -/** - * @brief Disable DCMI DMA stream - */ -void dcmi_dma_disable() -{ - /* Disable DMA2 stream 1 and DCMI interface then stop image capture */ - DMA_Cmd(DMA2_Stream1, DISABLE); - DCMI_Cmd(DISABLE); - DCMI_CaptureCmd(DISABLE); } -void reset_frame_counter() -{ - frame_counter = 0; -} - -/** - * @brief HW initialization of DCMI clock - */ -void dcmi_clock_init() -{ +static void dcmi_clock_init() { GPIO_InitTypeDef GPIO_InitStructure; TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_OCInitTypeDef TIM_OCInitStructure; @@ -464,20 +225,8 @@ void dcmi_clock_init() TIM_Cmd(TIM3, ENABLE); } -/** - * @brief HW initialization DCMI - */ -void dcmi_hw_init(void) -{ - uint16_t image_size = global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; - - /* Reset image buffers */ - for (int i = 0; i < image_size; i++) { - dcmi_image_buffer_8bit_1 [i] = 0; - dcmi_image_buffer_8bit_2 [i] = 0; - dcmi_image_buffer_8bit_3 [i] = 0; - } - +static void dcmi_hw_init(void) { + GPIO_InitTypeDef GPIO_InitStructure; /*** Configures the DCMI GPIOs to interface with the OV2640 camera module ***/ /* Enable DCMI GPIOs clocks */ RCC_AHB1PeriphClockCmd( @@ -520,22 +269,13 @@ void dcmi_hw_init(void) GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_6; GPIO_Init(GPIOE, &GPIO_InitStructure); - } -/** - * @brief Configures DCMI/DMA to capture image from the mt9v034 camera. - * - * @param buffer_size Buffer size in bytes - */ -void dcmi_dma_init(uint16_t buffer_size) -{ - reset_frame_counter(); - +static void dcmi_dma_init() { DCMI_InitTypeDef DCMI_InitStructure; DMA_InitTypeDef DMA_InitStructure; - /*** Configures the DCMI to interface with the mt9v034 camera module ***/ + /*** Configures the DCMI to interface with the camera module ***/ /* Enable DCMI clock */ RCC_AHB2PeriphClockCmd(RCC_AHB2Periph_DCMI, ENABLE); @@ -557,9 +297,9 @@ void dcmi_dma_init(uint16_t buffer_size) DMA_InitStructure.DMA_Channel = DMA_Channel_1; DMA_InitStructure.DMA_PeripheralBaseAddr = DCMI_DR_ADDRESS; - DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t) dcmi_image_buffer_8bit_1; + DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)dcmi_dma_buffer_1; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory; - DMA_InitStructure.DMA_BufferSize = buffer_size / 4; // buffer size in date unit (word) + DMA_InitStructure.DMA_BufferSize = CONFIG_DCMI_DMA_BUFFER_SIZE / 4; // buffer size in date unit (word) DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word; @@ -571,8 +311,8 @@ void dcmi_dma_init(uint16_t buffer_size) DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single; DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; - DMA_DoubleBufferModeConfig(DMA2_Stream1,(uint32_t) dcmi_image_buffer_8bit_2, DMA_Memory_0); - DMA_DoubleBufferModeCmd(DMA2_Stream1,ENABLE); + DMA_DoubleBufferModeConfig(DMA2_Stream1, (uint32_t)dcmi_dma_buffer_2, DMA_Memory_0); + DMA_DoubleBufferModeCmd(DMA2_Stream1, ENABLE); /* DCMI configuration */ DCMI_Init(&DCMI_InitStructure); diff --git a/src/main.c b/src/main.c index 7db7a3d..b14d3ac 100644 --- a/src/main.c +++ b/src/main.c @@ -5,6 +5,7 @@ * Dominik Honegger * Petri Tanskanen * Samuel Zihlmann + * Simon Laube * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -72,25 +73,14 @@ #endif -/* prototypes */ -void buffer_reset(void); - __ALIGN_BEGIN USB_OTG_CORE_HANDLE USB_OTG_dev __ALIGN_END; -/* fast image buffers for calculations */ -uint8_t* image_buffer_8bit_1 = ((uint8_t*) 0x10000000); -uint8_t* image_buffer_8bit_2 = ((uint8_t*) ( 0x10000000 | FULL_IMAGE_SIZE )); -uint8_t buffer_reset_needed; - /* timer constants */ #define SONAR_POLL_MS 100 /* steps in milliseconds ticks */ #define SYSTEM_STATE_MS 1000/* steps in milliseconds ticks */ #define PARAMS_MS 100 /* steps in milliseconds ticks */ #define LPOS_TIMER_COUNT 100 /* steps in milliseconds ticks */ -static uint8_t *current_image; -static uint8_t *previous_image; - /* local position estimate without orientation, useful for unit testing w/o FMU */ struct lpos_t { float x; @@ -101,7 +91,6 @@ struct lpos_t { float vz; } lpos = {0}; - void sonar_update_fn(void) { sonar_trigger(); } @@ -165,46 +154,19 @@ void send_params_fn(void) { communication_parameter_send(); } -void buffer_reset(void) { - buffer_reset_needed = 1; -} - -void enter_image_calibration_mode() { - while(global_data.param[PARAM_VIDEO_ONLY]) - { - dcmi_restart_calibration_routine(); - - /* waiting for first quarter of image */ - while(get_frame_counter() < 2){} - dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1); - - /* waiting for second quarter of image */ - while(get_frame_counter() < 3){} - dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1); - - /* waiting for all image parts */ - while(get_frame_counter() < 4){} - - send_calibration_image(&previous_image, ¤t_image); - - /* check timers */ - timer_check(); - - LEDToggle(LED_COM); - } +#define FLOW_IMAGE_SIZE (64) - dcmi_restart_calibration_routine(); - LEDOff(LED_COM); -} +uint8_t image_buffer_8bit_1[FLOW_IMAGE_SIZE * FLOW_IMAGE_SIZE] __attribute__((section(".ccm"))); +uint8_t image_buffer_8bit_2[FLOW_IMAGE_SIZE * FLOW_IMAGE_SIZE] __attribute__((section(".ccm"))); +uint8_t image_buffer_8bit_3[FLOW_IMAGE_SIZE * FLOW_IMAGE_SIZE] __attribute__((section(".ccm"))); +uint8_t image_buffer_8bit_4[FLOW_IMAGE_SIZE * FLOW_IMAGE_SIZE] __attribute__((section(".ccm"))); +uint8_t image_buffer_8bit_5[FLOW_IMAGE_SIZE * FLOW_IMAGE_SIZE] __attribute__((section(".ccm"))); /** * @brief Main function. */ int main(void) { - current_image = image_buffer_8bit_1; - previous_image = image_buffer_8bit_2; - /* load settings and parameters */ global_data_reset_param_defaults(); global_data_reset(); @@ -233,19 +195,27 @@ int main(void) /* init mavlink */ communication_init(); - /* enable image capturing */ - enable_image_capture(); + /* initialize camera: */ + camera_ctx cam_ctx; + camera_img_param img_stream_param; + img_stream_param.size.x = FLOW_IMAGE_SIZE; + img_stream_param.size.y = FLOW_IMAGE_SIZE; + img_stream_param.binning = 4; + { + camera_image_buffer buffers[5] = { + BuildCameraImageBuffer(image_buffer_8bit_1), + BuildCameraImageBuffer(image_buffer_8bit_2), + BuildCameraImageBuffer(image_buffer_8bit_3), + BuildCameraImageBuffer(image_buffer_8bit_4), + BuildCameraImageBuffer(image_buffer_8bit_5) + }; + camera_init(&cam_ctx, mt9v034_get_sensor_interface(), dcmi_get_transport_interface(), + &img_stream_param, buffers, 5); + } /* gyro config */ gyro_config(); - /* init and clear fast image buffers */ - for (int i = 0; i < global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; i++) - { - image_buffer_8bit_1[i] = 0; - image_buffer_8bit_2[i] = 0; - } - /* usart config*/ usart_init(); @@ -281,25 +251,6 @@ int main(void) /* check timers */ timer_check(); - /* reset flow buffers if needed */ - if(buffer_reset_needed) - { - buffer_reset_needed = 0; - if(!global_data.param[PARAM_VIDEO_ONLY]) { - /* get two new fresh images: */ - dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1); - dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1); - } - continue; - } - - /* calibration routine */ - if(global_data.param[PARAM_VIDEO_ONLY]) - { - enter_image_calibration_mode(); - continue; - } - uint16_t image_size = global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; /* calculate focal_length in pixel */ diff --git a/src/mt9v034.c b/src/mt9v034.c index 7029f8e..05f47a5 100644 --- a/src/mt9v034.c +++ b/src/mt9v034.c @@ -79,16 +79,6 @@ static uint8_t mt9v034_ReadReg(uint16_t Addr); */ static uint8_t mt9v034_WriteReg(uint16_t Addr, uint8_t Data); -static mt9v034_sensor_ctx mt9v034_ctx; - -const camera_sensor_interface mt9v034_sensor_interface = { - .usr = &mt9v034_ctx, - .init = mt9v034_init, - .prepare_update_param = mt9v034_prepare_update_param, - .notify_readout_start = mt9v034_notify_readout_start, - .get_current_param = mt9v034_get_current_param -}; - struct _mt9v034_sensor_ctx { /* context switching */ @@ -108,13 +98,30 @@ struct _mt9v034_sensor_ctx { camera_img_param exp_param; ///< Because exposure parameters take one more frame time to propagate to the output this holds the exposure settings. }; +static mt9v034_sensor_ctx mt9v034_ctx; + +const camera_sensor_interface mt9v034_sensor_interface = { + .usr = &mt9v034_ctx, + .init = mt9v034_init, + .prepare_update_param = mt9v034_prepare_update_param, + .notify_readout_start = mt9v034_notify_readout_start, + .get_current_param = mt9v034_get_current_param +}; + +const camera_sensor_interface *mt9v034_get_sensor_interface() { + return &mt9v034_sensor_interface; +} + bool mt9v034_init(void *usr, const camera_img_param *img_param) { mt9v034_sensor_ctx *ctx = (mt9v034_sensor_ctx *)usr; + /* init context: */ memset(ctx, 0, sizeof(mt9v034_sensor_ctx)); ctx->cur_context = 0; ctx->desired_context = 0; ctx->do_switch_context = false; + /* init hardware: */ if (!mt9v034_init_hw(ctx)) return false; + /* configure sensor: */ mt9v034_configure_context(ctx, 0, img_param, true); mt9v034_configure_context(ctx, 1, img_param, true); mt9v034_configure_general(ctx); From f4b010d407e462c095804e74a8860f93bd71a58a Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Wed, 5 Aug 2015 18:04:45 +0200 Subject: [PATCH 047/120] - updated main to use new API. - changed KLT flow to use its own image struct which stores the preprocessed data. - implemented buffer management in camera. --- inc/camera.h | 17 ++++--- inc/flow.h | 14 ++++-- src/camera.c | 122 +++++++++++++++++++++++++++++++++++++++++++++++---- src/flow.c | 42 ++++++++++-------- src/main.c | 91 ++++++++++++++++++++++++++++---------- 5 files changed, 226 insertions(+), 60 deletions(-) diff --git a/inc/camera.h b/inc/camera.h index 3676cde..ef9cc5f 100644 --- a/inc/camera.h +++ b/inc/camera.h @@ -116,7 +116,7 @@ bool camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const c bool camera_img_stream_schedule_param_change(camera_ctx *ctx, const camera_img_param *img_param); /** - * Gets the most recent images. If no more recent image is available this function returns immediatly without doing anything. + * Gets the most recent images. If no more recent image is available this function returns immediately without doing anything. * @param ctx The context to use. * @param buffers Pointer to an array of buffer pointers which are updated to point to the most recent buffers. * The first buffer pointer will be updated to the most recent image followed by the next @@ -124,14 +124,14 @@ bool camera_img_stream_schedule_param_change(camera_ctx *ctx, const camera_img_p * It is guaranteed to retrieve consecutive images. * @param count Number of most recent images to retrieve. Must be lower than buffer_count - 1 which was * passed to camera_init. - * @return 0 when a new set of count most recent images has been retrieved. - * 1 if there was no new most recent image since the last call to this function - * or it is not possible to return count consecutive frames. + * @param wait_for_new When true the function will wait until the the requested images are available. + * @return 0 when a set of count consecutive most recent images have been retrieved. + * 1 it is not possible to return count consecutive frames. you need to wait until the frames are captured. * -1 on error. * @note When this function is successful (return value 0) the buffers need to be returned to the camera driver before * requesting new buffers. (use camera_img_stream_return_buffers) */ -int camera_img_stream_get_buffers(camera_ctx *ctx, camera_image_buffer **buffers[], size_t count); +int camera_img_stream_get_buffers(camera_ctx *ctx, camera_image_buffer **buffers[], size_t count, bool wait_for_new); /** * Returns the buffers that have been retrieved by camera_img_stream_get_buffers back to the camera driver. @@ -238,8 +238,10 @@ struct _camera_ctx { camera_img_param img_stream_param; ///< The parameters of the image streaming mode. camera_image_buffer buffers[CAMERA_MAX_BUFFER_COUNT]; ///< The image buffers for image stream mode. - uint8_t avail_bufs[CAMERA_MAX_BUFFER_COUNT]; ///< Indexes to the buffers that are available. Ordered in the MRU order. - uint8_t avail_buf_count; ///< Number of buffer indexes in the avail_bufs array. + int buffer_count; ///< Total number of buffers. + volatile uint8_t avail_bufs[CAMERA_MAX_BUFFER_COUNT]; ///< Indexes to the buffers that are available. Ordered in the MRU order. + volatile uint8_t avail_buf_count; ///< Number of buffer indexes in the avail_bufs array. + volatile uint8_t put_back_buf_pos; ///< Position where to put back the reserved buffers. volatile bool new_frame_arrived; ///< Flag which is set by the interrupt handler to notify that a new frame has arrived. /* image snapshot buffer and parameters */ @@ -249,6 +251,7 @@ struct _camera_ctx { /* retrieving buffers */ + volatile bool buffers_are_reserved; ///< True when buffers have been reserved by the camera_img_stream_get_buffers function and need to be returned. uint32_t last_read_frame_index; ///< The frame index of the last frame that has been read in streaming mode /* frame acquisition */ diff --git a/inc/flow.h b/inc/flow.h index 0d5cb93..e867639 100644 --- a/inc/flow.h +++ b/inc/flow.h @@ -36,6 +36,8 @@ #include +#define FLOW_FRAME_SIZE 64 + typedef struct _flow_raw_result { float x; ///< The flow in x direction float y; ///< The flow in y direction @@ -44,6 +46,12 @@ typedef struct _flow_raw_result { uint8_t at_y; ///< The mid-position of the patch that was used to calculate the flow. } flow_raw_result; +typedef struct _flow_klt_image { + uint8_t *image; + uint8_t preprocessed[(FLOW_FRAME_SIZE * FLOW_FRAME_SIZE) / 2]; + uint32_t meta; +} flow_klt_image; + /** * @brief Computes pixel flow from image1 to image2 * Searches the corresponding position in the new image (image2) of max. 64 pixels from the old image (image1). @@ -62,9 +70,9 @@ uint16_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_ra /** * Preprocesses the image for use with compute_klt. * This will add the pyramid levels. - * @note Image buffer needs to be at least twice as big as the original image. + * The pointed memory needs to be in CCM. */ -void klt_preprocess_image(uint8_t *image); +void klt_preprocess_image(uint8_t *image, flow_klt_image *klt_image); /** * @brief Computes pixel flow from image1 to image2 @@ -80,7 +88,7 @@ void klt_preprocess_image(uint8_t *image); * @param result_count The available space in the out buffer. * @return The number of results written to the out buffer. */ -uint16_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate, +uint16_t compute_klt(flow_klt_image *image1, flow_klt_image *image2, float x_rate, float y_rate, float z_rate, flow_raw_result *out, uint16_t max_out); /** diff --git a/src/camera.c b/src/camera.c index fc9dc7e..9d45dab 100644 --- a/src/camera.c +++ b/src/camera.c @@ -72,6 +72,7 @@ bool camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const c // init the avail_bufs array: ctx->avail_bufs[i] = i; } + ctx->buffer_count = buffer_count; ctx->avail_buf_count = buffer_count; ctx->new_frame_arrived = false; @@ -106,6 +107,46 @@ static void uint32_t_memcpy(uint32_t *dst, const uint32_t *src, size_t count) { } } +static void camera_buffer_fifo_remove_front(camera_ctx *ctx, int *out, size_t count) { + size_t bc = ctx->avail_buf_count; + size_t i; + // read out: + for (i = 0; i < count; ++i) { + *out++ = ctx->avail_bufs[i]; + } + // close gap: + for (i = count; i < bc; ++i) { + ctx->avail_bufs[i - count] = ctx->avail_bufs[i]; + } + ctx->avail_buf_count = bc - count; +} + +static void camera_buffer_fifo_remove_back(camera_ctx *ctx, int *out, size_t count) { + size_t bc = ctx->avail_buf_count; + size_t i; + // read out: + for (i = bc - count; i < bc; ++i) { + *out++ = ctx->avail_bufs[i]; + } + // reduce count: + ctx->avail_buf_count = bc - count; +} + +static void camera_buffer_fifo_push_at(camera_ctx *ctx, size_t pos, const int *in, size_t count) { + size_t bc = ctx->avail_buf_count; + size_t i; + // move away: + for (i = bc; i > pos; --i) { + ctx->avail_bufs[i - 1 + count] = ctx->avail_bufs[i - 1]; + } + // fill in: + for (i = pos; < pos + count; ++i) { + ctx->avail_bufs[i] = *in++; + } + // update count: + ctx->avail_buf_count = bc + count; +} + void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t size) { camera_ctx *ctx = (camera_ctx *)usr; if (ctx->receiving_frame) { @@ -121,9 +162,10 @@ void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t siz // frame done! if (ctx->target_buffer_index >= 0) { // put back into available buffers (in the front) - memmove(ctx->avail_bufs + 1, ctx->avail_bufs, ctx->avail_buf_count * sizeof(ctx->avail_bufs[0])); - ctx->avail_bufs[0] = ctx->target_buffer_index; - ctx->avail_buf_count += 1; + camera_buffer_fifo_push_at(ctx, 0, &ctx->target_buffer_index, 1); + if (ctx->put_back_buf_pos < ctx->avail_buf_count) { + ctx->put_back_buf_pos += 1; + } // notify camera_img_stream_get_buffers function. ctx->new_frame_arrived = true; } @@ -153,8 +195,7 @@ void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t siz if (cparam.size.x == ctx->img_stream_param.size.x || cparam.size.y == ctx->img_stream_param.size.y) { // get least recently used buffer from the available buffers: - ctx->target_buffer_index = ctx->avail_bufs[ctx->avail_buf_count - 1]; - ctx->avail_buf_count -= 1; + camera_buffer_fifo_remove_back(ctx, &ctx->target_buffer_index, 1); ctx->target_buffer = &ctx->buffers[ctx->target_buffer_index]; // initialize it: ctx->target_buffer->timestamp = get_boot_time_us(); @@ -172,12 +213,77 @@ void camera_transport_frame_done_fn(void *usr) { bool camera_img_stream_schedule_param_change(camera_ctx *ctx, const camera_img_param *img_param) { } -int camera_img_stream_get_buffers(camera_ctx *ctx, camera_image_buffer **buffers[], size_t count) { - +static bool camera_img_stream_get_buffers_idx(camera_ctx *ctx, int bidx[], size_t count, bool reset_new_frm) { + int i; + __disable_irq(); + camera_buffer_fifo_remove_front(ctx, bidx, count); + /* check that the buffers are in consecutive order: */ + bool consecutive = true; + uint32_t exp_frame = ctx->buffers[bidx[0]].frame_number + 1; + for (i = 1; i < count; ++i, ++exp_frame) { + if (ctx->buffers[bidx[i]].frame_number != exp_frame) { + consecutive = false; + break; + } + } + if (consecutive) { + /* good! */ + ctx->put_back_buf_pos = 0; + ctx->buffers_are_reserved = true; + } else { + /* not good. put back the buffers: */ + camera_buffer_fifo_push_at(ctx, 0, bidx, count); + } + if (reset_new_frm) { + ctx->new_frame_arrived = false; + } + __enable_irq(); + return consecutive; +} + +int camera_img_stream_get_buffers(camera_ctx *ctx, camera_image_buffer **buffers[], size_t count, bool wait_for_new) { + if (ctx->buffers_are_reserved) return -1; + if (count > ctx->buffer_count - 1 || count <= 0) return -1; + /* buffer management needs to be performed atomically: */ + int bidx[count]; + int i; + while (1) { + if (wait_for_new) { + /* wait until a new frame is available: */ + while(!ctx->new_frame_arrived); + } + if (camera_img_stream_get_buffers_idx(ctx, bidx, count, wait_for_new)) { + /* update the pointers: */ + for (i = 0; i < count; ++i) { + buffers[i] = &ctx->buffers[bidx[i]]; + } + return 0; + } else { + /* not possible! check if we want to wait for the new frame: */ + if (!wait_for_new) { + return 1; + } + } + } } void camera_img_stream_return_buffers(camera_ctx *ctx, camera_image_buffer **buffers[], size_t count) { - + if (!ctx->buffers_are_reserved) return; + /* get the buffer indexes: */ + int bidx[count]; + int i; + for (i = 0; i < count; ++i) { + int idx = buffers[i] - ctx->buffers[0]; + if (idx < 0 || idx >= ctx->buffer_count) return; + bidx[i] = idx; + } + /* buffer management needs to be performed atomically: */ + __disable_irq(); + size_t at_pos = ctx->put_back_buf_pos; + if (at_pos > ctx->avail_buf_count) at_pos = ctx->avail_buf_count; + camera_buffer_fifo_push_at(ctx, ctx->put_back_buf_pos, bidx, count); + ctx->buffers_are_reserved = false; + __enable_irq(); } bool camera_snapshot_schedule(camera_ctx *ctx, const camera_img_param *img_param, camera_image_buffer *dst, camera_snapshot_done_cb cb) { diff --git a/src/flow.c b/src/flow.c index c8a89dc..3da2ed3 100644 --- a/src/flow.c +++ b/src/flow.c @@ -50,7 +50,6 @@ #define __ASM asm #include "core_cm4_simd.h" -#define FRAME_SIZE global_data.param[PARAM_IMAGE_WIDTH] #define SEARCH_SIZE global_data.param[PARAM_FLOW_MAX_PIXEL] // maximum offset to search: 4 + 1/2 pixels #define TILE_SIZE 8 // x & y tile size #define NUM_BLOCKS 5 // x & y number of tiles to check @@ -395,7 +394,7 @@ uint16_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_ra { /* constants */ const uint16_t search_size = SEARCH_SIZE; - const uint16_t frame_size = FRAME_SIZE; + const uint16_t frame_size = FLOW_FRAME_SIZE; const int16_t winmin = -search_size; const int16_t winmax = search_size; @@ -493,22 +492,25 @@ uint16_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_ra return result_count; } -void klt_preprocess_image(uint8_t *image) { +void klt_preprocess_image(uint8_t *image, flow_klt_image *klt_image) { uint16_t i, j; + klt_image->image = image; + /* * compute image pyramid for current frame * there is 188*120 bytes per buffer, we are only using 64*64 per buffer, * so just add the pyramid levels after the image */ //first compute the offsets in the memory for the pyramid levels - uint16_t lvl_ofs[PYR_LVLS]; - uint16_t frame_size = (uint16_t)(FRAME_SIZE+0.5); - uint16_t s = frame_size; + uint8_t *lvl_base[PYR_LVLS]; + uint16_t frame_size = (uint16_t)(FLOW_FRAME_SIZE+0.5); + uint16_t s = frame_size / 2; uint16_t off = 0; - for (int l = 0; l < PYR_LVLS; l++) + lvl_base[0] = image; + for (int l = 1; l < PYR_LVLS; l++) { - lvl_ofs[l] = off; + lvl_base[l] = klt_image->preprocessed + off; off += s*s; s /= 2; } @@ -518,8 +520,8 @@ void klt_preprocess_image(uint8_t *image) { { uint16_t src_size = frame_size >> (l-1); uint16_t tar_size = frame_size >> l; - uint8_t *source = &image[lvl_ofs[l-1]]; //pointer to the beginning of the previous level - uint8_t *target = &image[lvl_ofs[l]]; //pointer to the beginning of the current level + uint8_t *source = lvl_base[l-1]; //pointer to the beginning of the previous level + uint8_t *target = lvl_base[l]; //pointer to the beginning of the current level for (j = 0; j < tar_size; j++) { for (i = 0; i < tar_size; i+=2) { @@ -536,7 +538,7 @@ void klt_preprocess_image(uint8_t *image) { } } -uint16_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate, +uint16_t compute_klt(flow_klt_image *image1, flow_klt_image *image2, float x_rate, float y_rate, float z_rate, flow_raw_result *out, uint16_t max_out) { /* variables */ @@ -553,13 +555,17 @@ uint16_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat * so just add the pyramid levels after the image */ //first compute the offsets in the memory for the pyramid levels - uint16_t lvl_ofs[PYR_LVLS]; - uint16_t frame_size = (uint16_t)(FRAME_SIZE+0.5); - uint16_t s = frame_size; + uint8_t *lvl_base1[PYR_LVLS]; + uint8_t *lvl_base2[PYR_LVLS]; + uint16_t frame_size = (uint16_t)(FLOW_FRAME_SIZE+0.5); + uint16_t s = frame_size / 2; uint16_t off = 0; - for (int l = 0; l < PYR_LVLS; l++) + lvl_base1[0] = image1->image; + lvl_base2[0] = image2->image; + for (int l = 1; l < PYR_LVLS; l++) { - lvl_ofs[l] = off; + lvl_base1[l] = image1->preprocessed + off; + lvl_base2[l] = image2->preprocessed + off; off += s*s; s /= 2; } @@ -621,7 +627,7 @@ uint16_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat uint16_t j = js[k] >> l; uint16_t iwidth = frame_size >> l; - uint8_t *base1 = image1 + lvl_ofs[l] + j * iwidth + i; + uint8_t *base1 = lvl_base1[l] + j * iwidth + i; float JTJ[4]; //the 2x2 Hessian JTJ[0] = 0; @@ -681,7 +687,7 @@ uint16_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat float JTe_x = 0; //accumulators for Jac transposed times error float JTe_y = 0; - uint8_t *base2 = image2 + lvl_ofs[l] + (uint16_t)v * iwidth + (uint16_t)u; + uint8_t *base2 = lvl_base2[l] + (uint16_t)v * iwidth + (uint16_t)u; //extract bilinearly filtered pixel values for the current location in image2 float dX = u - floorf(u); diff --git a/src/main.c b/src/main.c index b14d3ac..7c98543 100644 --- a/src/main.c +++ b/src/main.c @@ -109,6 +109,8 @@ void system_receive_fn(void) { communication_receive_usb(); } +uint8_t *previous_image = NULL; + void send_video_fn(void) { /* update the rate */ timer_update(send_video_fn, global_data.param[PARAM_VIDEO_RATE]); @@ -162,6 +164,8 @@ uint8_t image_buffer_8bit_3[FLOW_IMAGE_SIZE * FLOW_IMAGE_SIZE] __attribute__((se uint8_t image_buffer_8bit_4[FLOW_IMAGE_SIZE * FLOW_IMAGE_SIZE] __attribute__((section(".ccm"))); uint8_t image_buffer_8bit_5[FLOW_IMAGE_SIZE * FLOW_IMAGE_SIZE] __attribute__((section(".ccm"))); +flow_klt_image flow_klt_images[2] __attribute__((section(".ccm"))); + /** * @brief Main function. */ @@ -245,14 +249,15 @@ int main(void) uint32_t fps_timing_start = get_boot_time_us(); uint16_t fps_counter = 0; uint16_t fps_skipped_counter = 0; + + uint32_t last_frame_index = 0; + /* main loop */ while (1) { /* check timers */ timer_check(); - uint16_t image_size = global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; - /* calculate focal_length in pixel */ const float focal_length_px = (global_data.param[PARAM_FOCAL_LENGTH_MM]) / (4.0f * 0.006f); //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled @@ -278,28 +283,62 @@ int main(void) uint32_t start_computations = 0; - /* copy recent image to faster ram */ - int skipped_frames = 0; - int loop_count = 0; //< make sure that we dont end up in an infinite loop inside here .. - do { - skipped_frames = dma_copy_image_buffers(¤t_image, &previous_image, image_size, (int)(global_data.param[PARAM_IMAGE_INTERVAL] + 0.5)); - - start_computations = get_boot_time_us(); - - /* filter the new image */ - if (global_data.param[PARAM_ALGORITHM_IMAGE_FILTER]) { - filter_image(current_image, global_data.param[PARAM_IMAGE_WIDTH]); + /* get recent images */ + camera_image_buffer *frames[2]; + camera_img_stream_get_buffers(&cam_ctx, frames, 2, true); + int frame_delta = ((int32_t)frames[0]->frame_number - (int32_t)last_frame_index); + fps_skipped_counter += frame_delta - 1; + + flow_klt_image *klt_images[2] = {NULL, NULL}; + { + /* make sure that the new images get the correct treatment */ + /* this algorithm will still work if both images are new */ + int i; + bool used_klt_image[2] = {false, false}; + for (i = 0; i < 2; ++i) { + if (frames[i]->frame_number != frames[i]->meta) { + // the image is new. apply pre-processing: + /* filter the new image */ + if (global_data.param[PARAM_ALGORITHM_IMAGE_FILTER]) { + filter_image(frames[i]->buffer, frames[i]->param.size.x); + } + /* update meta data to mark it as an up-to date image: */ + frames[i]->meta = frames[i]->frame_number; + } else { + // the image has the preprocessing already applied. + if (use_klt) { + int j; + /* find the klt image that matches: */ + for (j = 0; j < 2; ++j) { + if (flow_klt_images[j].meta == frames[i]->frame_number) { + used_klt_image[j] = true; + klt_images[i] = &flow_klt_images[j]; + } + } + } + } } - - /* Preprocessing needed by the klt algorithm: */ if (use_klt) { - klt_preprocess_image(current_image); + /* only for KLT: */ + /* preprocess the images if they are not yet preprocessed */ + for (i = 0; i < 2; ++i) { + if (klt_images[i] == NULL) { + // need processing. find unused KLT image: + int j; + for (j = 0; j < 2; ++j) { + if (!used_klt_image[j]) { + used_klt_image[j] = true; + klt_images[i] = &flow_klt_images[j]; + break; + } + } + klt_preprocess_image(frames[i]->buffer, klt_images[i]); + } + } } - /* make sure no frames have been skipped. request a new image if a frame was skipped to ensure minimal time delta between images */ - fps_skipped_counter += skipped_frames; - loop_count++; - } while (skipped_frames > 0 && loop_count < 2); - float frame_dt = get_time_between_images() * 0.000001f; + } + + float frame_dt = (frames[0]->timestamp - frames[1]->timestamp) * 0.000001f; /* compute gyro rate in pixels and change to image coordinates */ float x_rate_px = - y_rate * (focal_length_px * frame_dt); @@ -310,9 +349,9 @@ int main(void) flow_raw_result flow_rslt[32]; uint16_t flow_rslt_count = 0; if (!use_klt) { - flow_rslt_count = compute_flow(previous_image, current_image, x_rate_px, y_rate_px, z_rate_fr, flow_rslt, 32); + flow_rslt_count = compute_flow(frames[1]->buffer, frames[0]->buffer, x_rate_px, y_rate_px, z_rate_fr, flow_rslt, 32); } else { - flow_rslt_count = compute_klt(previous_image, current_image, x_rate_px, y_rate_px, z_rate_fr, flow_rslt, 32); + flow_rslt_count = compute_klt(klt_images[1], klt_images[0], x_rate_px, y_rate_px, z_rate_fr, flow_rslt, 32); } /* calculate flow value from the raw results */ @@ -333,6 +372,7 @@ int main(void) /* create flow image if needed (previous_image is not needed anymore) * -> can be used for debugging purpose */ + previous_image = frames[1]->buffer; if (global_data.param[PARAM_USB_SEND_VIDEO]) { uint16_t frame_size = global_data.param[PARAM_IMAGE_WIDTH]; @@ -347,6 +387,9 @@ int main(void) } } + /* return the image buffers */ + camera_img_stream_return_buffers(&cam_ctx, frames, 2); + /* decide which distance to use */ float ground_distance = 0.0f; @@ -359,7 +402,7 @@ int main(void) ground_distance = sonar_distance_raw; } - /* update I2C transmitbuffer */ + /* update I2C transmit buffer */ update_TX_buffer(frame_dt, x_rate, y_rate, z_rate, gyro_temp, qual, pixel_flow_x, pixel_flow_y, 1.0f / focal_length_px, From 18798eeca7443e7fa9a78b14087f8fbeaa477f54 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Wed, 5 Aug 2015 18:34:39 +0200 Subject: [PATCH 048/120] made code compile. testing is next. --- inc/camera.h | 5 +++-- inc/main.h | 3 --- src/camera.c | 15 +++++++++++---- src/communication.c | 8 -------- src/dcmi.c | 7 ++++++- src/flow.c | 5 +++-- src/mt9v034.c | 2 +- src/timer.c | 4 ++-- 8 files changed, 26 insertions(+), 23 deletions(-) diff --git a/inc/camera.h b/inc/camera.h index ef9cc5f..9e3808c 100644 --- a/inc/camera.h +++ b/inc/camera.h @@ -37,6 +37,7 @@ #include #include +#include #define CAMERA_MAX_BUFFER_COUNT 5 @@ -131,7 +132,7 @@ bool camera_img_stream_schedule_param_change(camera_ctx *ctx, const camera_img_p * @note When this function is successful (return value 0) the buffers need to be returned to the camera driver before * requesting new buffers. (use camera_img_stream_return_buffers) */ -int camera_img_stream_get_buffers(camera_ctx *ctx, camera_image_buffer **buffers[], size_t count, bool wait_for_new); +int camera_img_stream_get_buffers(camera_ctx *ctx, camera_image_buffer *buffers[], size_t count, bool wait_for_new); /** * Returns the buffers that have been retrieved by camera_img_stream_get_buffers back to the camera driver. @@ -140,7 +141,7 @@ int camera_img_stream_get_buffers(camera_ctx *ctx, camera_image_buffer **buffers * @param buffers Pointer to an array of buffer pointers which will be returned to the camera driver. * @param count Number of buffer pointers in the array. */ -void camera_img_stream_return_buffers(camera_ctx *ctx, camera_image_buffer **buffers[], size_t count); +void camera_img_stream_return_buffers(camera_ctx *ctx, camera_image_buffer *buffers[], size_t count); /** * Schedules a snapshot to be taken with the camera. The snapshot will be scheduled as soon as possible. diff --git a/inc/main.h b/inc/main.h index 7e7f796..01c6cd8 100644 --- a/inc/main.h +++ b/inc/main.h @@ -34,7 +34,4 @@ #ifndef __PX4_FLOWBOARD_H #define __PX4_FLOWBOARD_H -extern uint32_t get_time_between_images(void); -void buffer_reset(void); - #endif /* __PX4_FLOWBOARD_H */ diff --git a/src/camera.c b/src/camera.c index 9d45dab..184329f 100644 --- a/src/camera.c +++ b/src/camera.c @@ -35,8 +35,13 @@ #include "camera.h" #include "timer.h" +#define __INLINE inline +#define __ASM asm +#include "core_cmFunc.h" + #include + void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t size); void camera_transport_frame_done_fn(void *usr); @@ -140,7 +145,7 @@ static void camera_buffer_fifo_push_at(camera_ctx *ctx, size_t pos, const int *i ctx->avail_bufs[i - 1 + count] = ctx->avail_bufs[i - 1]; } // fill in: - for (i = pos; < pos + count; ++i) { + for (i = pos; i < pos + count; ++i) { ctx->avail_bufs[i] = *in++; } // update count: @@ -211,6 +216,7 @@ void camera_transport_frame_done_fn(void *usr) { } bool camera_img_stream_schedule_param_change(camera_ctx *ctx, const camera_img_param *img_param) { + return false; } static bool camera_img_stream_get_buffers_idx(camera_ctx *ctx, int bidx[], size_t count, bool reset_new_frm) { @@ -241,7 +247,7 @@ static bool camera_img_stream_get_buffers_idx(camera_ctx *ctx, int bidx[], size_ return consecutive; } -int camera_img_stream_get_buffers(camera_ctx *ctx, camera_image_buffer **buffers[], size_t count, bool wait_for_new) { +int camera_img_stream_get_buffers(camera_ctx *ctx, camera_image_buffer *buffers[], size_t count, bool wait_for_new) { if (ctx->buffers_are_reserved) return -1; if (count > ctx->buffer_count - 1 || count <= 0) return -1; /* buffer management needs to be performed atomically: */ @@ -267,13 +273,13 @@ int camera_img_stream_get_buffers(camera_ctx *ctx, camera_image_buffer **buffers } } -void camera_img_stream_return_buffers(camera_ctx *ctx, camera_image_buffer **buffers[], size_t count) { +void camera_img_stream_return_buffers(camera_ctx *ctx, camera_image_buffer *buffers[], size_t count) { if (!ctx->buffers_are_reserved) return; /* get the buffer indexes: */ int bidx[count]; int i; for (i = 0; i < count; ++i) { - int idx = buffers[i] - ctx->buffers[0]; + int idx = buffers[i] - &ctx->buffers[0]; if (idx < 0 || idx >= ctx->buffer_count) return; bidx[i] = idx; } @@ -287,6 +293,7 @@ void camera_img_stream_return_buffers(camera_ctx *ctx, camera_image_buffer **buf } bool camera_snapshot_schedule(camera_ctx *ctx, const camera_img_param *img_param, camera_image_buffer *dst, camera_snapshot_done_cb cb) { + return false; } diff --git a/src/communication.c b/src/communication.c index 83205aa..a322ce0 100644 --- a/src/communication.c +++ b/src/communication.c @@ -48,7 +48,6 @@ #include "communication.h" #include "timer.h" -extern void buffer_reset(void); extern void systemreset(bool to_bootloader); mavlink_system_t mavlink_system; @@ -239,18 +238,11 @@ void handle_mavlink_message(mavlink_channel_t chan, /* handle low light mode and noise correction */ if(i == PARAM_IMAGE_LOW_LIGHT || i == PARAM_IMAGE_ROW_NOISE_CORR || i == PARAM_IMAGE_TEST_PATTERN) { - mt9v034_context_configuration(); - dma_reconfigure(); - buffer_reset(); } /* handle calibration on/off */ else if(i == PARAM_VIDEO_ONLY) { - mt9v034_set_context(); - dma_reconfigure(); - buffer_reset(); - if(global_data.param[PARAM_VIDEO_ONLY]) debug_string_message_buffer("Calibration Mode On"); else diff --git a/src/dcmi.c b/src/dcmi.c index 58e046e..40ee883 100644 --- a/src/dcmi.c +++ b/src/dcmi.c @@ -56,11 +56,15 @@ typedef struct _dcmi_transport_ctx dcmi_transport_ctx; uint8_t dcmi_dma_buffer_1[CONFIG_DCMI_DMA_BUFFER_SIZE]; uint8_t dcmi_dma_buffer_2[CONFIG_DCMI_DMA_BUFFER_SIZE]; +bool dcmi_init(void *usr, + camera_transport_transfer_done_cb transfer_done_cb, + camera_transport_frame_done_cb frame_done_cb, + void *cb_usr); + /** * @brief HW initialization of DCMI clock */ static void dcmi_clock_init(); - /** * @brief HW initialization DCMI */ @@ -112,6 +116,7 @@ bool dcmi_init(void *usr, dcmi_dma_init(); dcmi_dma_enable(); dcmi_dma_it_init(); + return true; } /** diff --git a/src/flow.c b/src/flow.c index 3da2ed3..42d3a59 100644 --- a/src/flow.c +++ b/src/flow.c @@ -40,6 +40,7 @@ #include #include "mavlink_bridge_header.h" +#include "settings.h" #include #include "flow.h" #include "dcmi.h" @@ -438,11 +439,11 @@ uint16_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_ra int8_t sumy = 0; int8_t ii, jj; - uint8_t *base1 = image1 + j * frame_size + i; + //uint8_t *base1 = image1 + j * frame_size + i; for (jj = winmin; jj <= winmax; jj++) { - uint8_t *base2 = image2 + (j+jj) * frame_size + i; + //uint8_t *base2 = image2 + (j+jj) * frame_size + i; for (ii = winmin; ii <= winmax; ii++) { diff --git a/src/mt9v034.c b/src/mt9v034.c index 05f47a5..51de99e 100644 --- a/src/mt9v034.c +++ b/src/mt9v034.c @@ -185,7 +185,7 @@ void mt9v034_notify_readout_start(void *usr) { void mt9v034_get_current_param(void *usr, camera_img_param *img_param) { mt9v034_sensor_ctx *ctx = (mt9v034_sensor_ctx *)usr; - *img_param = *ctx->cur_param; + *img_param = ctx->cur_param; } static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx) { diff --git a/src/timer.c b/src/timer.c index b72ab83..6b6067c 100644 --- a/src/timer.c +++ b/src/timer.c @@ -171,7 +171,7 @@ void timer_init(void) uint32_t get_boot_time_ms(void) { // clear the COUNTFLAG: - volatile bool dummy = (SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) != 0; + volatile bool dummy __attribute__((unused)) = (SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) != 0; volatile uint32_t val; do { // read the value: @@ -184,7 +184,7 @@ uint32_t get_boot_time_ms(void) uint32_t get_boot_time_us(void) { // clear the COUNTFLAG: - volatile bool dummy = (SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) != 0; + volatile bool dummy __attribute__((unused)) = (SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) != 0; volatile uint32_t val_us_base; volatile uint32_t val_tick; do { From 8fa137ef385b0f4c0b150c5682ba90b9d21245d3 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Wed, 5 Aug 2015 18:36:23 +0200 Subject: [PATCH 049/120] fixed warnings in code. --- src/flow.c | 4 ++-- src/timer.c | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/flow.c b/src/flow.c index c8a89dc..9c65f54 100644 --- a/src/flow.c +++ b/src/flow.c @@ -439,11 +439,11 @@ uint16_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_ra int8_t sumy = 0; int8_t ii, jj; - uint8_t *base1 = image1 + j * frame_size + i; + //uint8_t *base1 = image1 + j * frame_size + i; for (jj = winmin; jj <= winmax; jj++) { - uint8_t *base2 = image2 + (j+jj) * frame_size + i; + //uint8_t *base2 = image2 + (j+jj) * frame_size + i; for (ii = winmin; ii <= winmax; ii++) { diff --git a/src/timer.c b/src/timer.c index b72ab83..6b6067c 100644 --- a/src/timer.c +++ b/src/timer.c @@ -171,7 +171,7 @@ void timer_init(void) uint32_t get_boot_time_ms(void) { // clear the COUNTFLAG: - volatile bool dummy = (SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) != 0; + volatile bool dummy __attribute__((unused)) = (SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) != 0; volatile uint32_t val; do { // read the value: @@ -184,7 +184,7 @@ uint32_t get_boot_time_ms(void) uint32_t get_boot_time_us(void) { // clear the COUNTFLAG: - volatile bool dummy = (SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) != 0; + volatile bool dummy __attribute__((unused)) = (SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) != 0; volatile uint32_t val_us_base; volatile uint32_t val_tick; do { From 757f0112a57c3df39a762b9066f0b7c5eb5890c2 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Wed, 5 Aug 2015 19:04:59 +0200 Subject: [PATCH 050/120] fixed bug in camera.c. The code now runs. However there is an issue with where the frame starts in the buffer. --- src/camera.c | 7 +++---- src/main.c | 5 ++++- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/camera.c b/src/camera.c index 184329f..c85fcb2 100644 --- a/src/camera.c +++ b/src/camera.c @@ -35,8 +35,7 @@ #include "camera.h" #include "timer.h" -#define __INLINE inline -#define __ASM asm +#include "stm32f4xx_conf.h" #include "core_cmFunc.h" #include @@ -225,8 +224,8 @@ static bool camera_img_stream_get_buffers_idx(camera_ctx *ctx, int bidx[], size_ camera_buffer_fifo_remove_front(ctx, bidx, count); /* check that the buffers are in consecutive order: */ bool consecutive = true; - uint32_t exp_frame = ctx->buffers[bidx[0]].frame_number + 1; - for (i = 1; i < count; ++i, ++exp_frame) { + uint32_t exp_frame = ctx->buffers[bidx[0]].frame_number - 1; + for (i = 1; i < count; ++i, --exp_frame) { if (ctx->buffers[bidx[i]].frame_number != exp_frame) { consecutive = false; break; diff --git a/src/main.c b/src/main.c index 7c98543..4edfabc 100644 --- a/src/main.c +++ b/src/main.c @@ -115,6 +115,8 @@ void send_video_fn(void) { /* update the rate */ timer_update(send_video_fn, global_data.param[PARAM_VIDEO_RATE]); + if (previous_image == NULL) return; + /* transmit raw 8-bit image */ if (global_data.param[PARAM_USB_SEND_VIDEO] && !global_data.param[PARAM_VIDEO_ONLY]) { @@ -282,11 +284,12 @@ int main(void) bool use_klt = global_data.param[PARAM_ALGORITHM_CHOICE] != 0; uint32_t start_computations = 0; - + /* get recent images */ camera_image_buffer *frames[2]; camera_img_stream_get_buffers(&cam_ctx, frames, 2, true); int frame_delta = ((int32_t)frames[0]->frame_number - (int32_t)last_frame_index); + last_frame_index = frames[0]->frame_number; fps_skipped_counter += frame_delta - 1; flow_klt_image *klt_images[2] = {NULL, NULL}; From b409ca65c220dfa5abb0af2fde0f81937ea35da9 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Wed, 5 Aug 2015 19:32:18 +0200 Subject: [PATCH 051/120] fixed transfer alignment issue in camera driver. --- inc/camera.h | 8 ++++++++ src/camera.c | 12 ++++++++++++ src/dcmi.c | 19 +++++++++++++++---- src/main.c | 3 +++ 4 files changed, 38 insertions(+), 4 deletions(-) diff --git a/inc/camera.h b/inc/camera.h index 9e3808c..1f25fa6 100644 --- a/inc/camera.h +++ b/inc/camera.h @@ -224,12 +224,20 @@ struct _camera_transport_interface { camera_transport_transfer_done_cb transfer_done_cb, camera_transport_frame_done_cb frame_done_cb, void *cb_usr); + /** + * This function is called to reset the transport layer to align it with the frame boundaries. + * It is called in the frame_done_cb after a few frames when we can be sure that the camera module has the correct resolution. + */ + void (*reset)(void *usr); }; /** * The camera driver context struct. */ struct _camera_ctx { + /* startup control */ + int startup_discard_frame_count; ///< Number of frames to discard at startup. + /* assets of the camera driver */ const camera_sensor_interface *sensor; ///< Sensor interface. diff --git a/src/camera.c b/src/camera.c index c85fcb2..2963fb4 100644 --- a/src/camera.c +++ b/src/camera.c @@ -60,6 +60,7 @@ bool camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const c return false; } // initialize state: + ctx->startup_discard_frame_count = 4; ctx->img_stream_param = *img_param; int i; for (i = 0; i < buffer_count; ++i) { @@ -153,6 +154,9 @@ static void camera_buffer_fifo_push_at(camera_ctx *ctx, size_t pos, const int *i void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t size) { camera_ctx *ctx = (camera_ctx *)usr; + if (ctx->startup_discard_frame_count > 0) { + return; + } if (ctx->receiving_frame) { // check if we have a target buffer: if (ctx->target_buffer != NULL) { @@ -212,6 +216,14 @@ void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t siz } void camera_transport_frame_done_fn(void *usr) { + camera_ctx *ctx = (camera_ctx *)usr; + if (ctx->startup_discard_frame_count > 0) { + ctx->startup_discard_frame_count--; + if (ctx->startup_discard_frame_count == 0) { + /* re-initialize the transport */ + ctx->transport->reset(ctx->transport->usr); + } + } } bool camera_img_stream_schedule_param_change(camera_ctx *ctx, const camera_img_param *img_param) { diff --git a/src/dcmi.c b/src/dcmi.c index 40ee883..9896cb5 100644 --- a/src/dcmi.c +++ b/src/dcmi.c @@ -61,6 +61,8 @@ bool dcmi_init(void *usr, camera_transport_frame_done_cb frame_done_cb, void *cb_usr); +void dcmi_reset(void *usr); + /** * @brief HW initialization of DCMI clock */ @@ -94,6 +96,7 @@ const camera_transport_interface dcmi_transport_interface = { .usr = &dcmi_ctx, .transfer_size = CONFIG_DCMI_DMA_BUFFER_SIZE, .init = dcmi_init, + .reset = dcmi_reset }; const camera_transport_interface *dcmi_get_transport_interface() { @@ -119,6 +122,15 @@ bool dcmi_init(void *usr, return true; } +void dcmi_reset(void *usr) { + /* stop the DMA: */ + DMA_Cmd(DMA2_Stream1, DISABLE); + /* clear pending interrupt: */ + DMA_ClearITPendingBit(DMA2_Stream1, DMA_IT_TCIF1); + /* re-enable */ + DMA_Cmd(DMA2_Stream1, ENABLE); +} + /** * @brief Interrupt handler of DCMI */ @@ -138,15 +150,14 @@ void DCMI_IRQHandler(void) { void DMA2_Stream1_IRQHandler(void) { /* transfer completed */ - if (DMA_GetITStatus(DMA2_Stream1, DMA_IT_TCIF1) != RESET) - { + if (DMA_GetITStatus(DMA2_Stream1, DMA_IT_TCIF1) != RESET) { DMA_ClearITPendingBit(DMA2_Stream1, DMA_IT_TCIF1); /* get the buffer that has been completed: */ void *buffer; if (DMA_GetCurrentMemoryTarget(DMA2_Stream1)) { - buffer = dcmi_dma_buffer_1; - } else { buffer = dcmi_dma_buffer_2; + } else { + buffer = dcmi_dma_buffer_1; } /* get context: */ dcmi_transport_ctx *ctx = &dcmi_ctx; diff --git a/src/main.c b/src/main.c index 4edfabc..565fdca 100644 --- a/src/main.c +++ b/src/main.c @@ -288,6 +288,9 @@ int main(void) /* get recent images */ camera_image_buffer *frames[2]; camera_img_stream_get_buffers(&cam_ctx, frames, 2, true); + + start_computations = get_boot_time_us(); + int frame_delta = ((int32_t)frames[0]->frame_number - (int32_t)last_frame_index); last_frame_index = frames[0]->frame_number; fps_skipped_counter += frame_delta - 1; From f2fc46d691678f16cfe44f88e3f8ef2e48ad0c02 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Wed, 5 Aug 2015 19:42:46 +0200 Subject: [PATCH 052/120] fixed filter to not use space inside the image to do the computation. --- inc/filter.h | 2 +- src/filter.c | 5 ++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/inc/filter.h b/inc/filter.h index a11187e..a85cba5 100644 --- a/inc/filter.h +++ b/inc/filter.h @@ -38,7 +38,7 @@ /** * @brief Filters the image to improve the flow processing - * @note It uses the space after the image in the image buffer to work on the image. + * @note It uses a temporary buffer on the stack to work on the image. */ void filter_image(uint8_t *image, uint16_t width); diff --git a/src/filter.c b/src/filter.c index 82980af..d48af93 100644 --- a/src/filter.c +++ b/src/filter.c @@ -40,8 +40,7 @@ #include #include -#define __INLINE inline -#define __ASM asm +#include "stm32f4xx_conf.h" #include "core_cm4_simd.h" #include "filter.h" @@ -49,7 +48,7 @@ void filter_image(uint8_t *image, uint16_t width) { uint16_t ext_width = width + 2; /* first we make a copy of the image and extend it beyond the edges by 1 pixel */ - uint8_t *img_ext = image + width * width; + uint8_t img_ext[ext_width * ext_width]; #define IMG_PX(x, y) (*(image + width * (y) + (x))) #define EXT_PX(x, y) (*(img_ext + ext_width * (y) + (x))) int y, x; From 3a629fd7448a61afd31b6f449b8154874ca577c5 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Wed, 5 Aug 2015 20:13:50 +0200 Subject: [PATCH 053/120] made initialisation and buffer alignment more reliable. --- inc/camera.h | 2 +- src/camera.c | 16 ++++++++++------ src/dcmi.c | 12 +++++++----- 3 files changed, 18 insertions(+), 12 deletions(-) diff --git a/inc/camera.h b/inc/camera.h index 1f25fa6..3d5ccb7 100644 --- a/inc/camera.h +++ b/inc/camera.h @@ -236,7 +236,7 @@ struct _camera_transport_interface { */ struct _camera_ctx { /* startup control */ - int startup_discard_frame_count; ///< Number of frames to discard at startup. + volatile int startup_discard_frame_count; ///< Number of frames to discard at startup. /* assets of the camera driver */ diff --git a/src/camera.c b/src/camera.c index 2963fb4..2d3411b 100644 --- a/src/camera.c +++ b/src/camera.c @@ -60,7 +60,7 @@ bool camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const c return false; } // initialize state: - ctx->startup_discard_frame_count = 4; + ctx->startup_discard_frame_count = -1; ctx->img_stream_param = *img_param; int i; for (i = 0; i < buffer_count; ++i) { @@ -102,6 +102,8 @@ bool camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const c if (!ctx->sensor->init(ctx->sensor->usr, img_param)) { return false; } + /* after initialization start the discard count down! */ + ctx->startup_discard_frame_count = 16; return true; } @@ -154,7 +156,7 @@ static void camera_buffer_fifo_push_at(camera_ctx *ctx, size_t pos, const int *i void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t size) { camera_ctx *ctx = (camera_ctx *)usr; - if (ctx->startup_discard_frame_count > 0) { + if (ctx->startup_discard_frame_count != 0) { return; } if (ctx->receiving_frame) { @@ -217,10 +219,12 @@ void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t siz void camera_transport_frame_done_fn(void *usr) { camera_ctx *ctx = (camera_ctx *)usr; - if (ctx->startup_discard_frame_count > 0) { - ctx->startup_discard_frame_count--; - if (ctx->startup_discard_frame_count == 0) { - /* re-initialize the transport */ + int fdc = ctx->startup_discard_frame_count; + if (fdc > 0) { + fdc--; + ctx->startup_discard_frame_count = fdc; + /* re-initialize the transport twice */ + if (fdc == 0 || fdc == 1) { ctx->transport->reset(ctx->transport->usr); } } diff --git a/src/dcmi.c b/src/dcmi.c index 9896cb5..2aabb07 100644 --- a/src/dcmi.c +++ b/src/dcmi.c @@ -126,8 +126,10 @@ void dcmi_reset(void *usr) { /* stop the DMA: */ DMA_Cmd(DMA2_Stream1, DISABLE); /* clear pending interrupt: */ - DMA_ClearITPendingBit(DMA2_Stream1, DMA_IT_TCIF1); - /* re-enable */ + if (DMA_GetITStatus(DMA2_Stream1, DMA_IT_TCIF1) != RESET) { + DMA_ClearITPendingBit(DMA2_Stream1, DMA_IT_TCIF1); + } + /* re-enable DMA */ DMA_Cmd(DMA2_Stream1, ENABLE); } @@ -154,10 +156,10 @@ void DMA2_Stream1_IRQHandler(void) DMA_ClearITPendingBit(DMA2_Stream1, DMA_IT_TCIF1); /* get the buffer that has been completed: */ void *buffer; - if (DMA_GetCurrentMemoryTarget(DMA2_Stream1)) { - buffer = dcmi_dma_buffer_2; - } else { + if (DMA_GetCurrentMemoryTarget(DMA2_Stream1) == 0) { buffer = dcmi_dma_buffer_1; + } else { + buffer = dcmi_dma_buffer_2; } /* get context: */ dcmi_transport_ctx *ctx = &dcmi_ctx; From 7fdc1b19f0161b215be6dcccba3446dd86767a73 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Wed, 5 Aug 2015 20:50:35 +0200 Subject: [PATCH 054/120] increase I2C bus speed to make it possible to switch sensor contexts between two DMA transfers. This resolves all problems with the de-syncing. Changed back bogus fix in DMA buffer selection. --- src/camera.c | 4 ++++ src/dcmi.c | 7 +++---- src/mt9v034.c | 2 +- 3 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/camera.c b/src/camera.c index 2d3411b..40409bc 100644 --- a/src/camera.c +++ b/src/camera.c @@ -37,6 +37,7 @@ #include "stm32f4xx_conf.h" #include "core_cmFunc.h" +#include "led.h" #include @@ -231,6 +232,9 @@ void camera_transport_frame_done_fn(void *usr) { } bool camera_img_stream_schedule_param_change(camera_ctx *ctx, const camera_img_param *img_param) { + if (!ctx->seq_snapshot_active) { + return ctx->sensor->prepare_update_param(ctx->sensor->usr, img_param); + } return false; } diff --git a/src/dcmi.c b/src/dcmi.c index 2aabb07..296e948 100644 --- a/src/dcmi.c +++ b/src/dcmi.c @@ -149,17 +149,16 @@ void DCMI_IRQHandler(void) { /** * @brief Interrupt handler of DCMI DMA stream */ -void DMA2_Stream1_IRQHandler(void) -{ +void DMA2_Stream1_IRQHandler(void) { /* transfer completed */ if (DMA_GetITStatus(DMA2_Stream1, DMA_IT_TCIF1) != RESET) { DMA_ClearITPendingBit(DMA2_Stream1, DMA_IT_TCIF1); /* get the buffer that has been completed: */ void *buffer; if (DMA_GetCurrentMemoryTarget(DMA2_Stream1) == 0) { - buffer = dcmi_dma_buffer_1; - } else { buffer = dcmi_dma_buffer_2; + } else { + buffer = dcmi_dma_buffer_1; } /* get context: */ dcmi_transport_ctx *ctx = &dcmi_ctx; diff --git a/src/mt9v034.c b/src/mt9v034.c index 51de99e..fcd8e2c 100644 --- a/src/mt9v034.c +++ b/src/mt9v034.c @@ -533,7 +533,7 @@ static bool mt9v034_init_hw(mt9v034_sensor_ctx *ctx) { I2C_InitStruct.I2C_OwnAddress1 = 0xFE; I2C_InitStruct.I2C_Ack = I2C_Ack_Enable; I2C_InitStruct.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; - I2C_InitStruct.I2C_ClockSpeed = 100000; + I2C_InitStruct.I2C_ClockSpeed = 400000; /* Initialize the I2C peripheral w/ selected parameters */ I2C_Init(I2C2, &I2C_InitStruct); From 7c045177bc86abea536534b5831644def69d503e Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Wed, 5 Aug 2015 21:17:55 +0200 Subject: [PATCH 055/120] cleanup i2c code. - switched to 16bit read and write mode to improve context switching time. --- src/mt9v034.c | 230 ++++++++++++++++++++++---------------------------- 1 file changed, 99 insertions(+), 131 deletions(-) diff --git a/src/mt9v034.c b/src/mt9v034.c index fcd8e2c..372b147 100644 --- a/src/mt9v034.c +++ b/src/mt9v034.c @@ -57,27 +57,18 @@ static bool mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, /** * @brief Reads from a specific Camera register - */ -static uint16_t mt9v034_ReadReg16(uint8_t address); -/** - * @brief Writes to a specific Camera register - */ -static uint8_t mt9v034_WriteReg16(uint16_t address, uint16_t Data); -/** - * @brief Reads a byte from a specific Camera register * @param Addr: mt9v034 register address. - * @retval data read from the specific register or 0xFF if timeout condition + * @retval data read from the specific register or 0xFFFF if timeout condition * occurred. */ -static uint8_t mt9v034_ReadReg(uint16_t Addr); +static uint16_t mt9v034_ReadReg(uint16_t Addr); /** * @brief Writes a byte at a specific Camera register * @param Addr: mt9v034 register address. * @param Data: Data to be written to the specific register - * @retval 0x00 if write operation is OK. - * 0xFF if timeout condition occurred (device not connected or bus error). + * @retval true when operation is successful. */ -static uint8_t mt9v034_WriteReg(uint16_t Addr, uint8_t Data); +static bool mt9v034_WriteReg(uint16_t Addr, uint16_t Data); struct _mt9v034_sensor_ctx { /* context switching */ @@ -174,9 +165,9 @@ void mt9v034_notify_readout_start(void *usr) { case 0: ctx->chip_control_reg &= 0x7FFF; break; case 1: ctx->chip_control_reg |= 0x8000; break; } - mt9v034_WriteReg16(MTV_CHIP_CONTROL_REG, ctx->chip_control_reg); + mt9v034_WriteReg(MTV_CHIP_CONTROL_REG, ctx->chip_control_reg); /* update pixel count for AGC and AEC: */ - mt9v034_WriteReg16(MTV_AGC_AEC_PIXEL_COUNT_REG, ctx_param->size.x * ctx_param->size.y); + mt9v034_WriteReg(MTV_AGC_AEC_PIXEL_COUNT_REG, ctx_param->size.x * ctx_param->size.y); /* done. */ ctx->do_switch_context = false; ctx->cur_context = desired_ctx_idx; @@ -200,7 +191,7 @@ static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx) { * [15] Context A / B select 0 = Context A registers are used. */ ctx->chip_control_reg = 0x0188; - mt9v034_WriteReg16(MTV_CHIP_CONTROL_REG, ctx->chip_control_reg); + mt9v034_WriteReg(MTV_CHIP_CONTROL_REG, ctx->chip_control_reg); /* settings that are the same for both contexts: */ @@ -209,7 +200,7 @@ static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx) { * [1:0] ADC Mode Context A: 2 = 10-bit linear * [9:8] ADC Mode Context B: 2 = 10-bit linear */ - mt9v034_WriteReg16(MTV_ADC_RES_CTRL_REG, 0x0202); + mt9v034_WriteReg(MTV_ADC_RES_CTRL_REG, 0x0202); /* * Row Noise Correction Control @@ -224,21 +215,21 @@ static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx) { } else { row_noise_correction = 0x0000; } - mt9v034_WriteReg16(MTV_ROW_NOISE_CORR_CTRL_REG, row_noise_correction); + mt9v034_WriteReg(MTV_ROW_NOISE_CORR_CTRL_REG, row_noise_correction); /* * Minimum Coarse Shutter Width * Set to minimum. (1) */ - mt9v034_WriteReg16(MTV_MIN_COARSE_SW_REG, 0x0001); + mt9v034_WriteReg(MTV_MIN_COARSE_SW_REG, 0x0001); /* * Maximum Coarse Shutter Width */ - mt9v034_WriteReg16(MTV_MAX_COARSE_SW_REG, CONFIG_MAX_EXPOSURE_ROWS); + mt9v034_WriteReg(MTV_MAX_COARSE_SW_REG, CONFIG_MAX_EXPOSURE_ROWS); /* * Maximum Analog Gain */ - mt9v034_WriteReg16(MTV_ANALOG_MAX_GAIN_REG, CONFIG_MAX_ANALOG_GAIN); + mt9v034_WriteReg(MTV_ANALOG_MAX_GAIN_REG, CONFIG_MAX_ANALOG_GAIN); /* * AEC/AGC Desired Bin @@ -251,29 +242,29 @@ static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx) { } else { desired_brightness = 16; } - mt9v034_WriteReg16(MTV_AGC_AEC_DESIRED_BIN_REG, desired_brightness); + mt9v034_WriteReg(MTV_AGC_AEC_DESIRED_BIN_REG, desired_brightness); /* * AEC Update Frequency * Number of frames to skip between changes in AEC * Range: 0-15 */ - mt9v034_WriteReg16(MTV_AEC_UPDATE_FREQ_REG, 2); + mt9v034_WriteReg(MTV_AEC_UPDATE_FREQ_REG, 2); /* * AEC Low Pass Filter * Range: 0-2 */ - mt9v034_WriteReg16(MTV_AEC_LPF_REG, 1); + mt9v034_WriteReg(MTV_AEC_LPF_REG, 1); /* * AGC Output Update Frequency * Number of frames to skip between changes in AGC * Range: 0-15 */ - mt9v034_WriteReg16(MTV_AGC_UPDATE_FREQ_REG, 2); + mt9v034_WriteReg(MTV_AGC_UPDATE_FREQ_REG, 2); /* * AGC Low Pass Filter * Range: 0-2 */ - mt9v034_WriteReg16(MTV_AGC_LPF_REG, 2); + mt9v034_WriteReg(MTV_AGC_LPF_REG, 2); /* * AGC/AEC Enable * [0] AEC Enable Ctx A: 1 = Enable @@ -281,14 +272,14 @@ static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx) { * [8] AEC Enable Ctx B: 1 = Enable * [9] AGC Enable Ctx B: 1 = Enable */ - mt9v034_WriteReg16(MTV_AEC_AGC_ENABLE_REG, 0x0303); + mt9v034_WriteReg(MTV_AEC_AGC_ENABLE_REG, 0x0303); /* * Sensor Type Control * [0] HDR Enable Ctx A: 0 = Disable * [1] Color/Mono Sensor Control: 0 = Monochrome * [8] HDR Enable Ctx B: 0 = Disable */ - mt9v034_WriteReg16(MTV_SENSOR_TYPE_CTRL_REG, 0x0000); + mt9v034_WriteReg(MTV_SENSOR_TYPE_CTRL_REG, 0x0000); /* * Digital Test Pattern @@ -304,10 +295,10 @@ static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx) { } else { test_data = 0x0000; } - mt9v034_WriteReg16(MTV_TEST_PATTERN_REG, test_data);//enable test pattern + mt9v034_WriteReg(MTV_TEST_PATTERN_REG, test_data);//enable test pattern /* Reset */ - mt9v034_WriteReg16(MTV_SOFT_RESET_REG, 0x01); + mt9v034_WriteReg(MTV_SOFT_RESET_REG, 0x01); /* * NOTES: @@ -459,36 +450,36 @@ static bool mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, switch (context_idx) { case 0: if (update_size || update_binning) { - mt9v034_WriteReg16(MTV_WINDOW_WIDTH_REG_A, width); - mt9v034_WriteReg16(MTV_WINDOW_HEIGHT_REG_A, height); - mt9v034_WriteReg16(MTV_HOR_BLANKING_REG_A, hor_blanking); - mt9v034_WriteReg16(MTV_VER_BLANKING_REG_A, ver_blanking); - mt9v034_WriteReg16(MTV_READ_MODE_REG_A, readmode); - mt9v034_WriteReg16(MTV_COLUMN_START_REG_A, col_start); - mt9v034_WriteReg16(MTV_ROW_START_REG_A, row_start); + mt9v034_WriteReg(MTV_WINDOW_WIDTH_REG_A, width); + mt9v034_WriteReg(MTV_WINDOW_HEIGHT_REG_A, height); + mt9v034_WriteReg(MTV_HOR_BLANKING_REG_A, hor_blanking); + mt9v034_WriteReg(MTV_VER_BLANKING_REG_A, ver_blanking); + mt9v034_WriteReg(MTV_READ_MODE_REG_A, readmode); + mt9v034_WriteReg(MTV_COLUMN_START_REG_A, col_start); + mt9v034_WriteReg(MTV_ROW_START_REG_A, row_start); } if (full_refresh) { - mt9v034_WriteReg16(MTV_COARSE_SW_1_REG_A, coarse_sw1); - mt9v034_WriteReg16(MTV_COARSE_SW_2_REG_A, coarse_sw2); - mt9v034_WriteReg16(MTV_COARSE_SW_CTRL_REG_A, coarse_sw_ctrl); - mt9v034_WriteReg16(MTV_COARSE_SW_TOTAL_REG_A, coarse_sw_total); + mt9v034_WriteReg(MTV_COARSE_SW_1_REG_A, coarse_sw1); + mt9v034_WriteReg(MTV_COARSE_SW_2_REG_A, coarse_sw2); + mt9v034_WriteReg(MTV_COARSE_SW_CTRL_REG_A, coarse_sw_ctrl); + mt9v034_WriteReg(MTV_COARSE_SW_TOTAL_REG_A, coarse_sw_total); } break; case 1: if (update_size || update_binning) { - mt9v034_WriteReg16(MTV_WINDOW_WIDTH_REG_B, width); - mt9v034_WriteReg16(MTV_WINDOW_HEIGHT_REG_B, height); - mt9v034_WriteReg16(MTV_HOR_BLANKING_REG_B, hor_blanking); - mt9v034_WriteReg16(MTV_VER_BLANKING_REG_B, ver_blanking); - mt9v034_WriteReg16(MTV_READ_MODE_REG_B, readmode); - mt9v034_WriteReg16(MTV_COLUMN_START_REG_B, col_start); - mt9v034_WriteReg16(MTV_ROW_START_REG_B, row_start); + mt9v034_WriteReg(MTV_WINDOW_WIDTH_REG_B, width); + mt9v034_WriteReg(MTV_WINDOW_HEIGHT_REG_B, height); + mt9v034_WriteReg(MTV_HOR_BLANKING_REG_B, hor_blanking); + mt9v034_WriteReg(MTV_VER_BLANKING_REG_B, ver_blanking); + mt9v034_WriteReg(MTV_READ_MODE_REG_B, readmode); + mt9v034_WriteReg(MTV_COLUMN_START_REG_B, col_start); + mt9v034_WriteReg(MTV_ROW_START_REG_B, row_start); } if (full_refresh) { - mt9v034_WriteReg16(MTV_COARSE_SW_1_REG_B, coarse_sw1); - mt9v034_WriteReg16(MTV_COARSE_SW_2_REG_B, coarse_sw2); - mt9v034_WriteReg16(MTV_COARSE_SW_CTRL_REG_B, coarse_sw_ctrl); - mt9v034_WriteReg16(MTV_COARSE_SW_TOTAL_REG_B, coarse_sw_total); + mt9v034_WriteReg(MTV_COARSE_SW_1_REG_B, coarse_sw1); + mt9v034_WriteReg(MTV_COARSE_SW_2_REG_B, coarse_sw2); + mt9v034_WriteReg(MTV_COARSE_SW_CTRL_REG_B, coarse_sw_ctrl); + mt9v034_WriteReg(MTV_COARSE_SW_TOTAL_REG_B, coarse_sw_total); } break; } @@ -548,105 +539,87 @@ static bool mt9v034_init_hw(mt9v034_sensor_ctx *ctx) { GPIO_ResetBits(GPIOA, GPIO_Pin_2 | GPIO_Pin_3); /* test I2C: */ - uint16_t version = mt9v034_ReadReg16(MTV_CHIP_VERSION_REG); + uint16_t version = mt9v034_ReadReg(MTV_CHIP_VERSION_REG); if (version != 0x1324) return false; return true; } -static uint8_t mt9v034_WriteReg(uint16_t Addr, uint8_t Data) { - uint32_t timeout = TIMEOUT_MAX; +static bool mt9v034_I2cWaitEvent(uint32_t i2c_event) { + uint32_t timeout = TIMEOUT_MAX; /* Initialize timeout value */ + while(!I2C_CheckEvent(I2C2, i2c_event)) { + /* If the timeout delay is exceeded, exit with error code */ + if ((timeout--) == 0) return false; + } + return true; +} +static bool mt9v034_WriteReg(uint16_t Addr, uint16_t Data) { /* Generate the Start Condition */ I2C_GenerateSTART(I2C2, ENABLE); /* Test on I2C2 EV5 and clear it */ - timeout = TIMEOUT_MAX; /* Initialize timeout value */ - while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_MODE_SELECT)) - { - /* If the timeout delay is exceeded, exit with error code */ - if ((timeout--) == 0) return 0xFF; - } + if (!mt9v034_I2cWaitEvent(I2C_EVENT_MASTER_MODE_SELECT)) + return false; /* Send DCMI selected device slave Address for write */ I2C_Send7bitAddress(I2C2, mt9v034_DEVICE_WRITE_ADDRESS, I2C_Direction_Transmitter); /* Test on I2C2 EV6 and clear it */ - timeout = TIMEOUT_MAX; /* Initialize timeout value */ - while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)) - { - /* If the timeout delay is exceeded, exit with error code */ - if ((timeout--) == 0) return 0xFF; - } + if (!mt9v034_I2cWaitEvent(I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)) + return false; /* Send I2C2 location address LSB */ I2C_SendData(I2C2, (uint8_t)(Addr)); /* Test on I2C2 EV8 and clear it */ - timeout = TIMEOUT_MAX; /* Initialize timeout value */ - while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED)) - { - /* If the timeout delay is exceeded, exit with error code */ - if ((timeout--) == 0) return 0xFF; - } + if (!mt9v034_I2cWaitEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTED)) + return false; + + /* Send Data MSB */ + I2C_SendData(I2C2, Data >> 8); + + /* Test on I2C2 EV8 and clear it */ + if (!mt9v034_I2cWaitEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTED)) + return false; - /* Send Data */ + /* Send Data LSB */ I2C_SendData(I2C2, Data); /* Test on I2C2 EV8 and clear it */ - timeout = TIMEOUT_MAX; /* Initialize timeout value */ - while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED)) - { - /* If the timeout delay is exceeded, exit with error code */ - if ((timeout--) == 0) return 0xFF; - } + if (!mt9v034_I2cWaitEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTED)) + return false; /* Send I2C2 STOP Condition */ I2C_GenerateSTOP(I2C2, ENABLE); - /* If operation is OK, return 0 */ - return 0; -} - -static uint8_t mt9v034_WriteReg16(uint16_t address, uint16_t Data) { - uint8_t result = mt9v034_WriteReg(address, (uint8_t)( Data >> 8)); // write upper byte - result |= mt9v034_WriteReg(0xF0, (uint8_t) Data); // write lower byte - return result; + /* If operation is OK, return true */ + return true; } -static uint8_t mt9v034_ReadReg(uint16_t Addr) { - uint32_t timeout = TIMEOUT_MAX; - uint8_t Data = 0; +static uint16_t mt9v034_ReadReg(uint16_t Addr) { + uint16_t Data = 0; /* Generate the Start Condition */ I2C_GenerateSTART(I2C2, ENABLE); /* Test on I2C2 EV5 and clear it */ - timeout = TIMEOUT_MAX; /* Initialize timeout value */ - while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_MODE_SELECT)) { - /* If the timeout delay is exceeded, exit with error code */ - if ((timeout--) == 0) return 0xFF; - } + if (!mt9v034_I2cWaitEvent(I2C_EVENT_MASTER_MODE_SELECT)) + return 0xFFFF; /* Send DCMI selected device slave Address for write */ I2C_Send7bitAddress(I2C2, mt9v034_DEVICE_READ_ADDRESS, I2C_Direction_Transmitter); /* Test on I2C2 EV6 and clear it */ - timeout = TIMEOUT_MAX; /* Initialize timeout value */ - while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)) { - /* If the timeout delay is exceeded, exit with error code */ - if ((timeout--) == 0) return 0xFF; - } - + if (!mt9v034_I2cWaitEvent(I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)) + return 0xFFFF; + /* Send I2C2 location address LSB */ I2C_SendData(I2C2, (uint8_t)(Addr)); /* Test on I2C2 EV8 and clear it */ - timeout = TIMEOUT_MAX; /* Initialize timeout value */ - while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED)) { - /* If the timeout delay is exceeded, exit with error code */ - if ((timeout--) == 0) return 0xFF; - } + if (!mt9v034_I2cWaitEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTED)) + return 0xFFFF; /* Clear AF flag if set */ I2C2->SR1 |= (uint16_t)0x0400; @@ -655,44 +628,39 @@ static uint8_t mt9v034_ReadReg(uint16_t Addr) { I2C_GenerateSTART(I2C2, ENABLE); /* Test on I2C2 EV6 and clear it */ - timeout = TIMEOUT_MAX; /* Initialize timeout value */ - while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_MODE_SELECT)) { - /* If the timeout delay is exceeded, exit with error code */ - if ((timeout--) == 0) return 0xFF; - } + if (!mt9v034_I2cWaitEvent(I2C_EVENT_MASTER_MODE_SELECT)) + return 0xFFFF; /* Send DCMI selected device slave Address for write */ I2C_Send7bitAddress(I2C2, mt9v034_DEVICE_READ_ADDRESS, I2C_Direction_Receiver); /* Test on I2C2 EV6 and clear it */ - timeout = TIMEOUT_MAX; /* Initialize timeout value */ - while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED)) { - /* If the timeout delay is exceeded, exit with error code */ - if ((timeout--) == 0) return 0xFF; - } + if (!mt9v034_I2cWaitEvent(I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED)) + return 0xFFFF; + + /* Prepare an ACK for the next data received */ + I2C_AcknowledgeConfig(I2C2, ENABLE); + + /* Test on I2C2 EV7 and clear it */ + if (!mt9v034_I2cWaitEvent(I2C_EVENT_MASTER_BYTE_RECEIVED)) + return 0xFFFF; + + /* Receive the Data MSB */ + Data = ((uint16_t)I2C_ReceiveData(I2C2)) << 8; /* Prepare an NACK for the next data received */ I2C_AcknowledgeConfig(I2C2, DISABLE); /* Test on I2C2 EV7 and clear it */ - timeout = TIMEOUT_MAX; /* Initialize timeout value */ - while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_RECEIVED)) { - /* If the timeout delay is exceeded, exit with error code */ - if ((timeout--) == 0) return 0xFF; - } + if (!mt9v034_I2cWaitEvent(I2C_EVENT_MASTER_BYTE_RECEIVED)) + return 0xFFFF; + + /* Receive the Data LSB */ + Data |= I2C_ReceiveData(I2C2); /* Prepare Stop after receiving data */ I2C_GenerateSTOP(I2C2, ENABLE); - /* Receive the Data */ - Data = I2C_ReceiveData(I2C2); - /* return the read data */ return Data; } - -static uint16_t mt9v034_ReadReg16(uint8_t address) { - uint16_t result = mt9v034_ReadReg(address) << 8; // read upper byte - result |= mt9v034_ReadReg(0xF0); // read lower byte - return result; -} From 917cf94b0d05fda8e8b23298188b8b6d34fad9ce Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Wed, 5 Aug 2015 22:47:43 +0200 Subject: [PATCH 056/120] implemented snapshot mode and tested it. it works! --- inc/camera.h | 28 +++++++++----- src/camera.c | 102 +++++++++++++++++++++++++++++++++++++++++--------- src/main.c | 98 +++++++++++++++++++++++++++++++++--------------- src/mt9v034.c | 32 ++++++++++++++-- 4 files changed, 200 insertions(+), 60 deletions(-) diff --git a/inc/camera.h b/inc/camera.h index 3d5ccb7..6d6833b 100644 --- a/inc/camera.h +++ b/inc/camera.h @@ -76,14 +76,14 @@ typedef struct _camera_image_buffer { uint32_t meta; ///< Buffer meta data not used by the camera driver. This can be used to identify buffers. } camera_image_buffer; -#define BuildCameraImageBuffer(memory_variable) ((camera_image_buffer){.buffer = &memory_variable, .buffer_size = sizeof(memory_variable)}) +#define BuildCameraImageBuffer(memory_variable) ((const camera_image_buffer){.buffer = memory_variable, .buffer_size = sizeof(memory_variable)}) /** * Callback which is called when a snapshot capture has finished. * @note This callback may be called from an interrupt handler. * @param Pointer to buffer which contains the snapshot. */ -typedef void (*camera_snapshot_done_cb)(camera_image_buffer *buf); +typedef void (*camera_snapshot_done_cb)(); /** * Initializes the camera driver. @@ -158,6 +158,13 @@ void camera_img_stream_return_buffers(camera_ctx *ctx, camera_image_buffer *buff */ bool camera_snapshot_schedule(camera_ctx *ctx, const camera_img_param *img_param, camera_image_buffer *dst, camera_snapshot_done_cb cb); +/** + * This function must be called right after the snapshot callback function has been called. No other calls to the camera driver are allowed. + * @note This function must not be called directly from the callback! + * @param ctx The context to use. + */ +void camera_snapshot_acknowledge(camera_ctx *ctx); + /** Camera sensor configuration interface. */ struct _camera_sensor_interface { @@ -177,6 +184,11 @@ struct _camera_sensor_interface { * @return true on success. */ bool (*prepare_update_param)(void *usr, const camera_img_param *img_param); + /** + * Immediately switches back to the previous parameters. + * @param usr User pointer from this struct. + */ + void (*restore_previous_param)(void *usr); /** * Called every frame just after readout has started (but not completed yet). * This function may be used to switch the sensor to new parameters that have been previously prepared. @@ -188,8 +200,9 @@ struct _camera_sensor_interface { * Called to retrieve the image parameters of the current frame that is being output. * This function is called after notify_readout_start to retrieve the * parameters that were in effect at the time the image was taken. + * @param img_data_valid boolean which receives a flag whether the image data is valid or not. */ - void (*get_current_param)(void *usr, camera_img_param *img_param); + void (*get_current_param)(void *usr, camera_img_param *img_param, bool *img_data_valid); }; /** @@ -257,6 +270,7 @@ struct _camera_ctx { camera_img_param snapshot_param; ///< The parameters of the snapshot mode. camera_image_buffer *snapshot_buffer; ///< Pointer to buffer which receives the snapshot. NULL when no snapshot is pending. + camera_snapshot_done_cb snapshot_cb; ///< Callback pointer which is called when the snapshot is complete. /* retrieving buffers */ @@ -277,12 +291,8 @@ struct _camera_ctx { volatile bool seq_snapshot_active; /**< Flag that is asserted as long as a snapshot is not finished. * While asserted the camera_img_stream_schedule_param_change function * should not update the sensor. It should write the new parameters to the img_stream_param variable. */ - volatile bool seq_updating_img_stream; /**< Flag that must be set when snapshot is active and a write to the img_stream_param - * variable takes place. This is used by the interrupt to avoid switching the sensor back to normal - * mode while a parameter update takes place. */ - volatile bool seq_write_img_stream_param_yourself; /**< This flag is set by the interrupt handler when seq_snapshot_active and seq_updating_img_stream - * both where active while the interrupt handler attempted to switch the sensor back to streaming mode. - * in this case the parameter update function should do the parameter update of the sensor by itself. */ + volatile bool seq_img_stream_param_pending; /**< Flag that is set when pending img stream parameter updates are in the img_stream_param variable. + * These updates will be written to the sensor in the camera_snapshot_acknowledge function. */ }; #endif /* CAMERA_H_ */ \ No newline at end of file diff --git a/src/camera.c b/src/camera.c index 40409bc..5da229f 100644 --- a/src/camera.c +++ b/src/camera.c @@ -91,8 +91,7 @@ bool camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const c ctx->target_buffer = NULL; ctx->seq_snapshot_active = false; - ctx->seq_updating_img_stream = false; - ctx->seq_write_img_stream_param_yourself = false; + ctx->seq_img_stream_param_pending = false; // initialize hardware: if (!ctx->transport->init(ctx->transport->usr, camera_transport_transfer_done_fn, @@ -179,6 +178,10 @@ void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t siz } // notify camera_img_stream_get_buffers function. ctx->new_frame_arrived = true; + } else if (ctx->seq_snapshot_active && ctx->target_buffer != NULL) { + // snapshot has been taken! + ctx->snapshot_cb(); + ctx->seq_snapshot_active = false; } // reset state: ctx->target_buffer = NULL; @@ -194,7 +197,8 @@ void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t siz ctx->sensor->notify_readout_start(ctx->sensor->usr); // get current sensor parameter: camera_img_param cparam; - ctx->sensor->get_current_param(ctx->sensor->usr, &cparam); + bool img_data_valid; + ctx->sensor->get_current_param(ctx->sensor->usr, &cparam, &img_data_valid); // update the receiving variables: ctx->cur_frame_index += 1; ctx->receiving_frame = true; @@ -202,13 +206,32 @@ void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t siz ctx->cur_frame_size = (uint32_t)cparam.size.x * (uint32_t)cparam.size.y; ctx->cur_frame_pos = size; ctx->target_buffer_index = -1; - // check that the size parameters match: - if (cparam.size.x == ctx->img_stream_param.size.x || - cparam.size.y == ctx->img_stream_param.size.y) { - // get least recently used buffer from the available buffers: - camera_buffer_fifo_remove_back(ctx, &ctx->target_buffer_index, 1); - ctx->target_buffer = &ctx->buffers[ctx->target_buffer_index]; - // initialize it: + if (!ctx->seq_snapshot_active) { + // check that the size parameters match to the img stream: + if (cparam.size.x == ctx->img_stream_param.size.x && + cparam.size.y == ctx->img_stream_param.size.y && + cparam.binning == ctx->img_stream_param.binning) { + if (img_data_valid) { + // get least recently used buffer from the available buffers: + camera_buffer_fifo_remove_back(ctx, &ctx->target_buffer_index, 1); + ctx->target_buffer = &ctx->buffers[ctx->target_buffer_index]; + } + } + } else { + // check that the size parameters match to the snapshot parameters: + if (cparam.size.x == ctx->snapshot_param.size.x && + cparam.size.y == ctx->snapshot_param.size.y && + cparam.binning == ctx->snapshot_param.binning) { + if (img_data_valid) { + // get the buffer: + ctx->target_buffer = ctx->snapshot_buffer; + // initiate switching back to img stream mode: + ctx->sensor->restore_previous_param(ctx->sensor->usr); + } + } + } + // initialize the target buffer: + if (ctx->target_buffer != NULL) { ctx->target_buffer->timestamp = get_boot_time_us(); ctx->target_buffer->frame_number = ctx->cur_frame_index; ctx->target_buffer->param = cparam; @@ -232,12 +255,63 @@ void camera_transport_frame_done_fn(void *usr) { } bool camera_img_stream_schedule_param_change(camera_ctx *ctx, const camera_img_param *img_param) { + uint32_t img_size = (uint32_t)img_param->size.x * (uint32_t)img_param->size.y; + if (img_size % ctx->transport->transfer_size != 0 || img_size / ctx->transport->transfer_size < 2) { + // invalid size parameter! + return false; + } + int i; + for (i = 0; i < ctx->buffer_count; ++i) { + // check image size against each buffer: + if (img_size > ctx->buffers[i].buffer_size) { + return false; + } + } + if (!ctx->seq_snapshot_active && !ctx->seq_img_stream_param_pending) { + if (ctx->sensor->prepare_update_param(ctx->sensor->usr, img_param)) { + ctx->img_stream_param = *img_param; + return true; + } + } else { + ctx->img_stream_param = *img_param; + ctx->seq_img_stream_param_pending = true; + return true; + } + return false; +} + +bool camera_snapshot_schedule(camera_ctx *ctx, const camera_img_param *img_param, camera_image_buffer *dst, camera_snapshot_done_cb cb) { + uint32_t img_size = (uint32_t)img_param->size.x * (uint32_t)img_param->size.y; + if (img_size % ctx->transport->transfer_size != 0 || img_size / ctx->transport->transfer_size < 2) { + // invalid size parameter! + return false; + } + // check image size against the buffer: + if (img_size > dst->buffer_size || dst->buffer == NULL) { + return false; + } if (!ctx->seq_snapshot_active) { - return ctx->sensor->prepare_update_param(ctx->sensor->usr, img_param); + if (ctx->sensor->prepare_update_param(ctx->sensor->usr, img_param)) { + ctx->snapshot_param = *img_param; + ctx->snapshot_buffer = dst; + ctx->snapshot_cb = cb; + ctx->seq_snapshot_active = true; + return true; + } } return false; } +void camera_snapshot_acknowledge(camera_ctx *ctx) { + if (!ctx->seq_snapshot_active) { + if (ctx->seq_img_stream_param_pending) { + ctx->seq_img_stream_param_pending = false; + /* write the pending changes to the sensor: */ + ctx->sensor->prepare_update_param(ctx->sensor->usr, &ctx->img_stream_param); + } + } +} + static bool camera_img_stream_get_buffers_idx(camera_ctx *ctx, int bidx[], size_t count, bool reset_new_frm) { int i; __disable_irq(); @@ -310,9 +384,3 @@ void camera_img_stream_return_buffers(camera_ctx *ctx, camera_image_buffer *buff ctx->buffers_are_reserved = false; __enable_irq(); } - -bool camera_snapshot_schedule(camera_ctx *ctx, const camera_img_param *img_param, camera_image_buffer *dst, camera_snapshot_done_cb cb) { - return false; -} - - diff --git a/src/main.c b/src/main.c index 565fdca..0a14a17 100644 --- a/src/main.c +++ b/src/main.c @@ -91,6 +91,13 @@ struct lpos_t { float vz; } lpos = {0}; +static camera_ctx cam_ctx; +static camera_img_param img_stream_param; + +uint8_t snapshot_buffer_mem[128 * 128]; + +static camera_image_buffer snapshot_buffer; + void sonar_update_fn(void) { sonar_trigger(); } @@ -109,7 +116,24 @@ void system_receive_fn(void) { communication_receive_usb(); } -uint8_t *previous_image = NULL; +const camera_image_buffer *previous_image = NULL; + +void mavlink_send_image(const camera_image_buffer *image) { + uint32_t img_size = (uint32_t)image->param.size.x * (uint32_t)image->param.size.y; + mavlink_msg_data_transmission_handshake_send( + MAVLINK_COMM_2, + MAVLINK_DATA_STREAM_IMG_RAW8U, + img_size, + image->param.size.x, + image->param.size.y, + img_size / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1, + MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN, + 100); + uint16_t frame = 0; + for (frame = 0; frame < img_size / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1; frame++) { + mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, &((uint8_t *)image->buffer)[frame * MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN]); + } +} void send_video_fn(void) { /* update the rate */ @@ -120,32 +144,8 @@ void send_video_fn(void) { /* transmit raw 8-bit image */ if (global_data.param[PARAM_USB_SEND_VIDEO] && !global_data.param[PARAM_VIDEO_ONLY]) { - /* get size of image to send */ - uint16_t image_size_send; - uint16_t image_width_send; - uint16_t image_height_send; - - uint16_t image_size = global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; - - image_size_send = image_size; - image_width_send = global_data.param[PARAM_IMAGE_WIDTH]; - image_height_send = global_data.param[PARAM_IMAGE_HEIGHT]; - - mavlink_msg_data_transmission_handshake_send( - MAVLINK_COMM_2, - MAVLINK_DATA_STREAM_IMG_RAW8U, - image_size_send, - image_width_send, - image_height_send, - image_size_send / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1, - MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN, - 100); + mavlink_send_image(previous_image); LEDToggle(LED_COM); - uint16_t frame = 0; - for (frame = 0; frame < image_size_send / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1; frame++) - { - mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, &((uint8_t *) previous_image)[frame * MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN]); - } } else if (!global_data.param[PARAM_USB_SEND_VIDEO]) { @@ -158,6 +158,32 @@ void send_params_fn(void) { communication_parameter_send(); } +/*void switch_params_fn(void) { + switch (img_stream_param.binning) { + case 1: img_stream_param.binning = 4; break; + case 2: img_stream_param.binning = 1; break; + case 4: img_stream_param.binning = 2; break; + } + camera_img_stream_schedule_param_change(&cam_ctx, &img_stream_param); +}*/ + +volatile bool snap_capture_done = false; + +void snapshot_captured_fn() { + LEDToggle(LED_COM); + snap_capture_done = true; +} + +void take_snapshot_fn(void) { + if (global_data.param[PARAM_USB_SEND_VIDEO] && global_data.param[PARAM_VIDEO_ONLY]) { + static camera_img_param snapshot_param; + snapshot_param.size.x = 128; + snapshot_param.size.y = 128; + snapshot_param.binning = 1; + camera_snapshot_schedule(&cam_ctx, &snapshot_param, &snapshot_buffer, snapshot_captured_fn); + } +} + #define FLOW_IMAGE_SIZE (64) uint8_t image_buffer_8bit_1[FLOW_IMAGE_SIZE * FLOW_IMAGE_SIZE] __attribute__((section(".ccm"))); @@ -173,6 +199,8 @@ flow_klt_image flow_klt_images[2] __attribute__((section(".ccm"))); */ int main(void) { + snapshot_buffer = BuildCameraImageBuffer(snapshot_buffer_mem); + /* load settings and parameters */ global_data_reset_param_defaults(); global_data_reset(); @@ -202,8 +230,6 @@ int main(void) communication_init(); /* initialize camera: */ - camera_ctx cam_ctx; - camera_img_param img_stream_param; img_stream_param.size.x = FLOW_IMAGE_SIZE; img_stream_param.size.y = FLOW_IMAGE_SIZE; img_stream_param.binning = 4; @@ -240,6 +266,7 @@ int main(void) timer_register(system_receive_fn, SYSTEM_STATE_MS / 2); timer_register(send_params_fn, PARAMS_MS); timer_register(send_video_fn, global_data.param[PARAM_VIDEO_RATE]); + timer_register(take_snapshot_fn, 500); /* variables */ uint32_t counter = 0; @@ -259,6 +286,14 @@ int main(void) { /* check timers */ timer_check(); + + if (snap_capture_done) { + snap_capture_done = false; + camera_snapshot_acknowledge(&cam_ctx); + /* send the snapshot! */ + LEDToggle(LED_COM); + mavlink_send_image(&snapshot_buffer); + } /* calculate focal_length in pixel */ const float focal_length_px = (global_data.param[PARAM_FOCAL_LENGTH_MM]) / (4.0f * 0.006f); //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled @@ -378,16 +413,17 @@ int main(void) /* create flow image if needed (previous_image is not needed anymore) * -> can be used for debugging purpose */ - previous_image = frames[1]->buffer; + previous_image = frames[1]; if (global_data.param[PARAM_USB_SEND_VIDEO]) { uint16_t frame_size = global_data.param[PARAM_IMAGE_WIDTH]; + uint8_t *prev_img = previous_image->buffer; for (int i = 0; i < flow_rslt_count; i++) { if (flow_rslt[i].quality > 0) { - previous_image[flow_rslt[i].at_y * frame_size + flow_rslt[i].at_x] = 255; + prev_img[flow_rslt[i].at_y * frame_size + flow_rslt[i].at_x] = 255; int ofs = (int)floor(flow_rslt[i].at_y + flow_rslt[i].y * 2 + 0.5f) * frame_size + (int)floor(flow_rslt[i].at_x + flow_rslt[i].x * 2 + 0.5f); if (ofs >= 0 && ofs < frame_size * frame_size) { - previous_image[ofs] = 200; + prev_img[ofs] = 200; } } } diff --git a/src/mt9v034.c b/src/mt9v034.c index 372b147..f4a8819 100644 --- a/src/mt9v034.c +++ b/src/mt9v034.c @@ -48,8 +48,9 @@ typedef struct _mt9v034_sensor_ctx mt9v034_sensor_ctx; bool mt9v034_init(void *usr, const camera_img_param *img_param); bool mt9v034_prepare_update_param(void *usr, const camera_img_param *img_param); +void mt9v034_restore_previous_param(void *usr); void mt9v034_notify_readout_start(void *usr); -void mt9v034_get_current_param(void *usr, camera_img_param *img_param); +void mt9v034_get_current_param(void *usr, camera_img_param *img_param, bool *img_data_valid); static bool mt9v034_init_hw(mt9v034_sensor_ctx *ctx); static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx); @@ -76,6 +77,7 @@ struct _mt9v034_sensor_ctx { volatile int cur_context; ///< The current camera sensor context that has been activated. volatile bool do_switch_context;///< When true the mt9v034_notify_readout_start function will switch the camera sensor context. volatile int desired_context; ///< The desired context when do_switch_context is true. + volatile int previous_context; ///< The previous context index. camera_img_param context_p[2]; ///< The parameters of the camera sensor register sets. (for context A and B) @@ -86,6 +88,9 @@ struct _mt9v034_sensor_ctx { /* current frame parameter simulation model */ camera_img_param cur_param; ///< The parameters of the frame that is beeing read out. + bool cur_param_data_valid; /**< Flag telling wether the image data is going to be valid. + * The sensor outputs garbage on the first frame after changing resolution. + * TODO: verify that this is always the case and why. */ camera_img_param exp_param; ///< Because exposure parameters take one more frame time to propagate to the output this holds the exposure settings. }; @@ -95,6 +100,7 @@ const camera_sensor_interface mt9v034_sensor_interface = { .usr = &mt9v034_ctx, .init = mt9v034_init, .prepare_update_param = mt9v034_prepare_update_param, + .restore_previous_param = mt9v034_restore_previous_param, .notify_readout_start = mt9v034_notify_readout_start, .get_current_param = mt9v034_get_current_param }; @@ -109,6 +115,7 @@ bool mt9v034_init(void *usr, const camera_img_param *img_param) { memset(ctx, 0, sizeof(mt9v034_sensor_ctx)); ctx->cur_context = 0; ctx->desired_context = 0; + ctx->previous_context = 0; ctx->do_switch_context = false; /* init hardware: */ if (!mt9v034_init_hw(ctx)) return false; @@ -119,6 +126,7 @@ bool mt9v034_init(void *usr, const camera_img_param *img_param) { /* initialize the cur and exp param: */ ctx->cur_param = *img_param; ctx->exp_param = *img_param; + ctx->cur_param_data_valid = false; /* do a forced context update next. */ ctx->do_switch_context = true; return true; @@ -140,11 +148,21 @@ bool mt9v034_prepare_update_param(void *usr, const camera_img_param *img_param) return false; } /* setup context switching */ + ctx->previous_context = ctx->cur_context; ctx->desired_context = new_ctx_idx; ctx->do_switch_context = true; return true; } +void mt9v034_restore_previous_param(void *usr) { + mt9v034_sensor_ctx *ctx = (mt9v034_sensor_ctx *)usr; + /* deassert pending context switch command: */ + ctx->do_switch_context = false; + /* switch back to previous context */ + ctx->desired_context = ctx->previous_context; + ctx->do_switch_context = true; +} + void mt9v034_notify_readout_start(void *usr) { mt9v034_sensor_ctx *ctx = (mt9v034_sensor_ctx *)usr; camera_img_param *cur_ctx_param = &ctx->context_p[ctx->cur_context]; @@ -154,6 +172,13 @@ void mt9v034_notify_readout_start(void *usr) { /* transfer exposure settings of currently active context into exp_param */ // There are no exposure settings yet. /* transfer other settings of currently active context into cur_param */ + if (cur_ctx_param->size.x != ctx->cur_param.size.x || + cur_ctx_param->size.y != ctx->cur_param.size.y) { + // first frame after changing resolution is invalid. + ctx->cur_param_data_valid = false; + } else { + ctx->cur_param_data_valid = true; + } ctx->cur_param.size = cur_ctx_param->size; ctx->cur_param.binning = cur_ctx_param->binning; /* handle context switching: */ @@ -167,16 +192,17 @@ void mt9v034_notify_readout_start(void *usr) { } mt9v034_WriteReg(MTV_CHIP_CONTROL_REG, ctx->chip_control_reg); /* update pixel count for AGC and AEC: */ - mt9v034_WriteReg(MTV_AGC_AEC_PIXEL_COUNT_REG, ctx_param->size.x * ctx_param->size.y); + mt9v034_WriteReg(MTV_AGC_AEC_PIXEL_COUNT_REG, (uint32_t)ctx_param->size.x * (uint32_t)ctx_param->size.y); /* done. */ ctx->do_switch_context = false; ctx->cur_context = desired_ctx_idx; } } -void mt9v034_get_current_param(void *usr, camera_img_param *img_param) { +void mt9v034_get_current_param(void *usr, camera_img_param *img_param, bool *img_data_valid) { mt9v034_sensor_ctx *ctx = (mt9v034_sensor_ctx *)usr; *img_param = ctx->cur_param; + *img_data_valid = ctx->cur_param_data_valid; } static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx) { From e8a2b602d64bc7d995ef1fc4f157c26241d1efa7 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Wed, 5 Aug 2015 22:53:23 +0200 Subject: [PATCH 057/120] default VIDEO_ONLY parameter to ON. flow calculation still works in the background. --- src/settings.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/settings.c b/src/settings.c index 244d5d5..49547c3 100644 --- a/src/settings.c +++ b/src/settings.c @@ -211,7 +211,7 @@ void global_data_reset_param_defaults(void){ global_data.param_access[PARAM_USB_SEND_QUAL_0] = READ_WRITE; - global_data.param[PARAM_VIDEO_ONLY] = 0; + global_data.param[PARAM_VIDEO_ONLY] = 1; strcpy(global_data.param_name[PARAM_VIDEO_ONLY], "VIDEO_ONLY"); global_data.param_access[PARAM_VIDEO_ONLY] = READ_WRITE; From 329ff49035903c97200120ab9341a1854124479c Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Thu, 6 Aug 2015 15:41:32 +0200 Subject: [PATCH 058/120] implemented auto exposure control algorithm wich bypasses the buggy algorithm of the camera module. - implemented dynamic general sensor parameter update. - bugfix for corrupt pixel data --- inc/camera.h | 90 ++++++++-- inc/mt9v034.h | 13 ++ src/camera.c | 198 ++++++++++++++++++---- src/dcmi.c | 2 +- src/main.c | 42 +++-- src/mt9v034.c | 441 ++++++++++++++++++++++++++++++++++--------------- src/settings.c | 2 +- 7 files changed, 594 insertions(+), 194 deletions(-) diff --git a/inc/camera.h b/inc/camera.h index 6d6833b..5310fd7 100644 --- a/inc/camera.h +++ b/inc/camera.h @@ -39,7 +39,14 @@ #include #include -#define CAMERA_MAX_BUFFER_COUNT 5 +#define CONFIG_CAMERA_MAX_BUFFER_COUNT 5 + +#define CONFIG_CAMERA_EXPOSURE_BIN_BITS (5u) +#define CONFIG_CAMERA_EXPOSURE_BIN_COUNT (1u << CONFIG_CAMERA_EXPOSURE_BIN_BITS) +#define CONFIG_CAMERA_EXTREME_OVEREXPOSE_RATIO (40u) +#define CONFIG_CAMERA_OUTLIER_RATIO (5u) +#define CONFIG_CAMERA_DESIRED_EXPOSURE_8B (200u) +#define CONFIG_CAMERA_EXPOSURE_SMOOTHING_K (0.95f) struct _camera_sensor_interface; typedef struct _camera_sensor_interface camera_sensor_interface; @@ -50,9 +57,6 @@ typedef struct _camera_transport_interface camera_transport_interface; struct _camera_ctx; typedef struct _camera_ctx camera_ctx; -/** - * Struct holding image parameters for the camera. - */ typedef struct _camera_img_param { struct _size { uint16_t x; ///< Image size in x direction. @@ -64,11 +68,20 @@ typedef struct _camera_img_param { uint8_t binning; ///< Column and row binning ratio. } camera_img_param; +/** + * Struct holding image parameters for the camera. + */ +typedef struct _camera_img_param_ex { + camera_img_param p; ///< User editable parameters. + uint32_t exposure; ///< Exposure time in master clock times. + float analog_gain; ///< Analog gain value. 1 - 4. +} camera_img_param_ex; + /** * Struct holding information about an image and a pointer to the buffer itself. */ typedef struct _camera_image_buffer { - camera_img_param param; ///< The parameters of the image that is stored in the buffer. + camera_img_param_ex param; ///< The parameters of the image that is stored in the buffer. uint32_t frame_number; ///< The frame number. This number increases with every frame that is captured from the camera module. uint32_t timestamp; ///< Frame timestamp in microseconds. void *buffer; ///< Pointer to the buffer itself. @@ -90,6 +103,9 @@ typedef void (*camera_snapshot_done_cb)(); * @param ctx The context to use. * @param sensor The sensor interface to use. * @param transport The sensor data transport interface to use. + * @param exposure_min_clks Minimum exposure in clocks. + * @param exposure_max_clks Maximum exposrue in clocks. + * @param analog_gain_max Maximum analog gain. * @param img_param The initial image parameters to use for img_stream operation mode. * @param buffers Array of initialized buffers to use for buffering the images in img_stream mode. * In each camera_image_buffer the buffer and buffer_size members must be correctly set. @@ -103,9 +119,18 @@ typedef void (*camera_snapshot_done_cb)(); * @return True when successful. */ bool camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const camera_transport_interface *transport, + uint16_t exposure_min_clks, uint16_t exposure_max_clks, float analog_gain_max, const camera_img_param *img_param, camera_image_buffer buffers[], size_t buffer_count); + +/** + * Reconfigures the general camera parameters. + * Call this after some MAVLINK parameters have changed. + * @param ctx The context to use. + */ +void camera_reconfigure_general(camera_ctx *ctx); + /** * Schedules the new parameters to take effect as soon as possible. This function makes sure that * parameter changes are commited to the sensor in a time window which will guarantee glitch free operation. @@ -175,7 +200,7 @@ struct _camera_sensor_interface { * @param img_param The image parameters to use for initialization. * @return true on success. */ - bool (*init)(void *usr, const camera_img_param *img_param); + bool (*init)(void *usr, const camera_img_param_ex *img_param); /** * Prepares the sensor to switch to new parameters. * This function should perform most of the work that is needed to update the sensor with new parameters. @@ -183,7 +208,11 @@ struct _camera_sensor_interface { * @param img_param The new image parameters. * @return true on success. */ - bool (*prepare_update_param)(void *usr, const camera_img_param *img_param); + bool (*prepare_update_param)(void *usr, const camera_img_param_ex *img_param); + /** + * Reconfigures the general sensor parameters. + */ + void (*reconfigure_general)(void *usr); /** * Immediately switches back to the previous parameters. * @param usr User pointer from this struct. @@ -196,13 +225,25 @@ struct _camera_sensor_interface { * @note This function may be called from an interrupt vector and should do as little work as necessary. */ void (*notify_readout_start)(void *usr); + /** + * Called every frame just after readout has finished. + * @param usr User pointer from this struct. + * @note This function may be called from an interrupt vector and should do as little work as necessary. + */ + void (*notify_readout_end)(void *usr); + /** + * Called at the end of the readout after notify_readout_end to update the exposure parameters of the current context. + * This should only be called when it is guaranteed that no other sensor function will be called during the time this function executes. + * It should be treated like prepare_update_param except that notify_readout_start and notify_readout_end may not run at the same time. + */ + void (*update_exposure_param)(void *usr, uint32_t exposure, float gain); /** * Called to retrieve the image parameters of the current frame that is being output. * This function is called after notify_readout_start to retrieve the * parameters that were in effect at the time the image was taken. * @param img_data_valid boolean which receives a flag whether the image data is valid or not. */ - void (*get_current_param)(void *usr, camera_img_param *img_param, bool *img_data_valid); + void (*get_current_param)(void *usr, camera_img_param_ex *img_param, bool *img_data_valid); }; /** @@ -256,19 +297,36 @@ struct _camera_ctx { const camera_sensor_interface *sensor; ///< Sensor interface. const camera_transport_interface *transport; ///< Transport interface. + uint32_t exposure_min_clks; + uint32_t exposure_max_clks; + float analog_gain_max; + + /* exposure control */ + + float analog_gain; + uint32_t exposure; + + float exposure_smoothing; + + uint16_t exposure_sampling_stride; + uint16_t exposure_bins[CONFIG_CAMERA_EXPOSURE_BIN_COUNT]; + uint16_t exposure_hist_count; + int last_brightness; + /* image streaming buffer and parameters */ - camera_img_param img_stream_param; ///< The parameters of the image streaming mode. - camera_image_buffer buffers[CAMERA_MAX_BUFFER_COUNT]; ///< The image buffers for image stream mode. + camera_img_param_ex img_stream_param; ///< The parameters of the image streaming mode. + camera_img_param pend_img_stream_param; ///< The pending image streaming mode parameters. + camera_image_buffer buffers[CONFIG_CAMERA_MAX_BUFFER_COUNT]; ///< The image buffers for image stream mode. int buffer_count; ///< Total number of buffers. - volatile uint8_t avail_bufs[CAMERA_MAX_BUFFER_COUNT]; ///< Indexes to the buffers that are available. Ordered in the MRU order. + volatile uint8_t avail_bufs[CONFIG_CAMERA_MAX_BUFFER_COUNT]; ///< Indexes to the buffers that are available. Ordered in the MRU order. volatile uint8_t avail_buf_count; ///< Number of buffer indexes in the avail_bufs array. volatile uint8_t put_back_buf_pos; ///< Position where to put back the reserved buffers. volatile bool new_frame_arrived; ///< Flag which is set by the interrupt handler to notify that a new frame has arrived. /* image snapshot buffer and parameters */ - camera_img_param snapshot_param; ///< The parameters of the snapshot mode. + camera_img_param_ex snapshot_param; ///< The parameters of the snapshot mode. camera_image_buffer *snapshot_buffer; ///< Pointer to buffer which receives the snapshot. NULL when no snapshot is pending. camera_snapshot_done_cb snapshot_cb; ///< Callback pointer which is called when the snapshot is complete. @@ -279,6 +337,7 @@ struct _camera_ctx { /* frame acquisition */ + camera_img_param_ex cur_frame_param; uint32_t cur_frame_index; bool receiving_frame; camera_image_buffer *target_buffer; @@ -288,10 +347,13 @@ struct _camera_ctx { /* sequencing */ + volatile bool seq_updating_sensor; + volatile bool seq_repeat_updating_sensor; volatile bool seq_snapshot_active; /**< Flag that is asserted as long as a snapshot is not finished. * While asserted the camera_img_stream_schedule_param_change function - * should not update the sensor. It should write the new parameters to the img_stream_param variable. */ - volatile bool seq_img_stream_param_pending; /**< Flag that is set when pending img stream parameter updates are in the img_stream_param variable. + * should not update the sensor. It should write the new parameters to the pend_img_stream_param variable. */ + volatile bool seq_pend_reconfigure_general; /**< Flag signalling that a general reconfiguration is pending. */ + volatile bool seq_pend_img_stream_param; /**< Flag that is set when pending img stream parameter updates are in the pend_img_stream_param variable. * These updates will be written to the sensor in the camera_snapshot_acknowledge function. */ }; diff --git a/inc/mt9v034.h b/inc/mt9v034.h index b418260..c1eb3d4 100644 --- a/inc/mt9v034.h +++ b/inc/mt9v034.h @@ -44,6 +44,8 @@ */ const camera_sensor_interface *mt9v034_get_sensor_interface(); +uint32_t mt9v034_get_clks_per_row(uint16_t width, int binning); + /** * Configures the maximum exposure time in number of image rows. * Exposure should not affect frame time. @@ -56,6 +58,17 @@ const camera_sensor_interface *mt9v034_get_sensor_interface(); * Higher gain means more noise. */ #define CONFIG_MAX_ANALOG_GAIN (64) +/** + * Configures the workaround to eliminate corrupted pixel data. + * According to the datasheet the image data should be sampled at the rising edge of the clock. + * In binning mode 2 and 4 however this occasionally causes corrupted pixel data. + * If we sample on the falling edge of the clock there will be corrupted pixels in no binning mode. + * + * This workaround inverts the pixel clock at the correct time when switching binning modes. + * + * Provide 3 booleans which tell whether to invert the pixel clock for 1x, 2x and 4x binning mode. + */ +#define CONFIG_CLOCK_INVERSION_WORKAROUND {false, true, true} /* Constants */ #define TIMEOUT_MAX 10000 diff --git a/src/camera.c b/src/camera.c index 5da229f..5f005e1 100644 --- a/src/camera.c +++ b/src/camera.c @@ -46,13 +46,14 @@ void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t siz void camera_transport_frame_done_fn(void *usr); bool camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const camera_transport_interface *transport, + uint16_t exposure_min_clks, uint16_t exposure_max_clks, float analog_gain_max, const camera_img_param *img_param, camera_image_buffer buffers[], size_t buffer_count) { memset(ctx, 0, sizeof(camera_ctx)); ctx->sensor = sensor; ctx->transport = transport; // check parameter: - if (buffer_count > CAMERA_MAX_BUFFER_COUNT && buffer_count < 2) { + if (buffer_count > CONFIG_CAMERA_MAX_BUFFER_COUNT && buffer_count < 2) { return false; } uint32_t img_size = (uint32_t)img_param->size.x * (uint32_t)img_param->size.y; @@ -62,7 +63,19 @@ bool camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const c } // initialize state: ctx->startup_discard_frame_count = -1; - ctx->img_stream_param = *img_param; + ctx->exposure_min_clks = exposure_min_clks; + ctx->exposure_max_clks = exposure_max_clks; + ctx->analog_gain_max = analog_gain_max; + ctx->analog_gain = 1; + ctx->exposure = ctx->exposure_min_clks; + ctx->exposure_smoothing = ctx->exposure_min_clks; + ctx->exposure_sampling_stride = 0; + memset(ctx->exposure_bins, 0, sizeof(ctx->exposure_bins)); + ctx->exposure_hist_count = 0; + ctx->img_stream_param.p = *img_param; + ctx->img_stream_param.analog_gain = ctx->analog_gain; + ctx->img_stream_param.exposure = ctx->exposure; + ctx->snapshot_param = ctx->img_stream_param; int i; for (i = 0; i < buffer_count; ++i) { // check the buffer: @@ -73,7 +86,7 @@ bool camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const c ctx->buffers[i] = buffers[i]; ctx->buffers[i].frame_number = 0; ctx->buffers[i].timestamp = get_boot_time_us(); - ctx->buffers[i].param = *img_param; + ctx->buffers[i].param = ctx->img_stream_param; memset(ctx->buffers[i].buffer, 0, img_size); // init the avail_bufs array: ctx->avail_bufs[i] = i; @@ -91,7 +104,7 @@ bool camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const c ctx->target_buffer = NULL; ctx->seq_snapshot_active = false; - ctx->seq_img_stream_param_pending = false; + ctx->seq_pend_img_stream_param = false; // initialize hardware: if (!ctx->transport->init(ctx->transport->usr, camera_transport_transfer_done_fn, @@ -99,7 +112,7 @@ bool camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const c ctx)) { return false; } - if (!ctx->sensor->init(ctx->sensor->usr, img_param)) { + if (!ctx->sensor->init(ctx->sensor->usr, &ctx->img_stream_param)) { return false; } /* after initialization start the discard count down! */ @@ -154,12 +167,51 @@ static void camera_buffer_fifo_push_at(camera_ctx *ctx, size_t pos, const int *i ctx->avail_buf_count = bc + count; } +static void camera_update_exposure_hist(camera_ctx *ctx, const uint8_t *buffer, size_t size) { + if (ctx->exposure_sampling_stride > 0) { + int i; + int c = 0; + for (i = ctx->exposure_sampling_stride / 2; i < size; i += ctx->exposure_sampling_stride) { + int idx = (buffer[i] >> (8u - CONFIG_CAMERA_EXPOSURE_BIN_BITS)); + ctx->exposure_bins[idx]++; + c++; + } + ctx->exposure_hist_count += c; + } +} + +static int camera_exposure_hist_extract_brightness_8b(camera_ctx *ctx) { + /* test for over-exposed image: */ + int value = CONFIG_CAMERA_EXPOSURE_BIN_COUNT - 1; + if ((uint32_t)ctx->exposure_bins[value] * 100u > + ((uint32_t)ctx->exposure_hist_count) * CONFIG_CAMERA_EXTREME_OVEREXPOSE_RATIO) { + return 256; + } + /* find the bin where the outliers have been ignored: */ + uint32_t pix_rem = ((uint32_t)ctx->exposure_hist_count * (uint32_t)CONFIG_CAMERA_OUTLIER_RATIO) / 100u; + while (ctx->exposure_bins[value] < pix_rem && value > 0) { + pix_rem -= ctx->exposure_bins[value]; + value--; + } + /* expand to 8bits: */ + uint32_t bin_size = 1 << (8u - CONFIG_CAMERA_EXPOSURE_BIN_BITS); + value = value * bin_size; + /* interpolate linear: */ + if (ctx->exposure_bins[value] > 0) { + value += bin_size - 1 - (((bin_size - 1) * pix_rem) / ctx->exposure_bins[value]); + } else { + value += bin_size - 1; + } + return value; +} + void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t size) { camera_ctx *ctx = (camera_ctx *)usr; if (ctx->startup_discard_frame_count != 0) { return; } if (ctx->receiving_frame) { + camera_update_exposure_hist(ctx, buffer, size); // check if we have a target buffer: if (ctx->target_buffer != NULL) { uint32_t_memcpy((uint32_t *)((uint8_t *)ctx->target_buffer->buffer + ctx->cur_frame_pos), @@ -170,6 +222,7 @@ void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t siz // check if we are done: if (ctx->cur_frame_pos >= ctx->cur_frame_size) { // frame done! + ctx->sensor->notify_readout_end(ctx->sensor->usr); if (ctx->target_buffer_index >= 0) { // put back into available buffers (in the front) camera_buffer_fifo_push_at(ctx, 0, &ctx->target_buffer_index, 1); @@ -183,6 +236,48 @@ void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t siz ctx->snapshot_cb(); ctx->seq_snapshot_active = false; } + /* handle auto-exposure: */ + if (ctx->exposure_sampling_stride > 0) { + float exposure = (float)ctx->cur_frame_param.exposure * ctx->cur_frame_param.analog_gain; + // analyze histogram: + int brightness = camera_exposure_hist_extract_brightness_8b(ctx); + ctx->last_brightness = brightness; + if (brightness >= 256) { + // extremely over-exposed! halve the integration time: + exposure = exposure * 0.33f; + } else { + if (brightness < CONFIG_CAMERA_DESIRED_EXPOSURE_8B / 3) { + // extremely under-exposed! double the integration time: + exposure = exposure * 3.f; + } else { + // determine optimal exposure for next frame: + exposure = (exposure * CONFIG_CAMERA_DESIRED_EXPOSURE_8B) / brightness; + } + } + /* clip the value within bounds: */ + if (exposure < (float)ctx->exposure_min_clks) { + exposure = ctx->exposure_min_clks; + } else if (exposure > (float)ctx->exposure_max_clks * ctx->analog_gain_max) { + exposure = (float)ctx->exposure_max_clks * ctx->analog_gain_max; + } + ctx->exposure_smoothing = CONFIG_CAMERA_EXPOSURE_SMOOTHING_K * ctx->exposure_smoothing + + (1.0f - CONFIG_CAMERA_EXPOSURE_SMOOTHING_K) * exposure; + /* update the exposure and analog gain values: */ + if (ctx->exposure_smoothing > ctx->exposure_max_clks) { + ctx->exposure = ctx->exposure_max_clks; + ctx->analog_gain = ctx->exposure_smoothing / (float)ctx->exposure_max_clks; + } else { + ctx->exposure = ctx->exposure_smoothing; + ctx->analog_gain = 1; + } + /* update the sensor! */ + if (!ctx->seq_updating_sensor) { + /* update it ourself! */ + ctx->sensor->update_exposure_param(ctx->sensor->usr, ctx->exposure, ctx->analog_gain); + } else { + ctx->seq_repeat_updating_sensor = true; + } + } // reset state: ctx->target_buffer = NULL; ctx->receiving_frame = false; @@ -196,21 +291,19 @@ void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t siz // update the sensor: (this might trigger some I2C transfers) ctx->sensor->notify_readout_start(ctx->sensor->usr); // get current sensor parameter: - camera_img_param cparam; bool img_data_valid; - ctx->sensor->get_current_param(ctx->sensor->usr, &cparam, &img_data_valid); + ctx->sensor->get_current_param(ctx->sensor->usr, &ctx->cur_frame_param, &img_data_valid); // update the receiving variables: ctx->cur_frame_index += 1; ctx->receiving_frame = true; ctx->target_buffer = NULL; - ctx->cur_frame_size = (uint32_t)cparam.size.x * (uint32_t)cparam.size.y; + ctx->cur_frame_size = (uint32_t)ctx->cur_frame_param.p.size.x * (uint32_t)ctx->cur_frame_param.p.size.y; ctx->cur_frame_pos = size; ctx->target_buffer_index = -1; if (!ctx->seq_snapshot_active) { // check that the size parameters match to the img stream: - if (cparam.size.x == ctx->img_stream_param.size.x && - cparam.size.y == ctx->img_stream_param.size.y && - cparam.binning == ctx->img_stream_param.binning) { + if (ctx->cur_frame_param.p.size.x == ctx->img_stream_param.p.size.x && + ctx->cur_frame_param.p.size.y == ctx->img_stream_param.p.size.y) { if (img_data_valid) { // get least recently used buffer from the available buffers: camera_buffer_fifo_remove_back(ctx, &ctx->target_buffer_index, 1); @@ -219,9 +312,9 @@ void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t siz } } else { // check that the size parameters match to the snapshot parameters: - if (cparam.size.x == ctx->snapshot_param.size.x && - cparam.size.y == ctx->snapshot_param.size.y && - cparam.binning == ctx->snapshot_param.binning) { + if (ctx->cur_frame_param.p.size.x == ctx->snapshot_param.p.size.x && + ctx->cur_frame_param.p.size.y == ctx->snapshot_param.p.size.y && + ctx->cur_frame_param.p.binning == ctx->snapshot_param.p.binning) { if (img_data_valid) { // get the buffer: ctx->target_buffer = ctx->snapshot_buffer; @@ -234,10 +327,25 @@ void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t siz if (ctx->target_buffer != NULL) { ctx->target_buffer->timestamp = get_boot_time_us(); ctx->target_buffer->frame_number = ctx->cur_frame_index; - ctx->target_buffer->param = cparam; + ctx->target_buffer->param = ctx->cur_frame_param; // write data to it: (at position 0) uint32_t_memcpy((uint32_t *)ctx->target_buffer->buffer, buf, size_w); } + // initialize exposure measuring: + if (img_data_valid) { + /* make sure no more than 65536 pixels are sampled: */ + uint16_t skip = ctx->cur_frame_size / 65536u + 1; + if (skip < 4) { + skip = 4; + } + ctx->exposure_sampling_stride = skip; + // reset histogram: + ctx->exposure_hist_count = 0; + memset(ctx->exposure_bins, 0, sizeof(ctx->exposure_bins)); + } else { + ctx->exposure_sampling_stride = 0; + } + camera_update_exposure_hist(ctx, (const uint8_t *)buf, size); } } @@ -254,6 +362,37 @@ void camera_transport_frame_done_fn(void *usr) { } } +void camera_reconfigure_general(camera_ctx *ctx) { + if (!ctx->seq_snapshot_active && !ctx->seq_pend_reconfigure_general) { + ctx->sensor->reconfigure_general(ctx->sensor->usr); + } else { + ctx->seq_pend_reconfigure_general = true; + } +} + +static bool camera_update_sensor_param(camera_ctx *ctx, const camera_img_param *img_param, camera_img_param_ex *ptoup) { + camera_img_param_ex p = *ptoup; + p.p = *img_param; + bool result = false; + do { + ctx->seq_updating_sensor = true; + ctx->seq_repeat_updating_sensor = false; + /* update exposure and analog gain: */ + p.exposure = ctx->exposure; + p.analog_gain = ctx->analog_gain; + /* write: */ + if (ctx->sensor->prepare_update_param(ctx->sensor->usr, &p)) { + *ptoup = p; + result = true; + } else { + /* restore previous: */ + p = *ptoup; + } + ctx->seq_updating_sensor = false; + } while (ctx->seq_repeat_updating_sensor); + return result; +} + bool camera_img_stream_schedule_param_change(camera_ctx *ctx, const camera_img_param *img_param) { uint32_t img_size = (uint32_t)img_param->size.x * (uint32_t)img_param->size.y; if (img_size % ctx->transport->transfer_size != 0 || img_size / ctx->transport->transfer_size < 2) { @@ -267,14 +406,11 @@ bool camera_img_stream_schedule_param_change(camera_ctx *ctx, const camera_img_p return false; } } - if (!ctx->seq_snapshot_active && !ctx->seq_img_stream_param_pending) { - if (ctx->sensor->prepare_update_param(ctx->sensor->usr, img_param)) { - ctx->img_stream_param = *img_param; - return true; - } + if (!ctx->seq_snapshot_active && !ctx->seq_pend_img_stream_param) { + return camera_update_sensor_param(ctx, img_param, &ctx->img_stream_param); } else { - ctx->img_stream_param = *img_param; - ctx->seq_img_stream_param_pending = true; + ctx->pend_img_stream_param = *img_param; + ctx->seq_pend_img_stream_param = true; return true; } return false; @@ -291,23 +427,21 @@ bool camera_snapshot_schedule(camera_ctx *ctx, const camera_img_param *img_param return false; } if (!ctx->seq_snapshot_active) { - if (ctx->sensor->prepare_update_param(ctx->sensor->usr, img_param)) { - ctx->snapshot_param = *img_param; - ctx->snapshot_buffer = dst; - ctx->snapshot_cb = cb; - ctx->seq_snapshot_active = true; - return true; - } + return camera_update_sensor_param(ctx, img_param, &ctx->snapshot_param); } return false; } void camera_snapshot_acknowledge(camera_ctx *ctx) { if (!ctx->seq_snapshot_active) { - if (ctx->seq_img_stream_param_pending) { - ctx->seq_img_stream_param_pending = false; + if (ctx->seq_pend_reconfigure_general) { + ctx->seq_pend_reconfigure_general = false; + ctx->sensor->reconfigure_general(ctx->sensor->usr); + } + if (ctx->seq_pend_img_stream_param) { + ctx->seq_pend_img_stream_param = false; /* write the pending changes to the sensor: */ - ctx->sensor->prepare_update_param(ctx->sensor->usr, &ctx->img_stream_param); + camera_update_sensor_param(ctx, &ctx->pend_img_stream_param, &ctx->img_stream_param); } } } diff --git a/src/dcmi.c b/src/dcmi.c index 296e948..38128c9 100644 --- a/src/dcmi.c +++ b/src/dcmi.c @@ -299,7 +299,7 @@ static void dcmi_dma_init() { /* DCMI configuration */ DCMI_InitStructure.DCMI_CaptureMode = DCMI_CaptureMode_Continuous; DCMI_InitStructure.DCMI_SynchroMode = DCMI_SynchroMode_Hardware; - DCMI_InitStructure.DCMI_PCKPolarity = DCMI_PCKPolarity_Falling; + DCMI_InitStructure.DCMI_PCKPolarity = DCMI_PCKPolarity_Rising; DCMI_InitStructure.DCMI_VSPolarity = DCMI_VSPolarity_Low; DCMI_InitStructure.DCMI_HSPolarity = DCMI_HSPolarity_Low; DCMI_InitStructure.DCMI_CaptureRate = DCMI_CaptureRate_All_Frame; diff --git a/src/main.c b/src/main.c index 0a14a17..0b5ce56 100644 --- a/src/main.c +++ b/src/main.c @@ -119,13 +119,13 @@ void system_receive_fn(void) { const camera_image_buffer *previous_image = NULL; void mavlink_send_image(const camera_image_buffer *image) { - uint32_t img_size = (uint32_t)image->param.size.x * (uint32_t)image->param.size.y; + uint32_t img_size = (uint32_t)image->param.p.size.x * (uint32_t)image->param.p.size.y; mavlink_msg_data_transmission_handshake_send( MAVLINK_COMM_2, MAVLINK_DATA_STREAM_IMG_RAW8U, img_size, - image->param.size.x, - image->param.size.y, + image->param.p.size.x, + image->param.p.size.y, img_size / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1, MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN, 100); @@ -158,16 +158,17 @@ void send_params_fn(void) { communication_parameter_send(); } -/*void switch_params_fn(void) { +void switch_params_fn(void) { switch (img_stream_param.binning) { case 1: img_stream_param.binning = 4; break; case 2: img_stream_param.binning = 1; break; case 4: img_stream_param.binning = 2; break; } camera_img_stream_schedule_param_change(&cam_ctx, &img_stream_param); -}*/ +} -volatile bool snap_capture_done = false; +static volatile bool snap_capture_done = false; +static bool snap_ready = true; void snapshot_captured_fn() { LEDToggle(LED_COM); @@ -175,24 +176,30 @@ void snapshot_captured_fn() { } void take_snapshot_fn(void) { - if (global_data.param[PARAM_USB_SEND_VIDEO] && global_data.param[PARAM_VIDEO_ONLY]) { + if (global_data.param[PARAM_USB_SEND_VIDEO] && global_data.param[PARAM_VIDEO_ONLY] && snap_ready) { static camera_img_param snapshot_param; snapshot_param.size.x = 128; snapshot_param.size.y = 128; snapshot_param.binning = 1; - camera_snapshot_schedule(&cam_ctx, &snapshot_param, &snapshot_buffer, snapshot_captured_fn); + if (camera_snapshot_schedule(&cam_ctx, &snapshot_param, &snapshot_buffer, snapshot_captured_fn)) { + snap_ready = false; + } } } +void notify_changed_camera_parameters() { + camera_reconfigure_general(&cam_ctx); +} + #define FLOW_IMAGE_SIZE (64) -uint8_t image_buffer_8bit_1[FLOW_IMAGE_SIZE * FLOW_IMAGE_SIZE] __attribute__((section(".ccm"))); -uint8_t image_buffer_8bit_2[FLOW_IMAGE_SIZE * FLOW_IMAGE_SIZE] __attribute__((section(".ccm"))); -uint8_t image_buffer_8bit_3[FLOW_IMAGE_SIZE * FLOW_IMAGE_SIZE] __attribute__((section(".ccm"))); -uint8_t image_buffer_8bit_4[FLOW_IMAGE_SIZE * FLOW_IMAGE_SIZE] __attribute__((section(".ccm"))); -uint8_t image_buffer_8bit_5[FLOW_IMAGE_SIZE * FLOW_IMAGE_SIZE] __attribute__((section(".ccm"))); +static uint8_t image_buffer_8bit_1[FLOW_IMAGE_SIZE * FLOW_IMAGE_SIZE] __attribute__((section(".ccm"))); +static uint8_t image_buffer_8bit_2[FLOW_IMAGE_SIZE * FLOW_IMAGE_SIZE] __attribute__((section(".ccm"))); +static uint8_t image_buffer_8bit_3[FLOW_IMAGE_SIZE * FLOW_IMAGE_SIZE] __attribute__((section(".ccm"))); +static uint8_t image_buffer_8bit_4[FLOW_IMAGE_SIZE * FLOW_IMAGE_SIZE] __attribute__((section(".ccm"))); +static uint8_t image_buffer_8bit_5[FLOW_IMAGE_SIZE * FLOW_IMAGE_SIZE] __attribute__((section(".ccm"))); -flow_klt_image flow_klt_images[2] __attribute__((section(".ccm"))); +static flow_klt_image flow_klt_images[2] __attribute__((section(".ccm"))); /** * @brief Main function. @@ -242,6 +249,7 @@ int main(void) BuildCameraImageBuffer(image_buffer_8bit_5) }; camera_init(&cam_ctx, mt9v034_get_sensor_interface(), dcmi_get_transport_interface(), + mt9v034_get_clks_per_row(64, 4) * 1, mt9v034_get_clks_per_row(64, 4) * 64, 2.0, &img_stream_param, buffers, 5); } @@ -267,6 +275,7 @@ int main(void) timer_register(send_params_fn, PARAMS_MS); timer_register(send_video_fn, global_data.param[PARAM_VIDEO_RATE]); timer_register(take_snapshot_fn, 500); + timer_register(switch_params_fn, 2000); /* variables */ uint32_t counter = 0; @@ -290,6 +299,7 @@ int main(void) if (snap_capture_done) { snap_capture_done = false; camera_snapshot_acknowledge(&cam_ctx); + snap_ready = true; /* send the snapshot! */ LEDToggle(LED_COM); mavlink_send_image(&snapshot_buffer); @@ -341,7 +351,7 @@ int main(void) // the image is new. apply pre-processing: /* filter the new image */ if (global_data.param[PARAM_ALGORITHM_IMAGE_FILTER]) { - filter_image(frames[i]->buffer, frames[i]->param.size.x); + filter_image(frames[i]->buffer, frames[i]->param.p.size.x); } /* update meta data to mark it as an up-to date image: */ frames[i]->meta = frames[i]->frame_number; @@ -476,6 +486,8 @@ int main(void) mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "TIMING", get_boot_time_us(), computaiton_time_us, fps, fps_skip); } + mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "EXPOSURE", get_boot_time_us(), + frames[0]->param.exposure, frames[0]->param.analog_gain, cam_ctx.last_brightness); /* calculate the output values */ result_accumulator_output_flow output_flow; diff --git a/src/mt9v034.c b/src/mt9v034.c index f4a8819..dcedbc7 100644 --- a/src/mt9v034.c +++ b/src/mt9v034.c @@ -46,15 +46,18 @@ struct _mt9v034_sensor_ctx; typedef struct _mt9v034_sensor_ctx mt9v034_sensor_ctx; -bool mt9v034_init(void *usr, const camera_img_param *img_param); -bool mt9v034_prepare_update_param(void *usr, const camera_img_param *img_param); +bool mt9v034_init(void *usr, const camera_img_param_ex *img_param); +bool mt9v034_prepare_update_param(void *usr, const camera_img_param_ex *img_param); +void mt9v034_reconfigure_general(void *usr); void mt9v034_restore_previous_param(void *usr); void mt9v034_notify_readout_start(void *usr); -void mt9v034_get_current_param(void *usr, camera_img_param *img_param, bool *img_data_valid); +void mt9v034_notify_readout_end(void *usr); +void mt9v034_update_exposure_param(void *usr, uint32_t exposure, float gain); +void mt9v034_get_current_param(void *usr, camera_img_param_ex *img_param, bool *img_data_valid); static bool mt9v034_init_hw(mt9v034_sensor_ctx *ctx); -static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx); -static bool mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, const camera_img_param *img_param, bool full_refresh); +static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx, bool initial); +static bool mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, const camera_img_param_ex *img_param, bool full_refresh); /** * @brief Reads from a specific Camera register @@ -72,6 +75,10 @@ static uint16_t mt9v034_ReadReg(uint16_t Addr); static bool mt9v034_WriteReg(uint16_t Addr, uint16_t Data); struct _mt9v034_sensor_ctx { + /* sequencing */ + + volatile bool seq_i2c_in_use; + /* context switching */ volatile int cur_context; ///< The current camera sensor context that has been activated. @@ -79,19 +86,22 @@ struct _mt9v034_sensor_ctx { volatile int desired_context; ///< The desired context when do_switch_context is true. volatile int previous_context; ///< The previous context index. - camera_img_param context_p[2]; ///< The parameters of the camera sensor register sets. (for context A and B) + camera_img_param_ex context_p[2]; ///< The parameters of the camera sensor register sets. (for context A and B) /* mirrored registers of the sensor */ uint16_t chip_control_reg; ///< The current chip control register value. + uint16_t pixel_frame_line_ctrl_reg; ///< The current pixel frame, line control register value. + uint16_t ver_blnk_ctx_a_reg; ///< Vertical blanking context A register value. + uint16_t ver_blnk_ctx_b_reg; ///< Vertical blanking context A register value. /* current frame parameter simulation model */ - camera_img_param cur_param; ///< The parameters of the frame that is beeing read out. + camera_img_param_ex cur_param; ///< The parameters of the frame that is beeing read out. bool cur_param_data_valid; /**< Flag telling wether the image data is going to be valid. * The sensor outputs garbage on the first frame after changing resolution. * TODO: verify that this is always the case and why. */ - camera_img_param exp_param; ///< Because exposure parameters take one more frame time to propagate to the output this holds the exposure settings. + camera_img_param_ex exp_param; ///< Because exposure parameters take one more frame time to propagate to the output this holds the exposure settings. }; static mt9v034_sensor_ctx mt9v034_ctx; @@ -100,8 +110,11 @@ const camera_sensor_interface mt9v034_sensor_interface = { .usr = &mt9v034_ctx, .init = mt9v034_init, .prepare_update_param = mt9v034_prepare_update_param, + .reconfigure_general = mt9v034_reconfigure_general, .restore_previous_param = mt9v034_restore_previous_param, .notify_readout_start = mt9v034_notify_readout_start, + .notify_readout_end = mt9v034_notify_readout_end, + .update_exposure_param= mt9v034_update_exposure_param, .get_current_param = mt9v034_get_current_param }; @@ -109,7 +122,23 @@ const camera_sensor_interface *mt9v034_get_sensor_interface() { return &mt9v034_sensor_interface; } -bool mt9v034_init(void *usr, const camera_img_param *img_param) { +uint32_t mt9v034_get_clks_per_row(uint16_t width, int binning) { + width = width * binning; + uint16_t hor_blanking = 61; + /* init to minimum: */ + switch (binning) { + case 1: hor_blanking = 61; break; + case 2: hor_blanking = 71; break; + case 4: hor_blanking = 91; break; + } + /* increase if window is smaller: */ + if (width < 690 - hor_blanking) { + hor_blanking = 690 - width; + } + return width + hor_blanking; +} + +bool mt9v034_init(void *usr, const camera_img_param_ex *img_param) { mt9v034_sensor_ctx *ctx = (mt9v034_sensor_ctx *)usr; /* init context: */ memset(ctx, 0, sizeof(mt9v034_sensor_ctx)); @@ -117,12 +146,13 @@ bool mt9v034_init(void *usr, const camera_img_param *img_param) { ctx->desired_context = 0; ctx->previous_context = 0; ctx->do_switch_context = false; + ctx->seq_i2c_in_use = false; /* init hardware: */ if (!mt9v034_init_hw(ctx)) return false; /* configure sensor: */ mt9v034_configure_context(ctx, 0, img_param, true); mt9v034_configure_context(ctx, 1, img_param, true); - mt9v034_configure_general(ctx); + mt9v034_configure_general(ctx, true); /* initialize the cur and exp param: */ ctx->cur_param = *img_param; ctx->exp_param = *img_param; @@ -132,28 +162,129 @@ bool mt9v034_init(void *usr, const camera_img_param *img_param) { return true; } -bool mt9v034_prepare_update_param(void *usr, const camera_img_param *img_param) { +void mt9v034_reconfigure_general(void *usr) { + mt9v034_sensor_ctx *ctx = (mt9v034_sensor_ctx *)usr; + /* signal that we are going to use i2c: */ + ctx->seq_i2c_in_use = true; + /* reconfigure general settings: */ + mt9v034_configure_general(ctx, false); + /* i2c usage done: */ + ctx->seq_i2c_in_use = false; +} + +bool mt9v034_prepare_update_param(void *usr, const camera_img_param_ex *img_param) { mt9v034_sensor_ctx *ctx = (mt9v034_sensor_ctx *)usr; /* deassert pending context switch command: */ - ctx->do_switch_context = false; + ctx->seq_i2c_in_use = true; /* determine which context to use to do the parameter update: */ int cur_ctx_idx = ctx->cur_context; int new_ctx_idx = !cur_ctx_idx; if (!mt9v034_configure_context(ctx, new_ctx_idx, img_param, false)) { // invalid parameters. - // check if there was a pending context switch when we started: - if (cur_ctx_idx != ctx->desired_context) { - ctx->do_switch_context = true; - } + ctx->seq_i2c_in_use = false; return false; } /* setup context switching */ ctx->previous_context = ctx->cur_context; ctx->desired_context = new_ctx_idx; ctx->do_switch_context = true; + ctx->seq_i2c_in_use = false; return true; } +void mt9v034_update_exposure_param(void *usr, uint32_t exposure, float gain) { + mt9v034_sensor_ctx *ctx = (mt9v034_sensor_ctx *)usr; + bool update_exposure; + bool update_gain; + int context_idx = ctx->desired_context; + camera_img_param_ex *ctx_param = &ctx->context_p[context_idx]; + + /* image dimensions */ + uint16_t width = ctx_param->p.size.x * ctx_param->p.binning; + + /* + * Horizontal Blanking + * Number of blank columns in a row. + * Minimum horizontal blanking is 61 for normal mode, + * 71 for column bin 2 mode, and 91 for column bin 4 mode. + * + * The MT9V034 uses column parallel analog-digital converters, thus short row timing is not possible. + * The minimum total row time is 690 columns (horizontal width + horizontal blanking). The minimum + * horizontal blanking is 61( / 71 / 91). When the window width is set below 627, horizontal blanking + * must be increased. + * + * In old code for bin 4 and 64 pixels it was 350 + 91. + */ + uint16_t hor_blanking = 61; + /* init to minimum: */ + switch (ctx_param->p.binning) { + case 1: hor_blanking = 61; break; + case 2: hor_blanking = 71; break; + case 4: hor_blanking = 91; break; + } + /* increase if window is smaller: */ + if (width < 690 - hor_blanking) { + hor_blanking = 690 - width; + } + + uint16_t row_time = hor_blanking + width; + + /* + * Coarse Shutter Width Total + * Total integration time in number of rows. This value is used only when AEC is disabled. + * Default: 0x01E0 + */ + uint16_t coarse_sw_total = exposure / row_time; + //uint16_t fine_sw_total = img_param->exposure % row_time; + /* ensure minimum: */ + if (coarse_sw_total < 1) { + coarse_sw_total = 1; + } + uint32_t real_exposure = coarse_sw_total * row_time; + if (ctx_param->exposure != real_exposure) { + update_exposure = true; + } + /** + * Analog Gain + * [6:0] Analog Gain: Range 16 - 64 + * [15] Force 0.75X: 0 = Disabled. + */ + uint16_t analog_gain = gain * 16; + /* limit: */ + if (analog_gain < 16) { + analog_gain = 16; + } else if (analog_gain > 64) { + analog_gain = 64; + } + float real_analog_gain = analog_gain * 0.0625f; + if (ctx_param->analog_gain != real_analog_gain) { + update_gain = true; + } + + switch (context_idx) { + case 0: + if (update_exposure) { + mt9v034_WriteReg(MTV_COARSE_SW_TOTAL_REG_A, coarse_sw_total); + } + if (update_gain) { + mt9v034_WriteReg(MTV_ANALOG_GAIN_CTRL_REG_A, analog_gain); + } + break; + case 1: + if (update_exposure) { + mt9v034_WriteReg(MTV_COARSE_SW_TOTAL_REG_B, coarse_sw_total); + } + if (update_gain) { + mt9v034_WriteReg(MTV_ANALOG_GAIN_CTRL_REG_B, analog_gain); + } + break; + } + + /* update the settings: */ + ctx_param->exposure = real_exposure; + ctx_param->analog_gain = real_analog_gain; +} + void mt9v034_restore_previous_param(void *usr) { mt9v034_sensor_ctx *ctx = (mt9v034_sensor_ctx *)usr; /* deassert pending context switch command: */ @@ -165,60 +296,95 @@ void mt9v034_restore_previous_param(void *usr) { void mt9v034_notify_readout_start(void *usr) { mt9v034_sensor_ctx *ctx = (mt9v034_sensor_ctx *)usr; - camera_img_param *cur_ctx_param = &ctx->context_p[ctx->cur_context]; + camera_img_param_ex *cur_ctx_param = &ctx->context_p[ctx->cur_context]; /* update the sensor parameter simulation: */ /* transfer exposure settings from exp_param into cur_param */ - // There are no exposure settings yet. + ctx->cur_param.exposure = ctx->exp_param.exposure; /* transfer exposure settings of currently active context into exp_param */ - // There are no exposure settings yet. + ctx->exp_param.exposure = cur_ctx_param->exposure; /* transfer other settings of currently active context into cur_param */ - if (cur_ctx_param->size.x != ctx->cur_param.size.x || - cur_ctx_param->size.y != ctx->cur_param.size.y) { - // first frame after changing resolution is invalid. + if (cur_ctx_param->p.size.x != ctx->cur_param.p.size.x || + cur_ctx_param->p.size.y != ctx->cur_param.p.size.y || + cur_ctx_param->p.binning!= ctx->cur_param.p.binning) { + // first frame after changing resolution or binning is invalid. ctx->cur_param_data_valid = false; } else { ctx->cur_param_data_valid = true; } - ctx->cur_param.size = cur_ctx_param->size; - ctx->cur_param.binning = cur_ctx_param->binning; + ctx->cur_param.p.size = cur_ctx_param->p.size; + ctx->cur_param.p.binning = cur_ctx_param->p.binning; + ctx->cur_param.analog_gain = cur_ctx_param->analog_gain; /* handle context switching: */ - if (ctx->do_switch_context) { + if (ctx->do_switch_context && !ctx->seq_i2c_in_use) { /* update chip control register bit 15: */ int desired_ctx_idx = ctx->desired_context; - camera_img_param *ctx_param = &ctx->context_p[desired_ctx_idx]; + camera_img_param_ex *ctx_param = &ctx->context_p[desired_ctx_idx]; switch(desired_ctx_idx) { case 0: ctx->chip_control_reg &= 0x7FFF; break; case 1: ctx->chip_control_reg |= 0x8000; break; } mt9v034_WriteReg(MTV_CHIP_CONTROL_REG, ctx->chip_control_reg); /* update pixel count for AGC and AEC: */ - mt9v034_WriteReg(MTV_AGC_AEC_PIXEL_COUNT_REG, (uint32_t)ctx_param->size.x * (uint32_t)ctx_param->size.y); + mt9v034_WriteReg(MTV_AGC_AEC_PIXEL_COUNT_REG, (uint32_t)ctx_param->p.size.x * (uint32_t)ctx_param->p.size.y); /* done. */ ctx->do_switch_context = false; ctx->cur_context = desired_ctx_idx; } } -void mt9v034_get_current_param(void *usr, camera_img_param *img_param, bool *img_data_valid) { +void mt9v034_notify_readout_end(void *usr) { + mt9v034_sensor_ctx *ctx = (mt9v034_sensor_ctx *)usr; + /* adjust pix clock inversion as a work-around: */ + if (!ctx->seq_i2c_in_use) { + bool inv_clk = false; + const static bool invert_conf[3] = CONFIG_CLOCK_INVERSION_WORKAROUND; + switch (ctx->cur_param.p.binning) { + case 1: inv_clk = invert_conf[0]; break; + case 2: inv_clk = invert_conf[1]; break; + case 4: inv_clk = invert_conf[2]; break; + } + uint16_t reg = ctx->pixel_frame_line_ctrl_reg & ~0x0010; + if (inv_clk) reg |= 0x0010; + if (reg != ctx->pixel_frame_line_ctrl_reg) { + ctx->pixel_frame_line_ctrl_reg = reg; + mt9v034_WriteReg(MTV_PIXEL_FRAME_LINE_CTRL_REG, ctx->pixel_frame_line_ctrl_reg); + } + } +} + +void mt9v034_get_current_param(void *usr, camera_img_param_ex *img_param, bool *img_data_valid) { mt9v034_sensor_ctx *ctx = (mt9v034_sensor_ctx *)usr; *img_param = ctx->cur_param; *img_data_valid = ctx->cur_param_data_valid; } -static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx) { - /* - * Chip control register. - * [2:0] Scan Mode: 0 = Progressive scan. - * [4:3] Sensor operating mode: 1 = Master mode - * [5] Stereoscopy Mode: 0 = Disabled - * [6] Stereoscopic Master/Slave: 0 = Master - * [7] Parallel output: 1 = Enabled - * [8] Simultaneous mode: 1 = Enabled. Pixel and column readout take place in conjunction with exposure. - * [15] Context A / B select 0 = Context A registers are used. - */ - ctx->chip_control_reg = 0x0188; - mt9v034_WriteReg(MTV_CHIP_CONTROL_REG, ctx->chip_control_reg); - +static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx, bool initial) { + if (initial) { + /* + * Chip control register. + * [2:0] Scan Mode: 0 = Progressive scan. + * [4:3] Sensor operating mode: 1 = Master mode + * [5] Stereoscopy Mode: 0 = Disabled + * [6] Stereoscopic Master/Slave: 0 = Master + * [7] Parallel output: 1 = Enabled + * [8] Simultaneous mode: 1 = Enabled. Pixel and column readout take place in conjunction with exposure. + * [15] Context A / B select 0 = Context A registers are used. + */ + ctx->chip_control_reg = 0x0188; + mt9v034_WriteReg(MTV_CHIP_CONTROL_REG, ctx->chip_control_reg); + + /* + * Pixel Clock, FRAME and LINE VALID Control + * [0] Invert LINE_VALID: 0 = Do not invert. + * [1] Invert FRAME_VALID: 0 = Do not invert. + * [2] XOR LINE_VALID: 0 = Normal operation. + * [3] Continuous LINE_VALID: 0 = Normal operation. + * [4] Invert Pixel Clock: 0 = Do not invert. + */ + ctx->pixel_frame_line_ctrl_reg = 0x0000; + mt9v034_WriteReg(MTV_PIXEL_FRAME_LINE_CTRL_REG, ctx->pixel_frame_line_ctrl_reg); + } + /* settings that are the same for both contexts: */ /* @@ -243,6 +409,25 @@ static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx) { } mt9v034_WriteReg(MTV_ROW_NOISE_CORR_CTRL_REG, row_noise_correction); + /* + * Shutter Width Control + * When Exposure Knee Point Auto Adjust is enabled, + * then one-half to the power of this value indicates the + * ratio of duration time t2, when saturation control + * gate is adjusted to level V2, to total coarse + * integration. + * [3:0] T2 Ratio: 4 -> Tint_tot * (1/2) ^ n + * [7:4] T3 Ratio: 6 -> Tint_tot * (1/2) ^ n + * [8] Exposure Knee Auto Adj: 1 = Enabled. + * [9] Single Knee Enable: 0 = Disabled. + * + * * Tint_tot = Coarse Shutter Width Total + * + * NOTE: update vertical blanking if changing parameters here! + */ + uint16_t coarse_sw_ctrl = 0x0164; + mt9v034_WriteReg(MTV_COARSE_SW_CTRL_REG_A, coarse_sw_ctrl); + mt9v034_WriteReg(MTV_COARSE_SW_CTRL_REG_B, coarse_sw_ctrl); /* * Minimum Coarse Shutter Width * Set to minimum. (1) @@ -293,12 +478,12 @@ static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx) { mt9v034_WriteReg(MTV_AGC_LPF_REG, 2); /* * AGC/AEC Enable - * [0] AEC Enable Ctx A: 1 = Enable - * [1] AGC Enable Ctx A: 1 = Enable - * [8] AEC Enable Ctx B: 1 = Enable - * [9] AGC Enable Ctx B: 1 = Enable + * [0] AEC Enable Ctx A: 0 = Enable + * [1] AGC Enable Ctx A: 0 = Enable + * [8] AEC Enable Ctx B: 0 = Enable + * [9] AGC Enable Ctx B: 0 = Enable */ - mt9v034_WriteReg(MTV_AEC_AGC_ENABLE_REG, 0x0303); + mt9v034_WriteReg(MTV_AEC_AGC_ENABLE_REG, 0x0000); /* * Sensor Type Control * [0] HDR Enable Ctx A: 0 = Disable @@ -323,14 +508,16 @@ static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx) { } mt9v034_WriteReg(MTV_TEST_PATTERN_REG, test_data);//enable test pattern - /* Reset */ - mt9v034_WriteReg(MTV_SOFT_RESET_REG, 0x01); + if (initial) { + /* Reset */ + mt9v034_WriteReg(MTV_SOFT_RESET_REG, 0x01); + } /* * NOTES: * Old code unexpectedly used: - * - 12 to 10bit companding mode on 64x64 pixel image. - * - Enabled AGC & AEC only on 64x64pixel image. + * - 12 to 10bit companding mode on 64x64 pixel image. => TESTED: produces lots of noise, I don't see the gain. + * - Enabled AGC & AEC only on 64x64pixel image. => AGC and AEC is not good when resolution is changed. Implemented in software. * - Enabled HDR mode on big image. * - Used 64 x 64 = 4096 Pixels for AGC & AEC * - row / col bin 4 on 64x64 @@ -338,61 +525,27 @@ static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx) { */ } -static bool mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, const camera_img_param *img_param, bool full_refresh) { +static bool mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, const camera_img_param_ex *img_param, bool full_refresh) { bool update_size = full_refresh; bool update_binning = full_refresh; + bool update_exposure= full_refresh; + bool update_gain = full_refresh; if (context_idx < 0 || context_idx >= 2) return false; - camera_img_param *ctx_param = &ctx->context_p[context_idx]; + camera_img_param_ex *ctx_param = &ctx->context_p[context_idx]; /* check which sets of parameters we need to update: */ if (!full_refresh) { - if (ctx_param->size.x != img_param->size.x || - ctx_param->size.y != img_param->size.y) { + if (ctx_param->p.size.x != img_param->p.size.x || + ctx_param->p.size.y != img_param->p.size.y) { update_size = true; } - if (ctx_param->binning != img_param->binning) { + if (ctx_param->p.binning != img_param->p.binning) { update_binning = true; } } - /* - * Coarse Shutter Width 1 - * The row number in which the first knee occurs. - * Default: 0x01BB - */ - uint16_t coarse_sw1 = 0x01BB; - /* - * Coarse Shutter Width 2 - * The row number in which the second knee occurs. - * Default: 0x01D9 - */ - uint16_t coarse_sw2 = 0x01D9; - /* - * Shutter Width Control - * When Exposure Knee Point Auto Adjust is enabled, - * then one-half to the power of this value indicates the - * ratio of duration time t2, when saturation control - * gate is adjusted to level V2, to total coarse - * integration. - * [3:0] T2 Ratio: 4 -> Tint_tot * (1/2) ^ n - * [7:4] T3 Ratio: 6 -> Tint_tot * (1/2) ^ n - * [8] Exposure Knee Auto Adj: 1 = Enabled. - * [9] Single Knee Enable: 0 = Disabled. - * - * * Tint_tot = Coarse Shutter Width Total - * - * NOTE: update vertical blanking if changing parameters here! - */ - uint16_t coarse_sw_ctrl = 0x0164; - /* - * Coarse Shutter Width Total - * Total integration time in number of rows. This value is used only when AEC is disabled. - * Default: 0x01E0 - */ - uint16_t coarse_sw_total = 0x01E0; - /* image dimensions */ - uint16_t width = img_param->size.x * img_param->binning; - uint16_t height = img_param->size.y * img_param->binning; + uint16_t width = img_param->p.size.x * img_param->p.binning; + uint16_t height = img_param->p.size.y * img_param->p.binning; if (width < 1 || width > 752) return false; if (height < 1 || height > 480) return false; @@ -413,31 +566,49 @@ static bool mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, * In old code for bin 4 and 64 pixels it was 350 + 91. */ uint16_t hor_blanking = 61; - switch (img_param->binning) { - case 1: - /* init to minimum: */ - hor_blanking = 61; - /* increase if window is smaller: */ - if (width < 627) { - hor_blanking += 627 - width; - } - break; - case 2: - /* init to minimum: */ - hor_blanking = 71; - /* increase if window is smaller: */ - if (width < 627) { - hor_blanking += 627 - width; - } - break; - case 4: - /* init to minimum: */ - hor_blanking = 91; - /* increase if window is smaller: */ - if (width < 627) { - hor_blanking += 627 - width; - } - break; + /* init to minimum: */ + switch (img_param->p.binning) { + case 1: hor_blanking = 61; break; + case 2: hor_blanking = 71; break; + case 4: hor_blanking = 91; break; + } + /* increase if window is smaller: */ + if (width < 690 - hor_blanking) { + hor_blanking = 690 - width; + } + + uint16_t row_time = hor_blanking + width; + + /* + * Coarse Shutter Width Total + * Total integration time in number of rows. This value is used only when AEC is disabled. + * Default: 0x01E0 + */ + uint16_t coarse_sw_total = img_param->exposure / row_time; + //uint16_t fine_sw_total = img_param->exposure % row_time; + /* ensure minimum: */ + if (coarse_sw_total < 1) { + coarse_sw_total = 1; + } + uint32_t real_exposure = coarse_sw_total * row_time; + if (ctx_param->exposure != real_exposure) { + update_exposure = true; + } + /** + * Analog Gain + * [6:0] Analog Gain: Range 16 - 64 + * [15] Force 0.75X: 0 = Disabled. + */ + uint16_t analog_gain = img_param->analog_gain * 16; + /* limit: */ + if (analog_gain < 16) { + analog_gain = 16; + } else if (analog_gain > 64) { + analog_gain = 64; + } + float real_analog_gain = analog_gain * 0.0625f; + if (ctx_param->analog_gain != real_analog_gain) { + update_gain = true; } /* @@ -453,7 +624,7 @@ static bool mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, * t2 = Tint_tot * (1/2) ^ T2 Ratio -> 4 * t3 = Tint_tot * (1/2) ^ T3 Ratio -> 1 */ - uint16_t ver_blanking = 4 + 1 + 7; + uint16_t ver_blanking = (height >> 4) + (height >> 6) + 7; /* * Read Mode @@ -466,7 +637,7 @@ static bool mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, * [9:8] Reserved: 3 = Must be 3. */ uint16_t readmode = 0x0300; - switch (img_param->binning) { + switch (img_param->p.binning) { case 1: readmode |= 0x0000; break; case 2: readmode |= 0x0005; break; case 4: readmode |= 0x000A; break; @@ -479,39 +650,47 @@ static bool mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, mt9v034_WriteReg(MTV_WINDOW_WIDTH_REG_A, width); mt9v034_WriteReg(MTV_WINDOW_HEIGHT_REG_A, height); mt9v034_WriteReg(MTV_HOR_BLANKING_REG_A, hor_blanking); - mt9v034_WriteReg(MTV_VER_BLANKING_REG_A, ver_blanking); mt9v034_WriteReg(MTV_READ_MODE_REG_A, readmode); mt9v034_WriteReg(MTV_COLUMN_START_REG_A, col_start); mt9v034_WriteReg(MTV_ROW_START_REG_A, row_start); } - if (full_refresh) { - mt9v034_WriteReg(MTV_COARSE_SW_1_REG_A, coarse_sw1); - mt9v034_WriteReg(MTV_COARSE_SW_2_REG_A, coarse_sw2); - mt9v034_WriteReg(MTV_COARSE_SW_CTRL_REG_A, coarse_sw_ctrl); + if (ver_blanking != ctx->ver_blnk_ctx_a_reg || full_refresh) { + ctx->ver_blnk_ctx_a_reg = ver_blanking; + mt9v034_WriteReg(MTV_VER_BLANKING_REG_A, ctx->ver_blnk_ctx_a_reg); + } + if (update_exposure) { mt9v034_WriteReg(MTV_COARSE_SW_TOTAL_REG_A, coarse_sw_total); } + if (update_gain) { + mt9v034_WriteReg(MTV_ANALOG_GAIN_CTRL_REG_A, analog_gain); + } break; case 1: if (update_size || update_binning) { mt9v034_WriteReg(MTV_WINDOW_WIDTH_REG_B, width); mt9v034_WriteReg(MTV_WINDOW_HEIGHT_REG_B, height); mt9v034_WriteReg(MTV_HOR_BLANKING_REG_B, hor_blanking); - mt9v034_WriteReg(MTV_VER_BLANKING_REG_B, ver_blanking); mt9v034_WriteReg(MTV_READ_MODE_REG_B, readmode); mt9v034_WriteReg(MTV_COLUMN_START_REG_B, col_start); mt9v034_WriteReg(MTV_ROW_START_REG_B, row_start); } - if (full_refresh) { - mt9v034_WriteReg(MTV_COARSE_SW_1_REG_B, coarse_sw1); - mt9v034_WriteReg(MTV_COARSE_SW_2_REG_B, coarse_sw2); - mt9v034_WriteReg(MTV_COARSE_SW_CTRL_REG_B, coarse_sw_ctrl); + if (ver_blanking != ctx->ver_blnk_ctx_b_reg || full_refresh) { + ctx->ver_blnk_ctx_b_reg = ver_blanking; + mt9v034_WriteReg(MTV_VER_BLANKING_REG_B, ctx->ver_blnk_ctx_b_reg); + } + if (update_exposure) { mt9v034_WriteReg(MTV_COARSE_SW_TOTAL_REG_B, coarse_sw_total); } + if (update_gain) { + mt9v034_WriteReg(MTV_ANALOG_GAIN_CTRL_REG_B, analog_gain); + } break; } /* update the current settings: */ *ctx_param = *img_param; + ctx_param->exposure = real_exposure; + ctx_param->analog_gain = real_analog_gain; return true; } diff --git a/src/settings.c b/src/settings.c index 49547c3..244d5d5 100644 --- a/src/settings.c +++ b/src/settings.c @@ -211,7 +211,7 @@ void global_data_reset_param_defaults(void){ global_data.param_access[PARAM_USB_SEND_QUAL_0] = READ_WRITE; - global_data.param[PARAM_VIDEO_ONLY] = 1; + global_data.param[PARAM_VIDEO_ONLY] = 0; strcpy(global_data.param_name[PARAM_VIDEO_ONLY], "VIDEO_ONLY"); global_data.param_access[PARAM_VIDEO_ONLY] = READ_WRITE; From c8f24b4c5378b0126bd8b081804ad99cb0a1d9f5 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Thu, 6 Aug 2015 17:01:32 +0200 Subject: [PATCH 059/120] implemented high resolution shutter and improved CPU usage of auto exposure algorithm. --- inc/camera.h | 6 +++- inc/mt9v034.h | 2 ++ src/camera.c | 34 ++++++++++++------ src/communication.c | 9 ++--- src/main.c | 1 - src/mt9v034.c | 84 ++++++++++++++++++++++++++++++++++----------- src/settings.c | 2 +- 7 files changed, 98 insertions(+), 40 deletions(-) diff --git a/inc/camera.h b/inc/camera.h index 5310fd7..895cada 100644 --- a/inc/camera.h +++ b/inc/camera.h @@ -46,7 +46,8 @@ #define CONFIG_CAMERA_EXTREME_OVEREXPOSE_RATIO (40u) #define CONFIG_CAMERA_OUTLIER_RATIO (5u) #define CONFIG_CAMERA_DESIRED_EXPOSURE_8B (200u) -#define CONFIG_CAMERA_EXPOSURE_SMOOTHING_K (0.95f) +#define CONFIG_CAMERA_EXPOSURE_SMOOTHING_K (0.85f) +#define CONFIG_CAMERA_EXPOSURE_UPDATE_INTERVAL (4u) struct _camera_sensor_interface; typedef struct _camera_sensor_interface camera_sensor_interface; @@ -215,6 +216,7 @@ struct _camera_sensor_interface { void (*reconfigure_general)(void *usr); /** * Immediately switches back to the previous parameters. + * This function may only be called when it is guaranteed that notify_readout_start and notify_readout_end cannot start to run. * @param usr User pointer from this struct. */ void (*restore_previous_param)(void *usr); @@ -308,6 +310,8 @@ struct _camera_ctx { float exposure_smoothing; + int exposure_skip_frame_cnt; + uint16_t exposure_sampling_stride; uint16_t exposure_bins[CONFIG_CAMERA_EXPOSURE_BIN_COUNT]; uint16_t exposure_hist_count; diff --git a/inc/mt9v034.h b/inc/mt9v034.h index c1eb3d4..a471abb 100644 --- a/inc/mt9v034.h +++ b/inc/mt9v034.h @@ -70,6 +70,8 @@ uint32_t mt9v034_get_clks_per_row(uint16_t width, int binning); */ #define CONFIG_CLOCK_INVERSION_WORKAROUND {false, true, true} +#define CONFIG_MTV_VERTICAL_BLANKING_INTROWS (64) + /* Constants */ #define TIMEOUT_MAX 10000 diff --git a/src/camera.c b/src/camera.c index 5f005e1..cf506bb 100644 --- a/src/camera.c +++ b/src/camera.c @@ -70,6 +70,7 @@ bool camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const c ctx->exposure = ctx->exposure_min_clks; ctx->exposure_smoothing = ctx->exposure_min_clks; ctx->exposure_sampling_stride = 0; + ctx->exposure_skip_frame_cnt = 0; memset(ctx->exposure_bins, 0, sizeof(ctx->exposure_bins)); ctx->exposure_hist_count = 0; ctx->img_stream_param.p = *img_param; @@ -331,17 +332,23 @@ void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t siz // write data to it: (at position 0) uint32_t_memcpy((uint32_t *)ctx->target_buffer->buffer, buf, size_w); } - // initialize exposure measuring: - if (img_data_valid) { - /* make sure no more than 65536 pixels are sampled: */ - uint16_t skip = ctx->cur_frame_size / 65536u + 1; - if (skip < 4) { - skip = 4; + // initialize exposure measuring (do not process snapshot images) + if (img_data_valid && !ctx->seq_snapshot_active) { + ctx->exposure_skip_frame_cnt++; + if (ctx->exposure_skip_frame_cnt >= CONFIG_CAMERA_EXPOSURE_UPDATE_INTERVAL) { + ctx->exposure_skip_frame_cnt = 0; + /* make sure no more than 65535 pixels are sampled: */ + uint16_t skip = ctx->cur_frame_size / 65536u + 1; + if (skip < 6) { + skip = 6; + } + ctx->exposure_sampling_stride = skip; + // reset histogram: + ctx->exposure_hist_count = 0; + memset(ctx->exposure_bins, 0, sizeof(ctx->exposure_bins)); + } else { + ctx->exposure_sampling_stride = 0; } - ctx->exposure_sampling_stride = skip; - // reset histogram: - ctx->exposure_hist_count = 0; - memset(ctx->exposure_bins, 0, sizeof(ctx->exposure_bins)); } else { ctx->exposure_sampling_stride = 0; } @@ -427,7 +434,12 @@ bool camera_snapshot_schedule(camera_ctx *ctx, const camera_img_param *img_param return false; } if (!ctx->seq_snapshot_active) { - return camera_update_sensor_param(ctx, img_param, &ctx->snapshot_param); + if (camera_update_sensor_param(ctx, img_param, &ctx->snapshot_param)) { + ctx->snapshot_buffer = dst; + ctx->snapshot_cb = cb; + ctx->seq_snapshot_active = true; + return true; + } } return false; } diff --git a/src/communication.c b/src/communication.c index a322ce0..35993fd 100644 --- a/src/communication.c +++ b/src/communication.c @@ -50,6 +50,8 @@ extern void systemreset(bool to_bootloader); +extern void notify_changed_camera_parameters(); + mavlink_system_t mavlink_system; static uint32_t m_parameter_i = 0; @@ -233,13 +235,11 @@ void handle_mavlink_message(mavlink_channel_t chan, && global_data.param_access[i]) { global_data.param[i] = set.param_value; - - /* handle low light mode and noise correction */ if(i == PARAM_IMAGE_LOW_LIGHT || i == PARAM_IMAGE_ROW_NOISE_CORR || i == PARAM_IMAGE_TEST_PATTERN) { + notify_changed_camera_parameters(); } - /* handle calibration on/off */ else if(i == PARAM_VIDEO_ONLY) { @@ -248,13 +248,10 @@ void handle_mavlink_message(mavlink_channel_t chan, else debug_string_message_buffer("Calibration Mode Off"); } - - /* handle sensor position */ else if(i == PARAM_GYRO_SENSITIVITY_DPS) { l3gd20_config(); } - else { debug_int_message_buffer("Parameter received, param id =", i); diff --git a/src/main.c b/src/main.c index 0b5ce56..dbe5927 100644 --- a/src/main.c +++ b/src/main.c @@ -171,7 +171,6 @@ static volatile bool snap_capture_done = false; static bool snap_ready = true; void snapshot_captured_fn() { - LEDToggle(LED_COM); snap_capture_done = true; } diff --git a/src/mt9v034.c b/src/mt9v034.c index dcedbc7..97b7618 100644 --- a/src/mt9v034.c +++ b/src/mt9v034.c @@ -235,12 +235,12 @@ void mt9v034_update_exposure_param(void *usr, uint32_t exposure, float gain) { * Default: 0x01E0 */ uint16_t coarse_sw_total = exposure / row_time; - //uint16_t fine_sw_total = img_param->exposure % row_time; + uint16_t fine_sw_total = exposure % row_time; /* ensure minimum: */ - if (coarse_sw_total < 1) { - coarse_sw_total = 1; + if (coarse_sw_total < 1 && fine_sw_total < 260) { + fine_sw_total = 260; } - uint32_t real_exposure = coarse_sw_total * row_time; + uint32_t real_exposure = coarse_sw_total * row_time + fine_sw_total; if (ctx_param->exposure != real_exposure) { update_exposure = true; } @@ -261,22 +261,51 @@ void mt9v034_update_exposure_param(void *usr, uint32_t exposure, float gain) { update_gain = true; } + uint16_t sw_for_blanking = coarse_sw_total; + if (sw_for_blanking < ctx_param->p.size.y) { + sw_for_blanking = ctx_param->p.size.y; + } + /* + * Vertical Blanking + * Number of blank rows in a frame. V-Blank value must meet the following minimums: + * Linear Mode: + * V-Blank (min) = SW_total - R0x08 + 7 + * R0x08 (Coarse Shutter Width 1) + * If manual exposure then SW_total = R0x0B. (Coarse Shutter Width Total) + * If auto-exposure mode then SW_total = R0xAD. (Maximum Coarse Shutter Width) + * + * If Auto-Knee Point enabled, then V-Blank (min) = (t2 + t3 + 7). + * t2 = Tint_tot * (1/2) ^ T2 Ratio -> 4 + * t3 = Tint_tot * (1/2) ^ T3 Ratio -> 1 + */ + uint16_t ver_blanking = (sw_for_blanking >> 4) + (sw_for_blanking >> 6) + 7; + switch (context_idx) { case 0: if (update_exposure) { mt9v034_WriteReg(MTV_COARSE_SW_TOTAL_REG_A, coarse_sw_total); + mt9v034_WriteReg(MTV_FINE_SW_TOTAL_REG_A, fine_sw_total); } if (update_gain) { mt9v034_WriteReg(MTV_ANALOG_GAIN_CTRL_REG_A, analog_gain); } + if (ver_blanking != ctx->ver_blnk_ctx_a_reg) { + ctx->ver_blnk_ctx_a_reg = ver_blanking; + mt9v034_WriteReg(MTV_VER_BLANKING_REG_A, ctx->ver_blnk_ctx_a_reg); + } break; case 1: if (update_exposure) { mt9v034_WriteReg(MTV_COARSE_SW_TOTAL_REG_B, coarse_sw_total); + mt9v034_WriteReg(MTV_FINE_SW_TOTAL_REG_B, fine_sw_total); } if (update_gain) { mt9v034_WriteReg(MTV_ANALOG_GAIN_CTRL_REG_B, analog_gain); } + if (ver_blanking != ctx->ver_blnk_ctx_b_reg) { + ctx->ver_blnk_ctx_b_reg = ver_blanking; + mt9v034_WriteReg(MTV_VER_BLANKING_REG_B, ctx->ver_blnk_ctx_b_reg); + } break; } @@ -287,11 +316,22 @@ void mt9v034_update_exposure_param(void *usr, uint32_t exposure, float gain) { void mt9v034_restore_previous_param(void *usr) { mt9v034_sensor_ctx *ctx = (mt9v034_sensor_ctx *)usr; - /* deassert pending context switch command: */ - ctx->do_switch_context = false; - /* switch back to previous context */ - ctx->desired_context = ctx->previous_context; - ctx->do_switch_context = true; + /* switch back to previous context: */ + if (!ctx->seq_i2c_in_use) { + /* update chip control register bit 15: */ + int desired_ctx_idx = ctx->previous_context; + switch(desired_ctx_idx) { + case 0: ctx->chip_control_reg &= 0x7FFF; break; + case 1: ctx->chip_control_reg |= 0x8000; break; + } + mt9v034_WriteReg(MTV_CHIP_CONTROL_REG, ctx->chip_control_reg); + /* done. */ + ctx->cur_context = desired_ctx_idx; + ctx->do_switch_context = false; + } else { + ctx->desired_context = ctx->previous_context; + ctx->do_switch_context = true; + } } void mt9v034_notify_readout_start(void *usr) { @@ -318,17 +358,14 @@ void mt9v034_notify_readout_start(void *usr) { if (ctx->do_switch_context && !ctx->seq_i2c_in_use) { /* update chip control register bit 15: */ int desired_ctx_idx = ctx->desired_context; - camera_img_param_ex *ctx_param = &ctx->context_p[desired_ctx_idx]; switch(desired_ctx_idx) { case 0: ctx->chip_control_reg &= 0x7FFF; break; case 1: ctx->chip_control_reg |= 0x8000; break; } mt9v034_WriteReg(MTV_CHIP_CONTROL_REG, ctx->chip_control_reg); - /* update pixel count for AGC and AEC: */ - mt9v034_WriteReg(MTV_AGC_AEC_PIXEL_COUNT_REG, (uint32_t)ctx_param->p.size.x * (uint32_t)ctx_param->p.size.y); /* done. */ - ctx->do_switch_context = false; ctx->cur_context = desired_ctx_idx; + ctx->do_switch_context = false; } } @@ -338,7 +375,8 @@ void mt9v034_notify_readout_end(void *usr) { if (!ctx->seq_i2c_in_use) { bool inv_clk = false; const static bool invert_conf[3] = CONFIG_CLOCK_INVERSION_WORKAROUND; - switch (ctx->cur_param.p.binning) { + camera_img_param_ex *cur_ctx_param = &ctx->context_p[ctx->cur_context]; + switch (cur_ctx_param->p.binning) { case 1: inv_clk = invert_conf[0]; break; case 2: inv_clk = invert_conf[1]; break; case 4: inv_clk = invert_conf[2]; break; @@ -585,12 +623,12 @@ static bool mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, * Default: 0x01E0 */ uint16_t coarse_sw_total = img_param->exposure / row_time; - //uint16_t fine_sw_total = img_param->exposure % row_time; + uint16_t fine_sw_total = img_param->exposure % row_time; /* ensure minimum: */ - if (coarse_sw_total < 1) { - coarse_sw_total = 1; + if (coarse_sw_total < 1 && fine_sw_total < 260) { + fine_sw_total = 260; } - uint32_t real_exposure = coarse_sw_total * row_time; + uint32_t real_exposure = coarse_sw_total * row_time + fine_sw_total; if (ctx_param->exposure != real_exposure) { update_exposure = true; } @@ -599,7 +637,7 @@ static bool mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, * [6:0] Analog Gain: Range 16 - 64 * [15] Force 0.75X: 0 = Disabled. */ - uint16_t analog_gain = img_param->analog_gain * 16; + uint16_t analog_gain = img_param->analog_gain * 16 + 0.5; /* limit: */ if (analog_gain < 16) { analog_gain = 16; @@ -611,6 +649,10 @@ static bool mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, update_gain = true; } + uint16_t sw_for_blanking = coarse_sw_total; + if (sw_for_blanking < img_param->p.size.y) { + sw_for_blanking = img_param->p.size.y; + } /* * Vertical Blanking * Number of blank rows in a frame. V-Blank value must meet the following minimums: @@ -624,7 +666,7 @@ static bool mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, * t2 = Tint_tot * (1/2) ^ T2 Ratio -> 4 * t3 = Tint_tot * (1/2) ^ T3 Ratio -> 1 */ - uint16_t ver_blanking = (height >> 4) + (height >> 6) + 7; + uint16_t ver_blanking = (sw_for_blanking >> 4) + (sw_for_blanking >> 6) + 7; /* * Read Mode @@ -660,6 +702,7 @@ static bool mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, } if (update_exposure) { mt9v034_WriteReg(MTV_COARSE_SW_TOTAL_REG_A, coarse_sw_total); + mt9v034_WriteReg(MTV_FINE_SW_TOTAL_REG_A, fine_sw_total); } if (update_gain) { mt9v034_WriteReg(MTV_ANALOG_GAIN_CTRL_REG_A, analog_gain); @@ -680,6 +723,7 @@ static bool mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, } if (update_exposure) { mt9v034_WriteReg(MTV_COARSE_SW_TOTAL_REG_B, coarse_sw_total); + mt9v034_WriteReg(MTV_FINE_SW_TOTAL_REG_B, fine_sw_total); } if (update_gain) { mt9v034_WriteReg(MTV_ANALOG_GAIN_CTRL_REG_B, analog_gain); diff --git a/src/settings.c b/src/settings.c index 244d5d5..49547c3 100644 --- a/src/settings.c +++ b/src/settings.c @@ -211,7 +211,7 @@ void global_data_reset_param_defaults(void){ global_data.param_access[PARAM_USB_SEND_QUAL_0] = READ_WRITE; - global_data.param[PARAM_VIDEO_ONLY] = 0; + global_data.param[PARAM_VIDEO_ONLY] = 1; strcpy(global_data.param_name[PARAM_VIDEO_ONLY], "VIDEO_ONLY"); global_data.param_access[PARAM_VIDEO_ONLY] = READ_WRITE; From f63ee6ad67fc5c8bd4ae483e918814ded3a3e6a0 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Thu, 6 Aug 2015 17:02:20 +0200 Subject: [PATCH 060/120] fixed interrupt priorities: DCMI driver needs highes priority. --- src/dcmi.c | 4 ++-- src/i2c.c | 4 ++-- src/sonar.c | 2 +- src/usart.c | 2 +- src/usb_bsp.c | 2 +- 5 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/dcmi.c b/src/dcmi.c index 38128c9..7d88f7e 100644 --- a/src/dcmi.c +++ b/src/dcmi.c @@ -172,7 +172,7 @@ static void dcmi_dma_it_init() { /* Enable the DCMI global Interrupt */ NVIC_InitStructure.NVIC_IRQChannel = DCMI_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 5; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); @@ -181,7 +181,7 @@ static void dcmi_dma_it_init() { /* Enable the DMA global Interrupt */ NVIC_InitStructure.NVIC_IRQChannel = DMA2_Stream1_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 5; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); diff --git a/src/i2c.c b/src/i2c.c index c488576..bc21f23 100644 --- a/src/i2c.c +++ b/src/i2c.c @@ -106,13 +106,13 @@ void i2c_init() { NVIC_InitTypeDef NVIC_InitStructure, NVIC_InitStructure2; NVIC_InitStructure.NVIC_IRQChannel = I2C1_EV_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); NVIC_InitStructure2.NVIC_IRQChannel = I2C1_ER_IRQn; - NVIC_InitStructure2.NVIC_IRQChannelPreemptionPriority = 0; + NVIC_InitStructure2.NVIC_IRQChannelPreemptionPriority = 1; NVIC_InitStructure2.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure2.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure2); diff --git a/src/sonar.c b/src/sonar.c index e49e868..35500ab 100644 --- a/src/sonar.c +++ b/src/sonar.c @@ -224,7 +224,7 @@ void sonar_config(void) /* Enable the USARTx Interrupt */ NVIC_InitStructure.NVIC_IRQChannel = UART4_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); diff --git a/src/usart.c b/src/usart.c index fe17546..cf85bb0 100644 --- a/src/usart.c +++ b/src/usart.c @@ -295,7 +295,7 @@ void usart_init(void) /* Enable the USARTx Interrupt */ NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); diff --git a/src/usb_bsp.c b/src/usb_bsp.c index 5f19232..07906f4 100644 --- a/src/usb_bsp.c +++ b/src/usb_bsp.c @@ -87,7 +87,7 @@ void USB_OTG_BSP_EnableInterrupt(USB_OTG_CORE_HANDLE *pdev) NVIC_PriorityGroupConfig(NVIC_PriorityGroup_1); NVIC_InitStructure.NVIC_IRQChannel = OTG_FS_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); From 4342c16b1a79bb6b8c1ec5289fba2452cf26dfff Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Thu, 6 Aug 2015 17:23:41 +0200 Subject: [PATCH 061/120] bugfix in exposure algorithm. --- src/camera.c | 8 +------- src/main.c | 6 +++--- src/mt9v034.c | 7 ++++++- 3 files changed, 10 insertions(+), 11 deletions(-) diff --git a/src/camera.c b/src/camera.c index cf506bb..13687d8 100644 --- a/src/camera.c +++ b/src/camera.c @@ -197,12 +197,6 @@ static int camera_exposure_hist_extract_brightness_8b(camera_ctx *ctx) { /* expand to 8bits: */ uint32_t bin_size = 1 << (8u - CONFIG_CAMERA_EXPOSURE_BIN_BITS); value = value * bin_size; - /* interpolate linear: */ - if (ctx->exposure_bins[value] > 0) { - value += bin_size - 1 - (((bin_size - 1) * pix_rem) / ctx->exposure_bins[value]); - } else { - value += bin_size - 1; - } return value; } @@ -382,8 +376,8 @@ static bool camera_update_sensor_param(camera_ctx *ctx, const camera_img_param * p.p = *img_param; bool result = false; do { - ctx->seq_updating_sensor = true; ctx->seq_repeat_updating_sensor = false; + ctx->seq_updating_sensor = true; /* update exposure and analog gain: */ p.exposure = ctx->exposure; p.analog_gain = ctx->analog_gain; diff --git a/src/main.c b/src/main.c index dbe5927..054980f 100644 --- a/src/main.c +++ b/src/main.c @@ -158,14 +158,14 @@ void send_params_fn(void) { communication_parameter_send(); } -void switch_params_fn(void) { +/*void switch_params_fn(void) { switch (img_stream_param.binning) { case 1: img_stream_param.binning = 4; break; case 2: img_stream_param.binning = 1; break; case 4: img_stream_param.binning = 2; break; } camera_img_stream_schedule_param_change(&cam_ctx, &img_stream_param); -} +}*/ static volatile bool snap_capture_done = false; static bool snap_ready = true; @@ -274,7 +274,7 @@ int main(void) timer_register(send_params_fn, PARAMS_MS); timer_register(send_video_fn, global_data.param[PARAM_VIDEO_RATE]); timer_register(take_snapshot_fn, 500); - timer_register(switch_params_fn, 2000); + //timer_register(switch_params_fn, 2000); /* variables */ uint32_t counter = 0; diff --git a/src/mt9v034.c b/src/mt9v034.c index 97b7618..d4c6e7d 100644 --- a/src/mt9v034.c +++ b/src/mt9v034.c @@ -196,7 +196,12 @@ void mt9v034_update_exposure_param(void *usr, uint32_t exposure, float gain) { mt9v034_sensor_ctx *ctx = (mt9v034_sensor_ctx *)usr; bool update_exposure; bool update_gain; - int context_idx = ctx->desired_context; + int context_idx; + if (ctx->do_switch_context) { + context_idx = ctx->desired_context; + } else { + context_idx = ctx->cur_context; + } camera_img_param_ex *ctx_param = &ctx->context_p[context_idx]; /* image dimensions */ From c81e07d0e5a63eb1e097a5e7e0c66396457ce876 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Thu, 6 Aug 2015 17:52:40 +0200 Subject: [PATCH 062/120] fixed comparision with desired brightness: prevent invalid inputs. --- src/camera.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/camera.c b/src/camera.c index 13687d8..57e7368 100644 --- a/src/camera.c +++ b/src/camera.c @@ -241,12 +241,14 @@ void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t siz // extremely over-exposed! halve the integration time: exposure = exposure * 0.33f; } else { - if (brightness < CONFIG_CAMERA_DESIRED_EXPOSURE_8B / 3) { + uint8_t desired_bright = CONFIG_CAMERA_DESIRED_EXPOSURE_8B; + desired_bright &= (0xFF << (8u - CONFIG_CAMERA_EXPOSURE_BIN_BITS)); + if (brightness < desired_bright / 3) { // extremely under-exposed! double the integration time: exposure = exposure * 3.f; } else { // determine optimal exposure for next frame: - exposure = (exposure * CONFIG_CAMERA_DESIRED_EXPOSURE_8B) / brightness; + exposure = (exposure * desired_bright) / brightness; } } /* clip the value within bounds: */ From 05e42cb9cacdbf468c32e99b85042b61e3c27d00 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Fri, 7 Aug 2015 19:10:36 +0200 Subject: [PATCH 063/120] improved documentation and generalized variable names. implemented tolerance band in exposure algorithm. --- inc/camera.h | 151 +++++++++++++++++++++++++++++++++++--------------- src/camera.c | 130 +++++++++++++++++++++++-------------------- src/mt9v034.c | 47 ++++++++++------ 3 files changed, 206 insertions(+), 122 deletions(-) diff --git a/inc/camera.h b/inc/camera.h index 895cada..ae166cc 100644 --- a/inc/camera.h +++ b/inc/camera.h @@ -39,14 +39,51 @@ #include #include +/** + * The maximum number of buffers the camera driver supports. + */ #define CONFIG_CAMERA_MAX_BUFFER_COUNT 5 +/** + * Number of bits to use for the histogram bins. + * This affects the speed of the algorithm as well as the possible granularity of the other configuration options. + */ #define CONFIG_CAMERA_EXPOSURE_BIN_BITS (5u) #define CONFIG_CAMERA_EXPOSURE_BIN_COUNT (1u << CONFIG_CAMERA_EXPOSURE_BIN_BITS) -#define CONFIG_CAMERA_EXTREME_OVEREXPOSE_RATIO (40u) +/** + * The ratio of pixels (in percent) that need to fall in the highest histogram bin to trigger + * the extreme overexpose condition. This is intended to quickly react to highly over-exposed images + * by decreasing the exposure time quickly. + */ +#define CONFIG_CAMERA_EXTREME_OVEREXPOSE_RATIO (20u) +/** + * The ratio of pixels (in percent) that are rejected as outliers when evaluating the histogram starting from + * the brightest pixel. + */ #define CONFIG_CAMERA_OUTLIER_RATIO (5u) +/** + * The value that the algorithm uses as a target pixel intensity when adjusting the exposure time. + * The algorithm compares the extracted value from the histogram (after outlier rejection) to this value + * and adjusts the exposure time accordingly. + */ #define CONFIG_CAMERA_DESIRED_EXPOSURE_8B (200u) -#define CONFIG_CAMERA_EXPOSURE_SMOOTHING_K (0.85f) +/** + * The size of the exposure tolerance band. + * If there is a certain amount of change in the image brightness the tolerance band will decide after + * how much brightness delta the exposure algorithm will start to re-adjust the exposure time. + */ +#define CONFIG_CAMERA_DESIRED_EXPOSURE_TOL_8B (24u) +/** + * The smoothing factor of the exposure time. This is the constant of an exponential filter. + * + * new = old * K + (1 - K) * new + * + * This constant influences the speed with which the algorithm reacts to brightness changes. + */ +#define CONFIG_CAMERA_EXPOSURE_SMOOTHING_K (0.83f) +/** + * The interval between updating the exposure value in number of frames. Snapshot and invalid frames are skipped. + */ #define CONFIG_CAMERA_EXPOSURE_UPDATE_INTERVAL (4u) struct _camera_sensor_interface; @@ -105,8 +142,8 @@ typedef void (*camera_snapshot_done_cb)(); * @param sensor The sensor interface to use. * @param transport The sensor data transport interface to use. * @param exposure_min_clks Minimum exposure in clocks. - * @param exposure_max_clks Maximum exposrue in clocks. - * @param analog_gain_max Maximum analog gain. + * @param exposure_max_clks Maximum exposure in clocks. + * @param analog_gain_max Maximum analog gain. > 1 depending on what the sensor supports. (mt9v034: 1-4) * @param img_param The initial image parameters to use for img_stream operation mode. * @param buffers Array of initialized buffers to use for buffering the images in img_stream mode. * In each camera_image_buffer the buffer and buffer_size members must be correctly set. @@ -120,7 +157,7 @@ typedef void (*camera_snapshot_done_cb)(); * @return True when successful. */ bool camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const camera_transport_interface *transport, - uint16_t exposure_min_clks, uint16_t exposure_max_clks, float analog_gain_max, + uint32_t exposure_min_clks, uint32_t exposure_max_clks, float analog_gain_max, const camera_img_param *img_param, camera_image_buffer buffers[], size_t buffer_count); @@ -191,7 +228,37 @@ bool camera_snapshot_schedule(camera_ctx *ctx, const camera_img_param *img_param */ void camera_snapshot_acknowledge(camera_ctx *ctx); -/** Camera sensor configuration interface. +/** + * Camera sensor configuration interface. + * + * The functions inside this interface are divided into two sets of function classes: + * + * A) init, prepare_update_param, reconfigure_general + * These functions may only be called from the mainloop and have a possible long run-time. They may be interrupted by any of the class B functions. + * The sensor interface has to make sure that there will be no conflict when a class B function starts to run while a class A function is not finished yet. + * + * B) restore_previous_param, notify_readout_start, notify_readout_end, update_exposure_param, get_current_param + * These functions may be called from an interrupt context and may interrupt class A functions. They have to make sure to not access resources in use + * by class A functions. + * + * The camera driver will make sure that only one function of each class is entered at the same time and that class B functions are never interrupted by class A functions. + * + * + * The sequence of functions that are called is the following one: + * + * === First buffer of a new frame has arrived === + * notify_readout_start() + * get_current_param() -> returned parameters are used to calculate the number of buffers we need to receive for this frame. + * restore_previous_param() (optional if switching back to the previous parameters is desired) + * + * === ... receiving the buffers between the first and the last ... === + * + * === Last buffer of the current frame has arrived === + * notify_readout_end() + * update_exposure_param() (optional if the exposure parameters have been updated) + * + * ==> repeat + * */ struct _camera_sensor_interface { void *usr; ///< User pointer that should be passed to the following interface functions. @@ -216,7 +283,6 @@ struct _camera_sensor_interface { void (*reconfigure_general)(void *usr); /** * Immediately switches back to the previous parameters. - * This function may only be called when it is guaranteed that notify_readout_start and notify_readout_end cannot start to run. * @param usr User pointer from this struct. */ void (*restore_previous_param)(void *usr); @@ -235,10 +301,9 @@ struct _camera_sensor_interface { void (*notify_readout_end)(void *usr); /** * Called at the end of the readout after notify_readout_end to update the exposure parameters of the current context. - * This should only be called when it is guaranteed that no other sensor function will be called during the time this function executes. - * It should be treated like prepare_update_param except that notify_readout_start and notify_readout_end may not run at the same time. + * @return Return true if it was possible to perform the update. */ - void (*update_exposure_param)(void *usr, uint32_t exposure, float gain); + bool (*update_exposure_param)(void *usr, uint32_t exposure, float gain); /** * Called to retrieve the image parameters of the current frame that is being output. * This function is called after notify_readout_start to retrieve the @@ -299,34 +364,36 @@ struct _camera_ctx { const camera_sensor_interface *sensor; ///< Sensor interface. const camera_transport_interface *transport; ///< Transport interface. - uint32_t exposure_min_clks; - uint32_t exposure_max_clks; - float analog_gain_max; + uint32_t exposure_min_clks; ///< Minimum exposure in clocks. + uint32_t exposure_max_clks; ///< Maximum exposure in clocks. + float analog_gain_max; ///< Maximum gain. /* exposure control */ - float analog_gain; - uint32_t exposure; + float analog_gain; ///< Current analog gain. + uint32_t exposure; ///< Current exposure in clocks. - float exposure_smoothing; + float exposure_smoothing; ///< Exposure smoothing variable. - int exposure_skip_frame_cnt; + int exposure_skip_frame_cnt; ///< Counter to skip frames between exposure calculations. - uint16_t exposure_sampling_stride; - uint16_t exposure_bins[CONFIG_CAMERA_EXPOSURE_BIN_COUNT]; - uint16_t exposure_hist_count; - int last_brightness; + uint16_t exposure_sampling_stride; ///< Image sampling stride used for calculating exposure. + uint16_t exposure_bins[CONFIG_CAMERA_EXPOSURE_BIN_COUNT];///< Histogram of the sampled pixels. + uint16_t exposure_hist_count; ///< Number of sampled pixels in the histogram. + int last_brightness; ///< The last brightness value that has been extracted from the histogram. /* image streaming buffer and parameters */ - camera_img_param_ex img_stream_param; ///< The parameters of the image streaming mode. - camera_img_param pend_img_stream_param; ///< The pending image streaming mode parameters. - camera_image_buffer buffers[CONFIG_CAMERA_MAX_BUFFER_COUNT]; ///< The image buffers for image stream mode. + camera_img_param_ex img_stream_param; ///< The parameters of the image streaming mode. + camera_img_param img_stream_param_pend; ///< The pending image streaming mode parameters. + camera_image_buffer buffers[CONFIG_CAMERA_MAX_BUFFER_COUNT];///< The image buffers for image stream mode. int buffer_count; ///< Total number of buffers. - volatile uint8_t avail_bufs[CONFIG_CAMERA_MAX_BUFFER_COUNT]; ///< Indexes to the buffers that are available. Ordered in the MRU order. - volatile uint8_t avail_buf_count; ///< Number of buffer indexes in the avail_bufs array. - volatile uint8_t put_back_buf_pos; ///< Position where to put back the reserved buffers. + volatile uint8_t buf_avail[CONFIG_CAMERA_MAX_BUFFER_COUNT];///< Indexes to the buffers that are available. Ordered in the MRU order. + volatile uint8_t buf_avail_count; ///< Number of buffer indexes in the avail_bufs array. + volatile uint8_t buf_put_back_pos; ///< Position where to put back the reserved buffers. + volatile bool new_frame_arrived; ///< Flag which is set by the interrupt handler to notify that a new frame has arrived. + volatile bool buffers_are_reserved; ///< True when buffers have been reserved by the camera_img_stream_get_buffers function and need to be returned. /* image snapshot buffer and parameters */ @@ -334,30 +401,26 @@ struct _camera_ctx { camera_image_buffer *snapshot_buffer; ///< Pointer to buffer which receives the snapshot. NULL when no snapshot is pending. camera_snapshot_done_cb snapshot_cb; ///< Callback pointer which is called when the snapshot is complete. - /* retrieving buffers */ - - volatile bool buffers_are_reserved; ///< True when buffers have been reserved by the camera_img_stream_get_buffers function and need to be returned. - uint32_t last_read_frame_index; ///< The frame index of the last frame that has been read in streaming mode - /* frame acquisition */ - camera_img_param_ex cur_frame_param; - uint32_t cur_frame_index; - bool receiving_frame; - camera_image_buffer *target_buffer; - int target_buffer_index; - uint32_t cur_frame_size; - uint32_t cur_frame_pos; + camera_img_param_ex cur_frame_param; ///< Parameters of the frame we are currently receiving. + uint32_t cur_frame_number; ///< Number of the frame we are currently receiving. + camera_image_buffer *cur_frame_target_buf; ///< Target buffer of the frame we are currently receiving. When NULL we are discarding the frame. + int cur_frame_target_buf_idx; ///< When the target buffer points to one of the image stream buffers this is set to the index of that buffer. -1 otherwise. + uint32_t cur_frame_size; ///< The number of bytes that need to be received until the frame is complete. + uint32_t cur_frame_pos; ///< The current byte position inside the frame that is beeing received. /* sequencing */ - volatile bool seq_updating_sensor; - volatile bool seq_repeat_updating_sensor; + volatile bool seq_frame_receiving; ///< True when we are currently receiving a frame. + volatile bool seq_updating_sensor; ///< True when the mainloop is currently calling sensor functions to update the sensor context. + volatile bool seq_repeat_updating_sensor; /**< This variable is set when the interrupt hanlder could not update the exposure settings because seq_updating_sensor was set. + * After the mainloop has finished updating the sensor context, it should repeat it immediatly with new exposure settings. */ volatile bool seq_snapshot_active; /**< Flag that is asserted as long as a snapshot is not finished. * While asserted the camera_img_stream_schedule_param_change function - * should not update the sensor. It should write the new parameters to the pend_img_stream_param variable. */ - volatile bool seq_pend_reconfigure_general; /**< Flag signalling that a general reconfiguration is pending. */ - volatile bool seq_pend_img_stream_param; /**< Flag that is set when pending img stream parameter updates are in the pend_img_stream_param variable. + * should not update the sensor. It should write the new parameters to the img_stream_param_pend variable. */ + volatile bool seq_pend_reconfigure_general; /**< Flag signalling that a general reconfiguration is pending because seq_snapshot_active was active. */ + volatile bool seq_pend_img_stream_param; /**< Flag that is set when pending img stream parameter updates are in the img_stream_param_pend variable. * These updates will be written to the sensor in the camera_snapshot_acknowledge function. */ }; diff --git a/src/camera.c b/src/camera.c index 57e7368..52cdf86 100644 --- a/src/camera.c +++ b/src/camera.c @@ -41,12 +41,18 @@ #include +#define abs(x) ({ \ + typeof(x) __x = (x); \ + if (__x < 0) __x = -__x;\ + __x; \ +}) + void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t size); void camera_transport_frame_done_fn(void *usr); bool camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const camera_transport_interface *transport, - uint16_t exposure_min_clks, uint16_t exposure_max_clks, float analog_gain_max, + uint32_t exposure_min_clks, uint32_t exposure_max_clks, float analog_gain_max, const camera_img_param *img_param, camera_image_buffer buffers[], size_t buffer_count) { memset(ctx, 0, sizeof(camera_ctx)); @@ -90,19 +96,17 @@ bool camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const c ctx->buffers[i].param = ctx->img_stream_param; memset(ctx->buffers[i].buffer, 0, img_size); // init the avail_bufs array: - ctx->avail_bufs[i] = i; + ctx->buf_avail[i] = i; } ctx->buffer_count = buffer_count; - ctx->avail_buf_count = buffer_count; + ctx->buf_avail_count = buffer_count; ctx->new_frame_arrived = false; ctx->snapshot_buffer = NULL; - ctx->last_read_frame_index = 0; - - ctx->cur_frame_index = buffer_count + 1; - ctx->receiving_frame = false; - ctx->target_buffer = NULL; + ctx->cur_frame_number = buffer_count + 1; + ctx->seq_frame_receiving = false; + ctx->cur_frame_target_buf = NULL; ctx->seq_snapshot_active = false; ctx->seq_pend_img_stream_param = false; @@ -129,43 +133,43 @@ static void uint32_t_memcpy(uint32_t *dst, const uint32_t *src, size_t count) { } static void camera_buffer_fifo_remove_front(camera_ctx *ctx, int *out, size_t count) { - size_t bc = ctx->avail_buf_count; + size_t bc = ctx->buf_avail_count; size_t i; // read out: for (i = 0; i < count; ++i) { - *out++ = ctx->avail_bufs[i]; + *out++ = ctx->buf_avail[i]; } // close gap: for (i = count; i < bc; ++i) { - ctx->avail_bufs[i - count] = ctx->avail_bufs[i]; + ctx->buf_avail[i - count] = ctx->buf_avail[i]; } - ctx->avail_buf_count = bc - count; + ctx->buf_avail_count = bc - count; } static void camera_buffer_fifo_remove_back(camera_ctx *ctx, int *out, size_t count) { - size_t bc = ctx->avail_buf_count; + size_t bc = ctx->buf_avail_count; size_t i; // read out: for (i = bc - count; i < bc; ++i) { - *out++ = ctx->avail_bufs[i]; + *out++ = ctx->buf_avail[i]; } // reduce count: - ctx->avail_buf_count = bc - count; + ctx->buf_avail_count = bc - count; } static void camera_buffer_fifo_push_at(camera_ctx *ctx, size_t pos, const int *in, size_t count) { - size_t bc = ctx->avail_buf_count; + size_t bc = ctx->buf_avail_count; size_t i; // move away: for (i = bc; i > pos; --i) { - ctx->avail_bufs[i - 1 + count] = ctx->avail_bufs[i - 1]; + ctx->buf_avail[i - 1 + count] = ctx->buf_avail[i - 1]; } // fill in: for (i = pos; i < pos + count; ++i) { - ctx->avail_bufs[i] = *in++; + ctx->buf_avail[i] = *in++; } // update count: - ctx->avail_buf_count = bc + count; + ctx->buf_avail_count = bc + count; } static void camera_update_exposure_hist(camera_ctx *ctx, const uint8_t *buffer, size_t size) { @@ -205,11 +209,11 @@ void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t siz if (ctx->startup_discard_frame_count != 0) { return; } - if (ctx->receiving_frame) { + if (ctx->seq_frame_receiving) { camera_update_exposure_hist(ctx, buffer, size); // check if we have a target buffer: - if (ctx->target_buffer != NULL) { - uint32_t_memcpy((uint32_t *)((uint8_t *)ctx->target_buffer->buffer + ctx->cur_frame_pos), + if (ctx->cur_frame_target_buf != NULL) { + uint32_t_memcpy((uint32_t *)((uint8_t *)ctx->cur_frame_target_buf->buffer + ctx->cur_frame_pos), (const uint32_t *)buffer, size / 4); } // update current position: @@ -218,15 +222,15 @@ void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t siz if (ctx->cur_frame_pos >= ctx->cur_frame_size) { // frame done! ctx->sensor->notify_readout_end(ctx->sensor->usr); - if (ctx->target_buffer_index >= 0) { + if (ctx->cur_frame_target_buf_idx >= 0) { // put back into available buffers (in the front) - camera_buffer_fifo_push_at(ctx, 0, &ctx->target_buffer_index, 1); - if (ctx->put_back_buf_pos < ctx->avail_buf_count) { - ctx->put_back_buf_pos += 1; + camera_buffer_fifo_push_at(ctx, 0, &ctx->cur_frame_target_buf_idx, 1); + if (ctx->buf_put_back_pos < ctx->buf_avail_count) { + ctx->buf_put_back_pos += 1; } // notify camera_img_stream_get_buffers function. ctx->new_frame_arrived = true; - } else if (ctx->seq_snapshot_active && ctx->target_buffer != NULL) { + } else if (ctx->seq_snapshot_active && ctx->cur_frame_target_buf != NULL) { // snapshot has been taken! ctx->snapshot_cb(); ctx->seq_snapshot_active = false; @@ -241,14 +245,16 @@ void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t siz // extremely over-exposed! halve the integration time: exposure = exposure * 0.33f; } else { - uint8_t desired_bright = CONFIG_CAMERA_DESIRED_EXPOSURE_8B; + int desired_bright = CONFIG_CAMERA_DESIRED_EXPOSURE_8B; desired_bright &= (0xFF << (8u - CONFIG_CAMERA_EXPOSURE_BIN_BITS)); - if (brightness < desired_bright / 3) { + if (brightness < desired_bright / 4) { // extremely under-exposed! double the integration time: - exposure = exposure * 3.f; + exposure = exposure * 4.f; } else { - // determine optimal exposure for next frame: - exposure = (exposure * desired_bright) / brightness; + if (abs(desired_bright - brightness) > CONFIG_CAMERA_DESIRED_EXPOSURE_TOL_8B) { + // determine optimal exposure for next frame: + exposure = (exposure * desired_bright) / brightness; + } } } /* clip the value within bounds: */ @@ -276,8 +282,8 @@ void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t siz } } // reset state: - ctx->target_buffer = NULL; - ctx->receiving_frame = false; + ctx->cur_frame_target_buf = NULL; + ctx->seq_frame_receiving = false; } } else { // no frame currently as the target frame. it must be the beginning of a new frame then! @@ -291,20 +297,24 @@ void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t siz bool img_data_valid; ctx->sensor->get_current_param(ctx->sensor->usr, &ctx->cur_frame_param, &img_data_valid); // update the receiving variables: - ctx->cur_frame_index += 1; - ctx->receiving_frame = true; - ctx->target_buffer = NULL; + ctx->cur_frame_number += 1; + ctx->seq_frame_receiving = true; + ctx->cur_frame_target_buf = NULL; ctx->cur_frame_size = (uint32_t)ctx->cur_frame_param.p.size.x * (uint32_t)ctx->cur_frame_param.p.size.y; ctx->cur_frame_pos = size; - ctx->target_buffer_index = -1; + ctx->cur_frame_target_buf_idx = -1; if (!ctx->seq_snapshot_active) { // check that the size parameters match to the img stream: if (ctx->cur_frame_param.p.size.x == ctx->img_stream_param.p.size.x && ctx->cur_frame_param.p.size.y == ctx->img_stream_param.p.size.y) { if (img_data_valid) { // get least recently used buffer from the available buffers: - camera_buffer_fifo_remove_back(ctx, &ctx->target_buffer_index, 1); - ctx->target_buffer = &ctx->buffers[ctx->target_buffer_index]; + camera_buffer_fifo_remove_back(ctx, &ctx->cur_frame_target_buf_idx, 1); + ctx->cur_frame_target_buf = &ctx->buffers[ctx->cur_frame_target_buf_idx]; + /* if there are no more buffers in the fifo reset the flag again: */ + if (ctx->buf_avail_count == 0) { + ctx->new_frame_arrived = false; + } } } } else { @@ -314,19 +324,19 @@ void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t siz ctx->cur_frame_param.p.binning == ctx->snapshot_param.p.binning) { if (img_data_valid) { // get the buffer: - ctx->target_buffer = ctx->snapshot_buffer; + ctx->cur_frame_target_buf = ctx->snapshot_buffer; // initiate switching back to img stream mode: ctx->sensor->restore_previous_param(ctx->sensor->usr); } } } // initialize the target buffer: - if (ctx->target_buffer != NULL) { - ctx->target_buffer->timestamp = get_boot_time_us(); - ctx->target_buffer->frame_number = ctx->cur_frame_index; - ctx->target_buffer->param = ctx->cur_frame_param; + if (ctx->cur_frame_target_buf != NULL) { + ctx->cur_frame_target_buf->timestamp = get_boot_time_us(); + ctx->cur_frame_target_buf->frame_number = ctx->cur_frame_number; + ctx->cur_frame_target_buf->param = ctx->cur_frame_param; // write data to it: (at position 0) - uint32_t_memcpy((uint32_t *)ctx->target_buffer->buffer, buf, size_w); + uint32_t_memcpy((uint32_t *)ctx->cur_frame_target_buf->buffer, buf, size_w); } // initialize exposure measuring (do not process snapshot images) if (img_data_valid && !ctx->seq_snapshot_active) { @@ -365,14 +375,6 @@ void camera_transport_frame_done_fn(void *usr) { } } -void camera_reconfigure_general(camera_ctx *ctx) { - if (!ctx->seq_snapshot_active && !ctx->seq_pend_reconfigure_general) { - ctx->sensor->reconfigure_general(ctx->sensor->usr); - } else { - ctx->seq_pend_reconfigure_general = true; - } -} - static bool camera_update_sensor_param(camera_ctx *ctx, const camera_img_param *img_param, camera_img_param_ex *ptoup) { camera_img_param_ex p = *ptoup; p.p = *img_param; @@ -396,6 +398,14 @@ static bool camera_update_sensor_param(camera_ctx *ctx, const camera_img_param * return result; } +void camera_reconfigure_general(camera_ctx *ctx) { + if (!ctx->seq_snapshot_active && !ctx->seq_pend_reconfigure_general) { + ctx->sensor->reconfigure_general(ctx->sensor->usr); + } else { + ctx->seq_pend_reconfigure_general = true; + } +} + bool camera_img_stream_schedule_param_change(camera_ctx *ctx, const camera_img_param *img_param) { uint32_t img_size = (uint32_t)img_param->size.x * (uint32_t)img_param->size.y; if (img_size % ctx->transport->transfer_size != 0 || img_size / ctx->transport->transfer_size < 2) { @@ -412,7 +422,7 @@ bool camera_img_stream_schedule_param_change(camera_ctx *ctx, const camera_img_p if (!ctx->seq_snapshot_active && !ctx->seq_pend_img_stream_param) { return camera_update_sensor_param(ctx, img_param, &ctx->img_stream_param); } else { - ctx->pend_img_stream_param = *img_param; + ctx->img_stream_param_pend = *img_param; ctx->seq_pend_img_stream_param = true; return true; } @@ -449,7 +459,7 @@ void camera_snapshot_acknowledge(camera_ctx *ctx) { if (ctx->seq_pend_img_stream_param) { ctx->seq_pend_img_stream_param = false; /* write the pending changes to the sensor: */ - camera_update_sensor_param(ctx, &ctx->pend_img_stream_param, &ctx->img_stream_param); + camera_update_sensor_param(ctx, &ctx->img_stream_param_pend, &ctx->img_stream_param); } } } @@ -469,7 +479,7 @@ static bool camera_img_stream_get_buffers_idx(camera_ctx *ctx, int bidx[], size_ } if (consecutive) { /* good! */ - ctx->put_back_buf_pos = 0; + ctx->buf_put_back_pos = 0; ctx->buffers_are_reserved = true; } else { /* not good. put back the buffers: */ @@ -520,9 +530,9 @@ void camera_img_stream_return_buffers(camera_ctx *ctx, camera_image_buffer *buff } /* buffer management needs to be performed atomically: */ __disable_irq(); - size_t at_pos = ctx->put_back_buf_pos; - if (at_pos > ctx->avail_buf_count) at_pos = ctx->avail_buf_count; - camera_buffer_fifo_push_at(ctx, ctx->put_back_buf_pos, bidx, count); + size_t at_pos = ctx->buf_put_back_pos; + if (at_pos > ctx->buf_avail_count) at_pos = ctx->buf_avail_count; + camera_buffer_fifo_push_at(ctx, ctx->buf_put_back_pos, bidx, count); ctx->buffers_are_reserved = false; __enable_irq(); } diff --git a/src/mt9v034.c b/src/mt9v034.c index d4c6e7d..f5ce9b5 100644 --- a/src/mt9v034.c +++ b/src/mt9v034.c @@ -43,6 +43,8 @@ #include +#define DEBUG_TEST_WORSTCASE_LATENCY (false) + struct _mt9v034_sensor_ctx; typedef struct _mt9v034_sensor_ctx mt9v034_sensor_ctx; @@ -52,7 +54,7 @@ void mt9v034_reconfigure_general(void *usr); void mt9v034_restore_previous_param(void *usr); void mt9v034_notify_readout_start(void *usr); void mt9v034_notify_readout_end(void *usr); -void mt9v034_update_exposure_param(void *usr, uint32_t exposure, float gain); +bool mt9v034_update_exposure_param(void *usr, uint32_t exposure, float gain); void mt9v034_get_current_param(void *usr, camera_img_param_ex *img_param, bool *img_data_valid); static bool mt9v034_init_hw(mt9v034_sensor_ctx *ctx); @@ -150,6 +152,7 @@ bool mt9v034_init(void *usr, const camera_img_param_ex *img_param) { /* init hardware: */ if (!mt9v034_init_hw(ctx)) return false; /* configure sensor: */ + ctx->seq_i2c_in_use = true; mt9v034_configure_context(ctx, 0, img_param, true); mt9v034_configure_context(ctx, 1, img_param, true); mt9v034_configure_general(ctx, true); @@ -159,6 +162,7 @@ bool mt9v034_init(void *usr, const camera_img_param_ex *img_param) { ctx->cur_param_data_valid = false; /* do a forced context update next. */ ctx->do_switch_context = true; + ctx->seq_i2c_in_use = false; return true; } @@ -192,8 +196,13 @@ bool mt9v034_prepare_update_param(void *usr, const camera_img_param_ex *img_para return true; } -void mt9v034_update_exposure_param(void *usr, uint32_t exposure, float gain) { +bool mt9v034_update_exposure_param(void *usr, uint32_t exposure, float gain) { mt9v034_sensor_ctx *ctx = (mt9v034_sensor_ctx *)usr; + /* check whether i2c is in use: */ + if (ctx->seq_i2c_in_use) { + return false; + } + bool update_exposure; bool update_gain; int context_idx; @@ -245,7 +254,7 @@ void mt9v034_update_exposure_param(void *usr, uint32_t exposure, float gain) { if (coarse_sw_total < 1 && fine_sw_total < 260) { fine_sw_total = 260; } - uint32_t real_exposure = coarse_sw_total * row_time + fine_sw_total; + uint32_t real_exposure = coarse_sw_total * (uint32_t)row_time + fine_sw_total; if (ctx_param->exposure != real_exposure) { update_exposure = true; } @@ -287,27 +296,27 @@ void mt9v034_update_exposure_param(void *usr, uint32_t exposure, float gain) { switch (context_idx) { case 0: - if (update_exposure) { + if (update_exposure || DEBUG_TEST_WORSTCASE_LATENCY) { mt9v034_WriteReg(MTV_COARSE_SW_TOTAL_REG_A, coarse_sw_total); mt9v034_WriteReg(MTV_FINE_SW_TOTAL_REG_A, fine_sw_total); } - if (update_gain) { + if (update_gain || DEBUG_TEST_WORSTCASE_LATENCY) { mt9v034_WriteReg(MTV_ANALOG_GAIN_CTRL_REG_A, analog_gain); } - if (ver_blanking != ctx->ver_blnk_ctx_a_reg) { + if (ver_blanking != ctx->ver_blnk_ctx_a_reg || DEBUG_TEST_WORSTCASE_LATENCY) { ctx->ver_blnk_ctx_a_reg = ver_blanking; mt9v034_WriteReg(MTV_VER_BLANKING_REG_A, ctx->ver_blnk_ctx_a_reg); } break; case 1: - if (update_exposure) { + if (update_exposure || DEBUG_TEST_WORSTCASE_LATENCY) { mt9v034_WriteReg(MTV_COARSE_SW_TOTAL_REG_B, coarse_sw_total); mt9v034_WriteReg(MTV_FINE_SW_TOTAL_REG_B, fine_sw_total); } - if (update_gain) { + if (update_gain || DEBUG_TEST_WORSTCASE_LATENCY) { mt9v034_WriteReg(MTV_ANALOG_GAIN_CTRL_REG_B, analog_gain); } - if (ver_blanking != ctx->ver_blnk_ctx_b_reg) { + if (ver_blanking != ctx->ver_blnk_ctx_b_reg || DEBUG_TEST_WORSTCASE_LATENCY) { ctx->ver_blnk_ctx_b_reg = ver_blanking; mt9v034_WriteReg(MTV_VER_BLANKING_REG_B, ctx->ver_blnk_ctx_b_reg); } @@ -317,6 +326,8 @@ void mt9v034_update_exposure_param(void *usr, uint32_t exposure, float gain) { /* update the settings: */ ctx_param->exposure = real_exposure; ctx_param->analog_gain = real_analog_gain; + + return true; } void mt9v034_restore_previous_param(void *usr) { @@ -388,7 +399,7 @@ void mt9v034_notify_readout_end(void *usr) { } uint16_t reg = ctx->pixel_frame_line_ctrl_reg & ~0x0010; if (inv_clk) reg |= 0x0010; - if (reg != ctx->pixel_frame_line_ctrl_reg) { + if (reg != ctx->pixel_frame_line_ctrl_reg || DEBUG_TEST_WORSTCASE_LATENCY) { ctx->pixel_frame_line_ctrl_reg = reg; mt9v034_WriteReg(MTV_PIXEL_FRAME_LINE_CTRL_REG, ctx->pixel_frame_line_ctrl_reg); } @@ -693,7 +704,7 @@ static bool mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, switch (context_idx) { case 0: - if (update_size || update_binning) { + if (update_size || update_binning || DEBUG_TEST_WORSTCASE_LATENCY) { mt9v034_WriteReg(MTV_WINDOW_WIDTH_REG_A, width); mt9v034_WriteReg(MTV_WINDOW_HEIGHT_REG_A, height); mt9v034_WriteReg(MTV_HOR_BLANKING_REG_A, hor_blanking); @@ -701,20 +712,20 @@ static bool mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, mt9v034_WriteReg(MTV_COLUMN_START_REG_A, col_start); mt9v034_WriteReg(MTV_ROW_START_REG_A, row_start); } - if (ver_blanking != ctx->ver_blnk_ctx_a_reg || full_refresh) { + if (ver_blanking != ctx->ver_blnk_ctx_a_reg || full_refresh || DEBUG_TEST_WORSTCASE_LATENCY) { ctx->ver_blnk_ctx_a_reg = ver_blanking; mt9v034_WriteReg(MTV_VER_BLANKING_REG_A, ctx->ver_blnk_ctx_a_reg); } - if (update_exposure) { + if (update_exposure || DEBUG_TEST_WORSTCASE_LATENCY) { mt9v034_WriteReg(MTV_COARSE_SW_TOTAL_REG_A, coarse_sw_total); mt9v034_WriteReg(MTV_FINE_SW_TOTAL_REG_A, fine_sw_total); } - if (update_gain) { + if (update_gain || DEBUG_TEST_WORSTCASE_LATENCY) { mt9v034_WriteReg(MTV_ANALOG_GAIN_CTRL_REG_A, analog_gain); } break; case 1: - if (update_size || update_binning) { + if (update_size || update_binning || DEBUG_TEST_WORSTCASE_LATENCY) { mt9v034_WriteReg(MTV_WINDOW_WIDTH_REG_B, width); mt9v034_WriteReg(MTV_WINDOW_HEIGHT_REG_B, height); mt9v034_WriteReg(MTV_HOR_BLANKING_REG_B, hor_blanking); @@ -722,15 +733,15 @@ static bool mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, mt9v034_WriteReg(MTV_COLUMN_START_REG_B, col_start); mt9v034_WriteReg(MTV_ROW_START_REG_B, row_start); } - if (ver_blanking != ctx->ver_blnk_ctx_b_reg || full_refresh) { + if (ver_blanking != ctx->ver_blnk_ctx_b_reg || full_refresh || DEBUG_TEST_WORSTCASE_LATENCY) { ctx->ver_blnk_ctx_b_reg = ver_blanking; mt9v034_WriteReg(MTV_VER_BLANKING_REG_B, ctx->ver_blnk_ctx_b_reg); } - if (update_exposure) { + if (update_exposure || DEBUG_TEST_WORSTCASE_LATENCY) { mt9v034_WriteReg(MTV_COARSE_SW_TOTAL_REG_B, coarse_sw_total); mt9v034_WriteReg(MTV_FINE_SW_TOTAL_REG_B, fine_sw_total); } - if (update_gain) { + if (update_gain || DEBUG_TEST_WORSTCASE_LATENCY) { mt9v034_WriteReg(MTV_ANALOG_GAIN_CTRL_REG_B, analog_gain); } break; From 14962f865da69ff5e312cb0394de6b01f569da3a Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Fri, 7 Aug 2015 21:35:58 +0200 Subject: [PATCH 064/120] implemented de-synchronization detection and recovery. --- inc/camera.h | 16 +++++++++---- src/camera.c | 63 ++++++++++++++++++++++++++++++++++++++++------------ src/dcmi.c | 22 +++++++++++++++++- src/main.c | 12 ++++++---- 4 files changed, 90 insertions(+), 23 deletions(-) diff --git a/inc/camera.h b/inc/camera.h index ae166cc..44f548e 100644 --- a/inc/camera.h +++ b/inc/camera.h @@ -132,9 +132,10 @@ typedef struct _camera_image_buffer { /** * Callback which is called when a snapshot capture has finished. * @note This callback may be called from an interrupt handler. - * @param Pointer to buffer which contains the snapshot. + * @param success True when capture was successful, false if it failed due to desynchronization. + * You should retry in the mainloop after calling camera_snapshot_acknowledge. */ -typedef void (*camera_snapshot_done_cb)(); +typedef void (*camera_snapshot_done_cb)(bool success); /** * Initializes the camera driver. @@ -217,6 +218,8 @@ void camera_img_stream_return_buffers(camera_ctx *ctx, camera_image_buffer *buff * @param cb Callback function which is called when the snapshot has been taken. * Note that the callback is called from the interrupt context and should only do minimal stuff * like setting a flag to notify the main loop. Calling camera_* functions is not allowed. + * camera_snapshot_acknowledge must be called as soon as possible afterwards in the mainloop + * regardless of the success status of the snapshot. * @return True when snapshot has been successfully scheduled. */ bool camera_snapshot_schedule(camera_ctx *ctx, const camera_img_param *img_param, camera_image_buffer *dst, camera_snapshot_done_cb cb); @@ -323,9 +326,12 @@ typedef void (*camera_transport_transfer_done_cb)(void *usr, const void *buffer, /** * Callback for notifying the camera driver about a completed frame. + * This callback should be called AFTER the last camera_transport_transfer_done_cb call of the frame. * @param usr The user data pointer that has been specified in the init function. (cb_usr) + * @param probably_infront_dma A hint to the camera driver that the frame done call might be just in front of the transfer done call. + * It is up to the camera driver to interpret it. */ -typedef void (*camera_transport_frame_done_cb)(void *usr); +typedef void (*camera_transport_frame_done_cb)(void *usr, bool probably_infront_dma); /** Camera image transport interface. */ @@ -357,7 +363,7 @@ struct _camera_transport_interface { */ struct _camera_ctx { /* startup control */ - volatile int startup_discard_frame_count; ///< Number of frames to discard at startup. + volatile int resync_discard_frame_count; ///< Number of frames to discard at startup. /* assets of the camera driver */ @@ -412,6 +418,8 @@ struct _camera_ctx { /* sequencing */ + int frame_done_infront_count; + volatile bool seq_frame_receiving; ///< True when we are currently receiving a frame. volatile bool seq_updating_sensor; ///< True when the mainloop is currently calling sensor functions to update the sensor context. volatile bool seq_repeat_updating_sensor; /**< This variable is set when the interrupt hanlder could not update the exposure settings because seq_updating_sensor was set. diff --git a/src/camera.c b/src/camera.c index 52cdf86..7b7b0cc 100644 --- a/src/camera.c +++ b/src/camera.c @@ -49,7 +49,7 @@ void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t size); -void camera_transport_frame_done_fn(void *usr); +void camera_transport_frame_done_fn(void *usr, bool probably_infront_dma); bool camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const camera_transport_interface *transport, uint32_t exposure_min_clks, uint32_t exposure_max_clks, float analog_gain_max, @@ -68,7 +68,7 @@ bool camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const c return false; } // initialize state: - ctx->startup_discard_frame_count = -1; + ctx->resync_discard_frame_count = -1; ctx->exposure_min_clks = exposure_min_clks; ctx->exposure_max_clks = exposure_max_clks; ctx->analog_gain_max = analog_gain_max; @@ -104,6 +104,8 @@ bool camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const c ctx->snapshot_buffer = NULL; + ctx->frame_done_infront_count = 0; + ctx->cur_frame_number = buffer_count + 1; ctx->seq_frame_receiving = false; ctx->cur_frame_target_buf = NULL; @@ -121,7 +123,7 @@ bool camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const c return false; } /* after initialization start the discard count down! */ - ctx->startup_discard_frame_count = 16; + ctx->resync_discard_frame_count = 16; return true; } @@ -206,7 +208,7 @@ static int camera_exposure_hist_extract_brightness_8b(camera_ctx *ctx) { void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t size) { camera_ctx *ctx = (camera_ctx *)usr; - if (ctx->startup_discard_frame_count != 0) { + if (ctx->resync_discard_frame_count != 0) { return; } if (ctx->seq_frame_receiving) { @@ -232,7 +234,7 @@ void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t siz ctx->new_frame_arrived = true; } else if (ctx->seq_snapshot_active && ctx->cur_frame_target_buf != NULL) { // snapshot has been taken! - ctx->snapshot_cb(); + ctx->snapshot_cb(true); ctx->seq_snapshot_active = false; } /* handle auto-exposure: */ @@ -287,10 +289,6 @@ void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t siz } } else { // no frame currently as the target frame. it must be the beginning of a new frame then! - // empty the DMA buffer as quickly as possible: - size_t size_w = size / 4; - uint32_t buf[size_w]; - uint32_t_memcpy(buf, (const uint32_t *)buffer, size_w); // update the sensor: (this might trigger some I2C transfers) ctx->sensor->notify_readout_start(ctx->sensor->usr); // get current sensor parameter: @@ -336,7 +334,7 @@ void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t siz ctx->cur_frame_target_buf->frame_number = ctx->cur_frame_number; ctx->cur_frame_target_buf->param = ctx->cur_frame_param; // write data to it: (at position 0) - uint32_t_memcpy((uint32_t *)ctx->cur_frame_target_buf->buffer, buf, size_w); + uint32_t_memcpy((uint32_t *)ctx->cur_frame_target_buf->buffer, (uint32_t *)buffer, size / 4); } // initialize exposure measuring (do not process snapshot images) if (img_data_valid && !ctx->seq_snapshot_active) { @@ -358,20 +356,57 @@ void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t siz } else { ctx->exposure_sampling_stride = 0; } - camera_update_exposure_hist(ctx, (const uint8_t *)buf, size); + camera_update_exposure_hist(ctx, (const uint8_t *)buffer, size); } } -void camera_transport_frame_done_fn(void *usr) { +void camera_transport_frame_done_fn(void *usr, bool probably_infront_dma) { camera_ctx *ctx = (camera_ctx *)usr; - int fdc = ctx->startup_discard_frame_count; + /* we will only pay attention to the probably_infront_dma flag if we are in normal operating mode. */ + int fdc = ctx->resync_discard_frame_count; if (fdc > 0) { fdc--; - ctx->startup_discard_frame_count = fdc; + ctx->resync_discard_frame_count = fdc; /* re-initialize the transport twice */ if (fdc == 0 || fdc == 1) { + LEDOff(LED_ERR); ctx->transport->reset(ctx->transport->usr); } + } else { + /* we trust the probably infront DMA flag 3 times in a row. */ + if (!probably_infront_dma || ctx->frame_done_infront_count > 3) { + ctx->frame_done_infront_count = 0; + /* if we don't trust it anymore we trigger a re-sync */ + if (ctx->seq_frame_receiving || probably_infront_dma) { + /* we are out of sync! abort current frame: */ + if (ctx->cur_frame_target_buf_idx >= 0) { + // put back into available buffers (in the back) + uint16_t ac_b = ctx->buf_avail_count; + camera_buffer_fifo_push_at(ctx, ac_b, &ctx->cur_frame_target_buf_idx, 1); + // set frame number something much older than the last frame: + if (ac_b > 0) { + int last_b = ctx->buf_avail[ac_b - 1]; + int this_b = ctx->buf_avail[ac_b]; + ctx->buffers[this_b].frame_number = ctx->buffers[last_b].frame_number - ctx->buffer_count - 1; + } else { + ctx->new_frame_arrived = false; + } + } else if (ctx->seq_snapshot_active && ctx->cur_frame_target_buf != NULL) { + // snapshot abort! + ctx->snapshot_cb(false); + ctx->seq_snapshot_active = false; + } + // reset state: + ctx->cur_frame_target_buf = NULL; + ctx->seq_frame_receiving = false; + /* initiate resynchronization: */ + ctx->resync_discard_frame_count = 2; + + LEDOn(LED_ERR); + } + } else { + ctx->frame_done_infront_count++; + } } } diff --git a/src/dcmi.c b/src/dcmi.c index 7d88f7e..62943a7 100644 --- a/src/dcmi.c +++ b/src/dcmi.c @@ -48,6 +48,7 @@ #include "stm32f4xx_tim.h" #include "misc.h" #include "stm32f4xx.h" +//#include "led.h" struct _dcmi_transport_ctx; typedef struct _dcmi_transport_ctx dcmi_transport_ctx; @@ -85,9 +86,14 @@ static void dcmi_dma_enable(); static void dcmi_dma_it_init(); struct _dcmi_transport_ctx { + /* assets */ camera_transport_transfer_done_cb transfer_done_cb; camera_transport_frame_done_cb frame_done_cb; void *cb_usr; + + /* IRQ monitoring: */ + volatile uint32_t last_dma_irq_t; + uint32_t last_dma_irq_pause_t; }; static dcmi_transport_ctx dcmi_ctx; @@ -113,6 +119,8 @@ bool dcmi_init(void *usr, ctx->transfer_done_cb = transfer_done_cb; ctx->frame_done_cb = frame_done_cb; ctx->cb_usr = cb_usr; + ctx->last_dma_irq_t = get_boot_time_us(); + ctx->last_dma_irq_pause_t = 1000000; /* initialize hardware: */ dcmi_clock_init(); dcmi_hw_init(); @@ -141,8 +149,14 @@ void DCMI_IRQHandler(void) { DCMI_ClearITPendingBit(DCMI_IT_FRAME); /* get context: */ dcmi_transport_ctx *ctx = &dcmi_ctx; + /* calculate time delta to the end of DMA2_Stream1_IRQHandler: + * this should be almost zero because normally the DCMI interrupt will happen after the DMA interrupt */ + uint32_t dt = get_time_delta_us(ctx->last_dma_irq_t); + /* take the last pause between DMA request as a basis to calculate the maximum allowed delta time: */ + uint32_t allowed_dt = ctx->last_dma_irq_pause_t / 4; + if (allowed_dt < 10) allowed_dt = 10; /* typedef void (*camera_transport_frame_done_cb)(void *usr); */ - ctx->frame_done_cb(ctx->cb_usr); + ctx->frame_done_cb(ctx->cb_usr, dt > allowed_dt); } } @@ -154,6 +168,7 @@ void DMA2_Stream1_IRQHandler(void) { if (DMA_GetITStatus(DMA2_Stream1, DMA_IT_TCIF1) != RESET) { DMA_ClearITPendingBit(DMA2_Stream1, DMA_IT_TCIF1); /* get the buffer that has been completed: */ + //LEDOn(LED_ACT); void *buffer; if (DMA_GetCurrentMemoryTarget(DMA2_Stream1) == 0) { buffer = dcmi_dma_buffer_2; @@ -162,8 +177,13 @@ void DMA2_Stream1_IRQHandler(void) { } /* get context: */ dcmi_transport_ctx *ctx = &dcmi_ctx; + /* calculate the pause-time: */ + ctx->last_dma_irq_pause_t = get_time_delta_us(ctx->last_dma_irq_t); /* typedef void (*camera_transport_transfer_done_cb)(void *usr, const void *buffer, size_t size); */ ctx->transfer_done_cb(ctx->cb_usr, buffer, CONFIG_DCMI_DMA_BUFFER_SIZE); + /* store the time when the function finished. */ + ctx->last_dma_irq_t = get_boot_time_us(); + //LEDOff(LED_ACT); } } diff --git a/src/main.c b/src/main.c index 054980f..f5ce385 100644 --- a/src/main.c +++ b/src/main.c @@ -168,10 +168,12 @@ void send_params_fn(void) { }*/ static volatile bool snap_capture_done = false; +static volatile bool snap_capture_success = false; static bool snap_ready = true; -void snapshot_captured_fn() { +void snapshot_captured_fn(bool success) { snap_capture_done = true; + snap_capture_success = success; } void take_snapshot_fn(void) { @@ -299,9 +301,11 @@ int main(void) snap_capture_done = false; camera_snapshot_acknowledge(&cam_ctx); snap_ready = true; - /* send the snapshot! */ - LEDToggle(LED_COM); - mavlink_send_image(&snapshot_buffer); + if (snap_capture_success) { + /* send the snapshot! */ + LEDToggle(LED_COM); + mavlink_send_image(&snapshot_buffer); + } } /* calculate focal_length in pixel */ From e992fdb7b5ea2a20aaaa3570c7f394bc6859b318 Mon Sep 17 00:00:00 2001 From: Severin Latkovic Date: Thu, 13 Aug 2015 21:38:26 +0200 Subject: [PATCH 065/120] - Reverted SystemCoreClock to original setting. Overclocking is not needed. - Removed casting from uint32_t to int32_t in timer.c -(While this compiled fine, I could not test it in QGroundcontrol due to some Windows10 problem. But it should be working) --- src/system_stm32f4xx.c | 6 +++--- src/timer.c | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/system_stm32f4xx.c b/src/system_stm32f4xx.c index 224843e..fe62cd6 100644 --- a/src/system_stm32f4xx.c +++ b/src/system_stm32f4xx.c @@ -126,18 +126,18 @@ /************************* PLL Parameters *************************************/ /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N */ #define PLL_M 24 -#define PLL_N 384 +#define PLL_N 336 /* SYSCLK = PLL_VCO / PLL_P */ #define PLL_P 2 /* USB OTG FS, SDIO and RNG Clock = PLL_VCO / PLLQ */ -#define PLL_Q 8 +#define PLL_Q 7 /******************************************************************************/ -uint32_t SystemCoreClock = 192000000; +uint32_t SystemCoreClock = 168000000; __I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9}; diff --git a/src/timer.c b/src/timer.c index 6b6067c..383d680 100644 --- a/src/timer.c +++ b/src/timer.c @@ -198,7 +198,7 @@ uint32_t get_boot_time_us(void) } uint32_t calculate_time_delta_us(uint32_t end, uint32_t start) { - return (int32_t)end - (int32_t)start; + return end - start; } uint32_t get_time_delta_us(uint32_t start) From 0c9c94c79b6e8a57e5862e8cc53cff769ed1193c Mon Sep 17 00:00:00 2001 From: Severin Latkovic Date: Fri, 14 Aug 2015 16:00:19 +0200 Subject: [PATCH 066/120] - Fixed issue where high gyro values (like shaking) lead to a crash. KLT algorithm now checks whether the x_rate and y_rate values from the gyro are within bounds of the search area. If not, an empty flow is returned. - Corrected parameter name in header file. --- inc/flow.h | 4 ++-- src/flow.c | 10 ++++++++++ 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/inc/flow.h b/inc/flow.h index e867639..258fc0c 100644 --- a/inc/flow.h +++ b/inc/flow.h @@ -61,7 +61,7 @@ typedef struct _flow_klt_image { * @param y_rate The gyro y-rate during the frame interval in pixels. (In the image y direction) * @param z_rate The gyro z-rate during the frame interval in radians. * @param out Array which receives the raw result vectors computed for the blocks in the image. - * @param result_count The available space in the out buffer. + * @param max_out The available space in the out buffer. * @return The number of results written to the out buffer. */ uint16_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate, @@ -85,7 +85,7 @@ void klt_preprocess_image(uint8_t *image, flow_klt_image *klt_image); * @param y_rate The gyro y-rate during the frame interval in pixels. (In the image y direction) * @param z_rate The gyro z-rate during the frame interval in radians. * @param out Array which receives the raw result vectors computed for the blocks in the image. - * @param result_count The available space in the out buffer. + * @param max_out The available space in the out buffer. * @return The number of results written to the out buffer. */ uint16_t compute_klt(flow_klt_image *image1, flow_klt_image *image2, float x_rate, float y_rate, float z_rate, diff --git a/src/flow.c b/src/flow.c index 42d3a59..cbb5aea 100644 --- a/src/flow.c +++ b/src/flow.c @@ -579,6 +579,16 @@ uint16_t compute_klt(flow_klt_image *image1, flow_klt_image *image2, float x_rat //initialize flow values with the pixel value of the previous image uint16_t topPyrStep = 1 << (PYR_LVLS - 1); + + /* + * if the gyro change (x_rate & y_rate) is more than the maximum pixel + * difference that can be detected between two frames (depends on PYR_LVLS) + * we can't calculate the flow. So return empty flow. + */ + if(fabs(x_rate) > (float)(2 * topPyrStep) || fabs(y_rate) > (float)(2 * topPyrStep)){ + return 0; + } + uint16_t pixStep = frame_size / (NUM_BLOCK_KLT + 1); uint16_t pixLo = pixStep; /* align with topPyrStep */ From 6493c8ffc3d03514e32a80239ad547355d731221 Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Thu, 23 Jul 2015 14:23:54 -0700 Subject: [PATCH 067/120] Switch device descriptor to structure definition --- inc/usb_standard.h | 201 ++++++++++++++++++ .../Class/cdc/src/usbd_cdc_core.c | 6 - src/modules/flow/usbd_desc.c | 58 ++--- 3 files changed, 222 insertions(+), 43 deletions(-) create mode 100644 inc/usb_standard.h diff --git a/inc/usb_standard.h b/inc/usb_standard.h new file mode 100644 index 0000000..6392c5b --- /dev/null +++ b/inc/usb_standard.h @@ -0,0 +1,201 @@ +#pragma once + +#include + +// Device -> host +#define USB_IN 0x80 +// Host -> device +#define USB_OUT 0 + +#define USB_REQTYPE_DIRECTION_MASK 0x80 +#define USB_REQTYPE_TYPE_MASK 0x60 +#define USB_REQTYPE_RECIPIENT_MASK 0x1F + +#define USB_REQTYPE_STANDARD (0 << 5) +#define USB_REQTYPE_CLASS (1 << 5) +#define USB_REQTYPE_VENDOR (2 << 5) + +#define USB_RECIPIENT_DEVICE (0 << 0) +#define USB_RECIPIENT_INTERFACE (1 << 0) +#define USB_RECIPIENT_ENDPOINT (2 << 0) +#define USB_RECIPIENT_OTHER (3 << 0) + +typedef struct { + uint8_t bmRequestType; + uint8_t bRequest; + uint16_t wValue; + uint16_t wIndex; + uint16_t wLength; +} __attribute__ ((packed)) USB_SetupPacket; + +// Standard requests +enum { + USB_REQ_GetStatus = 0, + USB_REQ_ClearFeature = 1, + USB_REQ_SetFeature = 3, + USB_REQ_SetAddress = 5, + USB_REQ_GetDescriptor = 6, + USB_REQ_SetDescriptor = 7, + USB_REQ_GetConfiguration = 8, + USB_REQ_SetConfiguration = 9, + USB_REQ_GetInterface = 10, + USB_REQ_SetInterface = 11, + USB_REQ_SynchFrame = 12, +}; + +enum { + USB_FEATURE_EndpointHalt = 0x00, + USB_FEATURE_DeviceRemoteWakeup = 0x01, + USB_FEATURE_TestMode = 0x02, +}; + +enum { + USB_DTYPE_Device = 0x01, + USB_DTYPE_Configuration = 0x02, + USB_DTYPE_String = 0x03, + USB_DTYPE_Interface = 0x04, + USB_DTYPE_Endpoint = 0x05, + USB_DTYPE_DeviceQualifier = 0x06, + USB_DTYPE_Other = 0x07, + USB_DTYPE_InterfacePower = 0x08, + USB_DTYPE_InterfaceAssociation = 0x0B, + USB_DTYPE_CSInterface = 0x24, + USB_DTYPE_CSEndpoint = 0x25, +} USB_dtype; + +typedef enum { + USB_CSCP_NoDeviceClass = 0x00, + USB_CSCP_NoDeviceSubclass = 0x00, + USB_CSCP_NoDeviceProtocol = 0x00, + USB_CSCP_VendorSpecificClass = 0xFF, + USB_CSCP_VendorSpecificSubclass = 0xFF, + USB_CSCP_VendorSpecificProtocol = 0xFF, + USB_CSCP_IADDeviceClass = 0xEF, + USB_CSCP_IADDeviceSubclass = 0x02, + USB_CSCP_IADDeviceProtocol = 0x01, +} USB_cscp; + +#define USB_CONFIG_POWER_MA(mA) ((mA)/2) +#define USB_STRING_LEN(c) (sizeof(USB_DescriptorHeader) + ((c) * 2)) + +#define USB_LANGUAGE_EN_US 0x0409 + +#define ENDPOINT_DESCRIPTOR_DIR_IN 0x80 +#define ENDPOINT_DESCRIPTOR_DIR_OUT 0x00 + +#define USB_CONFIG_ATTR_BUSPOWERED 0x80 +#define USB_CONFIG_ATTR_SELFPOWERED 0x40 +#define USB_CONFIG_ATTR_REMOTEWAKEUP 0x20 + +#define ENDPOINT_ATTR_NO_SYNC (0 << 2) +#define ENDPOINT_ATTR_ASYNC (1 << 2) +#define ENDPOINT_ATTR_ADAPTIVE (2 << 2) +#define ENDPOINT_ATTR_SYNC (3 << 2) + +#define ENDPOINT_USAGE_DATA (0 << 4) +#define ENDPOINT_USAGE_FEEDBACK (1 << 4) +#define ENDPOINT_USAGE_IMPLICIT_FEEDBACK (2 << 4) + +#define USB_EP_TYPE_CONTROL 0x00 +#define USB_EP_TYPE_ISOCHRONOUS 0x01 +#define USB_EP_TYPE_BULK 0x02 +#define USB_EP_TYPE_INTERRUPT 0x03 + +typedef struct { + uint8_t bLength; + uint8_t bDescriptorType; +} __attribute__ ((packed)) USB_DescriptorHeader; + +typedef struct { + uint8_t bLength; + uint8_t bDescriptorType; + uint16_t bcdUSB; + uint8_t bDeviceClass; + uint8_t bDeviceSubClass; + uint8_t bDeviceProtocol; + uint8_t bMaxPacketSize0; + uint16_t idVendor; + uint16_t idProduct; + uint16_t bcdDevice; + uint8_t iManufacturer; + uint8_t iProduct; + uint8_t iSerialNumber; + uint8_t bNumConfigurations; +} __attribute__ ((packed)) USB_DeviceDescriptor; + +typedef struct { + uint8_t bLength; + uint8_t bDescriptorType; + uint16_t bcdUSB; + uint8_t bDeviceSubClass; + uint8_t bMaxPacketSize0; + uint8_t bNumConfigurations; + uint8_t bReserved; +} __attribute__ ((packed)) USB_DeviceQualifierDescriptor; + +typedef struct { + uint8_t bLength; + uint8_t bDescriptorType; + uint16_t wTotalLength; + uint8_t bNumInterfaces; + uint8_t bConfigurationValue; + uint8_t iConfiguration; + uint8_t bmAttributes; + uint8_t bMaxPower; +} __attribute__ ((packed)) USB_ConfigurationDescriptor; + +typedef struct { + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bInterfaceNumber; + uint8_t bAlternateSetting; + uint8_t bNumEndpoints; + uint8_t bInterfaceClass; + uint8_t bInterfaceSubClass; + uint8_t bInterfaceProtocol; + uint8_t iInterface; +} __attribute__ ((packed)) USB_InterfaceDescriptor; + +typedef struct { + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bFirstInterface; + uint8_t bInterfaceCount; + uint8_t bFunctionClass; + uint8_t bFunctionSubClass; + uint8_t bFunctionProtocol; + uint8_t iFunction; +} __attribute__ ((packed)) USB_InterfaceAssociationDescriptor; + +typedef struct { + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bEndpointAddress; + uint8_t bmAttributes; + uint16_t wMaxPacketSize; + uint8_t bInterval; +} __attribute__ ((packed)) USB_EndpointDescriptor; + +typedef struct { + uint8_t bLength; + uint8_t bDescriptorType; + __CHAR16_TYPE__ bString[]; +} __attribute__ ((packed)) USB_StringDescriptor; + +/// Microsoft WCID descriptor +typedef struct { + uint8_t bFirstInterfaceNumber; + uint8_t reserved1; + uint8_t compatibleID[8]; + uint8_t subCompatibleID[8]; + uint8_t reserved2[6]; +} __attribute__((packed)) USB_MicrosoftCompatibleDescriptor_Interface; + +typedef struct { + uint32_t dwLength; + uint16_t bcdVersion; + uint16_t wIndex; + uint8_t bCount; + uint8_t reserved[7]; + USB_MicrosoftCompatibleDescriptor_Interface interfaces[]; +} __attribute__((packed)) USB_MicrosoftCompatibleDescriptor; diff --git a/src/lib/stm32/st/v1.0.2/STM32_USB_Device_Library/Class/cdc/src/usbd_cdc_core.c b/src/lib/stm32/st/v1.0.2/STM32_USB_Device_Library/Class/cdc/src/usbd_cdc_core.c index a91772b..930c2ec 100644 --- a/src/lib/stm32/st/v1.0.2/STM32_USB_Device_Library/Class/cdc/src/usbd_cdc_core.c +++ b/src/lib/stm32/st/v1.0.2/STM32_USB_Device_Library/Class/cdc/src/usbd_cdc_core.c @@ -429,8 +429,6 @@ __ALIGN_BEGIN uint8_t usbd_cdc_OtherCfgDesc[USB_CDC_CONFIG_DESC_SIZ] __ALIGN_EN static uint8_t usbd_cdc_Init (void *pdev, uint8_t cfgidx) { - uint8_t *pbuf; - /* Open EP IN */ DCD_EP_Open(pdev, CDC_IN_EP, @@ -449,10 +447,6 @@ static uint8_t usbd_cdc_Init (void *pdev, CDC_CMD_PACKET_SZE, USB_OTG_EP_INT); - pbuf = (uint8_t *)USBD_DeviceDesc; - pbuf[4] = DEVICE_CLASS_CDC; - pbuf[5] = DEVICE_SUBCLASS_CDC; - /* Initialize the Interface physical components */ APP_FOPS.pIf_Init(); diff --git a/src/modules/flow/usbd_desc.c b/src/modules/flow/usbd_desc.c index 20e7624..b1942e3 100644 --- a/src/modules/flow/usbd_desc.c +++ b/src/modules/flow/usbd_desc.c @@ -30,6 +30,7 @@ #include "usbd_req.h" #include "usbd_conf.h" #include "usb_regs.h" +#include "usb_standard.h" /* Vendor ID */ #define USBD_VID 0x26AC @@ -63,42 +64,25 @@ USBD_DEVICE USR_desc = USBD_USR_InterfaceStrDescriptor, }; -/* USB Standard Device Descriptor */ -__ALIGN_BEGIN uint8_t USBD_DeviceDesc[USB_SIZ_DEVICE_DESC] __ALIGN_END = -{ - 0x12, /*bLength */ - USB_DEVICE_DESCRIPTOR_TYPE, /*bDescriptorType*/ - 0x00, /*bcdUSB */ - 0x02, - 0x00, /*bDeviceClass*/ - 0x00, /*bDeviceSubClass*/ - 0x00, /*bDeviceProtocol*/ - USB_OTG_MAX_EP0_SIZE, /*bMaxPacketSize*/ - LOBYTE(USBD_VID), /*idVendor*/ - HIBYTE(USBD_VID), /*idVendor*/ - LOBYTE(USBD_PID), /*idVendor*/ - HIBYTE(USBD_PID), /*idVendor*/ - 0x00, /*bcdDevice rel. 2.00*/ - 0x02, - USBD_IDX_MFC_STR, /*Index of manufacturer string*/ - USBD_IDX_PRODUCT_STR, /*Index of product string*/ - USBD_IDX_SERIAL_STR, /*Index of serial number string*/ - USBD_CFG_MAX_NUM /*bNumConfigurations*/ - } ; /* USB_DeviceDescriptor */ +const USB_DeviceDescriptor device_descriptor = { + .bLength = sizeof(USB_DeviceDescriptor), + .bDescriptorType = USB_DTYPE_Device, -/* USB Standard Device Descriptor */ -__ALIGN_BEGIN uint8_t USBD_DeviceQualifierDesc[USB_LEN_DEV_QUALIFIER_DESC] __ALIGN_END = -{ - USB_LEN_DEV_QUALIFIER_DESC, - USB_DESC_TYPE_DEVICE_QUALIFIER, - 0x00, - 0x02, - 0x00, - 0x00, - 0x00, - 0x40, - 0x01, - 0x00, + .bcdUSB = 0x0200, + .bDeviceClass = 0, + .bDeviceSubClass = USB_CSCP_NoDeviceSubclass, + .bDeviceProtocol = USB_CSCP_NoDeviceProtocol, + + .bMaxPacketSize0 = 64, + .idVendor = USBD_VID, + .idProduct = USBD_PID, + .bcdDevice = 0x0200, + + .iManufacturer = USBD_IDX_MFC_STR, + .iProduct = USBD_IDX_PRODUCT_STR, + .iSerialNumber = USBD_IDX_SERIAL_STR, + + .bNumConfigurations = USBD_CFG_MAX_NUM }; /* USB Standard Device Descriptor */ @@ -119,8 +103,8 @@ __ALIGN_BEGIN uint8_t USBD_LangIDDesc[USB_SIZ_STRING_LANGID] __ALIGN_END = */ uint8_t * USBD_USR_DeviceDescriptor( uint8_t speed , uint16_t *length) { - *length = sizeof(USBD_DeviceDesc); - return USBD_DeviceDesc; + *length = sizeof(device_descriptor); + return (uint8_t*) &device_descriptor; } /** From 430c01da4715689b31a4aa75c20130bb251f6284 Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Thu, 23 Jul 2015 15:21:32 -0700 Subject: [PATCH 068/120] Move CDC descriptors to composite device, using structures This replaces the descriptor byte arrays from usbd_cdc_core.c The following changes were made to the descriptor content: * The device is self-powered * Don't set flag indicating that it accepts AT commands * Remove Call Management Functional Descriptor --- src/include/cdc_standard.h | 45 ++++++++ {inc => src/include}/usb_standard.h | 2 +- src/modules/flow/main.c | 3 +- src/modules/flow/module.mk | 3 +- src/modules/flow/usb_composite.c | 155 ++++++++++++++++++++++++++++ 5 files changed, 205 insertions(+), 3 deletions(-) create mode 100644 src/include/cdc_standard.h rename {inc => src/include}/usb_standard.h (99%) create mode 100644 src/modules/flow/usb_composite.c diff --git a/src/include/cdc_standard.h b/src/include/cdc_standard.h new file mode 100644 index 0000000..ed19353 --- /dev/null +++ b/src/include/cdc_standard.h @@ -0,0 +1,45 @@ +#pragma once + +#define CDC_INTERFACE_CLASS 0x02 +#define CDC_INTERFACE_SUBCLASS_ACM 0x02 +#define CDC_INTERFACE_CLASS_DATA 0x0A + +#define CDC_SUBTYPE_HEADER 0x00 +#define CDC_SUBTYPE_ACM 0x02 +#define CDC_SUBTYPE_UNION 0x06 + +#define CDC_SEND_ENCAPSULATED_COMMAND 0x0 +#define CDC_GET_ENCAPSULATED_RESPONSE 0x1 +#define CDC_SET_LINE_ENCODING 0x20 +#define CDC_GET_LINE_ENCODING 0x21 +#define CDC_SET_CONTROL_LINE_STATE 0x22 +#define CDC_SEND_BREAK 0x23 + +typedef struct { + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bDescriptorSubtype; + uint16_t bcdCDC; +} __attribute__((packed)) CDC_FunctionalHeaderDescriptor; + +typedef struct { + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bDescriptorSubtype; + uint8_t bmCapabilities; +} __attribute__((packed)) CDC_FunctionalACMDescriptor; + +typedef struct { + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bDescriptorSubtype; + uint8_t bMasterInterface; + uint8_t bSlaveInterface; +} __attribute__((packed)) CDC_FunctionalUnionDescriptor; + +typedef struct { + uint32_t baud_rate; + uint8_t char_format; + uint8_t parity_type; + uint8_t data_bits; +} __attribute__((packed)) CDC_LineEncoding; diff --git a/inc/usb_standard.h b/src/include/usb_standard.h similarity index 99% rename from inc/usb_standard.h rename to src/include/usb_standard.h index 6392c5b..a830b02 100644 --- a/inc/usb_standard.h +++ b/src/include/usb_standard.h @@ -61,7 +61,7 @@ enum { USB_DTYPE_InterfaceAssociation = 0x0B, USB_DTYPE_CSInterface = 0x24, USB_DTYPE_CSEndpoint = 0x25, -} USB_dtype; +}; typedef enum { USB_CSCP_NoDeviceClass = 0x00, diff --git a/src/modules/flow/main.c b/src/modules/flow/main.c index da08b67..8d9c4ec 100644 --- a/src/modules/flow/main.c +++ b/src/modules/flow/main.c @@ -84,6 +84,7 @@ __ALIGN_BEGIN USB_OTG_CORE_HANDLE USB_OTG_dev __ALIGN_END; +extern USBD_Class_cb_TypeDef custom_composite_cb; /* timer constants */ #define SONAR_POLL_MS 100 /* steps in milliseconds ticks */ @@ -240,7 +241,7 @@ int main(void) USBD_Init( &USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, - &USBD_CDC_cb, + &custom_composite_cb, &USR_cb); /* init mavlink */ diff --git a/src/modules/flow/module.mk b/src/modules/flow/module.mk index 230ff77..13d057b 100644 --- a/src/modules/flow/module.mk +++ b/src/modules/flow/module.mk @@ -43,7 +43,8 @@ SRCS += main.c \ camera.c \ filter.c \ result_accumulator.c \ - timer.c + timer.c \ + usb_composite.c \ SRCS += $(ST_LIB)STM32F4xx_StdPeriph_Driver/src/misc.c \ diff --git a/src/modules/flow/usb_composite.c b/src/modules/flow/usb_composite.c new file mode 100644 index 0000000..88083cd --- /dev/null +++ b/src/modules/flow/usb_composite.c @@ -0,0 +1,155 @@ +#include "usb_standard.h" +#include "cdc_standard.h" +#include "usbd_conf.h" +#include "usbd_cdc_core.h" + +#define INTERFACE_CDC_CONTROL 0 +#define INTERFACE_CDC_DATA 1 + +typedef struct ConfigDesc { + USB_ConfigurationDescriptor Config; + + USB_InterfaceDescriptor CDC_control_interface; + CDC_FunctionalHeaderDescriptor CDC_functional_header; + CDC_FunctionalACMDescriptor CDC_functional_ACM; + CDC_FunctionalUnionDescriptor CDC_functional_union; + USB_EndpointDescriptor CDC_notification_endpoint; + + USB_InterfaceDescriptor CDC_data_interface; + USB_EndpointDescriptor CDC_out_endpoint; + USB_EndpointDescriptor CDC_in_endpoint; +} __attribute__((packed)) ConfigDesc; + +const ConfigDesc configuration_descriptor = { + .Config = { + .bLength = sizeof(USB_ConfigurationDescriptor), + .bDescriptorType = USB_DTYPE_Configuration, + .wTotalLength = sizeof(ConfigDesc), + .bNumInterfaces = 2, + .bConfigurationValue = 1, + .iConfiguration = 0, + .bmAttributes = USB_CONFIG_ATTR_BUSPOWERED, + .bMaxPower = USB_CONFIG_POWER_MA(500) + }, + .CDC_control_interface = { + .bLength = sizeof(USB_InterfaceDescriptor), + .bDescriptorType = USB_DTYPE_Interface, + .bInterfaceNumber = INTERFACE_CDC_CONTROL, + .bAlternateSetting = 0, + .bNumEndpoints = 1, + .bInterfaceClass = CDC_INTERFACE_CLASS, + .bInterfaceSubClass = CDC_INTERFACE_SUBCLASS_ACM, + .bInterfaceProtocol = 0, + .iInterface = 0, + }, + .CDC_functional_header = { + .bLength = sizeof(CDC_FunctionalHeaderDescriptor), + .bDescriptorType = USB_DTYPE_CSInterface, + .bDescriptorSubtype = CDC_SUBTYPE_HEADER, + .bcdCDC = 0x0110, + }, + .CDC_functional_ACM = { + .bLength = sizeof(CDC_FunctionalACMDescriptor), + .bDescriptorType = USB_DTYPE_CSInterface, + .bDescriptorSubtype = CDC_SUBTYPE_ACM, + .bmCapabilities = 0x00, + }, + .CDC_functional_union = { + .bLength = sizeof(CDC_FunctionalUnionDescriptor), + .bDescriptorType = USB_DTYPE_CSInterface, + .bDescriptorSubtype = CDC_SUBTYPE_UNION, + .bMasterInterface = INTERFACE_CDC_CONTROL, + .bSlaveInterface = INTERFACE_CDC_DATA, + }, + .CDC_notification_endpoint = { + .bLength = sizeof(USB_EndpointDescriptor), + .bDescriptorType = USB_DTYPE_Endpoint, + .bEndpointAddress = CDC_CMD_EP, + .bmAttributes = (USB_EP_TYPE_INTERRUPT | ENDPOINT_ATTR_NO_SYNC | ENDPOINT_USAGE_DATA), + .wMaxPacketSize = CDC_CMD_PACKET_SZE, + .bInterval = 0xFF + }, + .CDC_data_interface = { + .bLength = sizeof(USB_InterfaceDescriptor), + .bDescriptorType = USB_DTYPE_Interface, + .bInterfaceNumber = INTERFACE_CDC_DATA, + .bAlternateSetting = 0, + .bNumEndpoints = 2, + .bInterfaceClass = CDC_INTERFACE_CLASS_DATA, + .bInterfaceSubClass = 0, + .bInterfaceProtocol = 0, + .iInterface = 0, + }, + .CDC_out_endpoint = { + .bLength = sizeof(USB_EndpointDescriptor), + .bDescriptorType = USB_DTYPE_Endpoint, + .bEndpointAddress = CDC_OUT_EP, + .bmAttributes = (USB_EP_TYPE_BULK | ENDPOINT_ATTR_NO_SYNC | ENDPOINT_USAGE_DATA), + .wMaxPacketSize = CDC_DATA_MAX_PACKET_SIZE, + .bInterval = 0x0 + }, + .CDC_in_endpoint = { + .bLength = sizeof(USB_EndpointDescriptor), + .bDescriptorType = USB_DTYPE_Endpoint, + .bEndpointAddress = CDC_IN_EP, + .bmAttributes = (USB_EP_TYPE_BULK | ENDPOINT_ATTR_NO_SYNC | ENDPOINT_USAGE_DATA), + .wMaxPacketSize = CDC_DATA_MAX_PACKET_SIZE, + .bInterval = 0x0 + }, +}; + +static uint8_t usb_Init (void *pdev, uint8_t cfgidx); +static uint8_t usb_DeInit (void *pdev, uint8_t cfgidx); +static uint8_t usb_Setup (void *pdev, USB_SETUP_REQ *req); +static uint8_t usb_EP0_RxReady (void *pdev); +static uint8_t usb_DataIn (void *pdev, uint8_t epnum); +static uint8_t usb_DataOut (void *pdev, uint8_t epnum); +static uint8_t usb_SOF (void *pdev); +static uint8_t* usb_GetCfgDesc (uint8_t speed, uint16_t *length); + +USBD_Class_cb_TypeDef custom_composite_cb = { + .Init = usb_Init, + .DeInit = usb_DeInit, + .Setup = usb_Setup, + .EP0_TxSent = NULL, + .EP0_RxReady = usb_EP0_RxReady, + .DataIn = usb_DataIn, + .DataOut = usb_DataOut, + .SOF = usb_SOF, + .IsoINIncomplete = NULL, + .IsoOUTIncomplete = NULL, + .GetConfigDescriptor = usb_GetCfgDesc, +}; + +static uint8_t usb_Init (void *pdev, uint8_t cfgidx) { + return USBD_CDC_cb.Init(pdev, cfgidx); +} + +static uint8_t usb_DeInit (void *pdev, uint8_t cfgidx) { + return USBD_CDC_cb.DeInit(pdev, cfgidx); +} + +static uint8_t usb_Setup (void *pdev, USB_SETUP_REQ *req) { + return USBD_CDC_cb.Setup(pdev, req); +} + +static uint8_t usb_EP0_RxReady (void *pdev) { + return USBD_CDC_cb.EP0_RxReady(pdev); +} + +static uint8_t usb_DataIn (void *pdev, uint8_t epnum) { + return USBD_CDC_cb.DataIn(pdev, epnum); +} + +static uint8_t usb_DataOut (void *pdev, uint8_t epnum) { + return USBD_CDC_cb.DataOut(pdev, epnum); +} + +static uint8_t usb_SOF (void *pdev) { + return USBD_CDC_cb.SOF(pdev); +} + +static uint8_t* usb_GetCfgDesc (uint8_t speed, uint16_t *length) { + *length = sizeof (configuration_descriptor); + return (uint8_t*) &configuration_descriptor; +} From 16af0b346c60097335d3cb3dc1e11d432c6d2af0 Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Fri, 24 Jul 2015 11:16:28 -0700 Subject: [PATCH 069/120] Add image transfer endpoints --- read_image.py | 28 +++++++++++++++ src/include/main.h | 1 + src/include/usb_conf.h | 8 ++--- src/include/usbd_conf.h | 6 ++-- src/modules/flow/main.c | 62 +++++++++++++++++++++++++++----- src/modules/flow/usb_composite.c | 43 +++++++++++++++++++--- 6 files changed, 129 insertions(+), 19 deletions(-) create mode 100644 read_image.py diff --git a/read_image.py b/read_image.py new file mode 100644 index 0000000..7a5fd4b --- /dev/null +++ b/read_image.py @@ -0,0 +1,28 @@ +import usb.core, usb.util +import numpy as np +import matplotlib.pyplot as plt + +dev = usb.core.find(idVendor=0x26ac, idProduct=0x0015) +endpoint = dev[0][(2,0)][0] + +SIZE = 352 +image = np.zeros((SIZE, SIZE), dtype='uint8') +imshow = plt.imshow(image, cmap=plt.cm.gray) +imshow.norm.vmin = 0 +imshow.norm.vmax = 255 +plt.ion() +plt.show() + +while True: + data = endpoint.read(64 * (1 + (SIZE*SIZE) / 64), timeout=20000) + + if len(data) != SIZE*SIZE: + # We read a partial image from killing a previous instance of This + # program in the middle of a transfer. We should be using an interface + # alternate setting to enable and reset the endpoint. + print "Ignoring partial buffer of", len(data) + continue + + image = np.frombuffer(data, dtype='uint8').reshape(SIZE, SIZE) + imshow.set_data(image) + plt.pause(1e-9) diff --git a/src/include/main.h b/src/include/main.h index b22024b..a9005f3 100644 --- a/src/include/main.h +++ b/src/include/main.h @@ -35,5 +35,6 @@ #define __PX4_FLOWBOARD_H void notify_changed_camera_parameters(void); +void send_image_completed(void); #endif /* __PX4_FLOWBOARD_H */ diff --git a/src/include/usb_conf.h b/src/include/usb_conf.h index 3ab5c3c..ab7c357 100644 --- a/src/include/usb_conf.h +++ b/src/include/usb_conf.h @@ -69,11 +69,11 @@ * so total FIFO size should be 1012 Only instead of 1024 *******************************************************************************/ -#define RX_FIFO_FS_SIZE 128 +#define RX_FIFO_FS_SIZE 64 #define TX0_FIFO_FS_SIZE 64 -#define TX1_FIFO_FS_SIZE 128 -#define TX2_FIFO_FS_SIZE 32 -#define TX3_FIFO_FS_SIZE 0 +#define TX1_FIFO_FS_SIZE 64 +#define TX2_FIFO_FS_SIZE 16 +#define TX3_FIFO_FS_SIZE 64 /****************** USB OTG MISC CONFIGURATION ********************************/ #define VBUS_SENSING_ENABLED diff --git a/src/include/usbd_conf.h b/src/include/usbd_conf.h index 3ae75b3..56e309b 100644 --- a/src/include/usbd_conf.h +++ b/src/include/usbd_conf.h @@ -31,17 +31,19 @@ #include "usb_conf.h" #define USBD_CFG_MAX_NUM 1 -#define USBD_ITF_MAX_NUM 1 -#define USB_MAX_STR_DESC_SIZ 255 +#define USBD_ITF_MAX_NUM 3 +#define USB_MAX_STR_DESC_SIZ 255 #define CDC_IN_EP 0x81 /* EP1 for data IN */ #define CDC_OUT_EP 0x01 /* EP1 for data OUT */ #define CDC_CMD_EP 0x82 /* EP2 for CDC commands */ +#define IMAGE_IN_EP 0x83 /* CDC Endpoints parameters: you can fine tune these values depending on the needed baudrates and performance. */ #define CDC_DATA_MAX_PACKET_SIZE 64 /* Endpoint IN & OUT Packet size */ #define CDC_CMD_PACKET_SZE 8 /* Control Endpoint Packet size */ + #define IMAGE_IN_PACKET_SIZE 64 /* Number of frames between IN transfers */ #define CDC_IN_FRAME_INTERVAL 40 //5 diff --git a/src/modules/flow/main.c b/src/modules/flow/main.c index 8d9c4ec..8e68a44 100644 --- a/src/modules/flow/main.c +++ b/src/modules/flow/main.c @@ -86,6 +86,21 @@ __ALIGN_BEGIN USB_OTG_CORE_HANDLE USB_OTG_dev __ALIGN_END; extern USBD_Class_cb_TypeDef custom_composite_cb; +//TODO: this should use board-specific code instead of an ifdef here +#ifdef CONFIG_ARCH_BOARD_PX4FLOW_V2 +// USB_IMAGE_PIXELS*USB_IMAGE_PIXELS % 1024 must equal 0 +#define USB_IMAGE_PIXELS (352) +#else +#define USB_IMAGE_PIXELS (128) +#endif + +#define FLOW_IMAGE_SIZE (64) + +static volatile bool snap_capture_done = false; +static volatile bool snap_capture_success = false; +static bool snap_ready = true; +volatile bool usb_image_transfer_active = false; + /* timer constants */ #define SONAR_POLL_MS 100 /* steps in milliseconds ticks */ #define SYSTEM_STATE_MS 1000/* steps in milliseconds ticks */ @@ -95,7 +110,7 @@ extern USBD_Class_cb_TypeDef custom_composite_cb; static camera_ctx cam_ctx; static camera_img_param img_stream_param; -uint8_t snapshot_buffer_mem[128 * 128]; +uint8_t snapshot_buffer_mem[USB_IMAGE_PIXELS * USB_IMAGE_PIXELS]; static camera_image_buffer snapshot_buffer; @@ -168,20 +183,16 @@ static void send_params_fn(void) { camera_img_stream_schedule_param_change(&cam_ctx, &img_stream_param); }*/ -static volatile bool snap_capture_done = false; -static volatile bool snap_capture_success = false; -static bool snap_ready = true; - static void snapshot_captured_fn(bool success) { snap_capture_done = true; snap_capture_success = success; } static void take_snapshot_fn(void) { - if (FLOAT_AS_BOOL(global_data.param[PARAM_USB_SEND_VIDEO]) && FLOAT_AS_BOOL(global_data.param[PARAM_VIDEO_ONLY]) && snap_ready) { + if (FLOAT_AS_BOOL(global_data.param[PARAM_USB_SEND_VIDEO]) && FLOAT_AS_BOOL(global_data.param[PARAM_VIDEO_ONLY]) && snap_ready && !usb_image_transfer_active) { static camera_img_param snapshot_param; - snapshot_param.size.x = 128; - snapshot_param.size.y = 128; + snapshot_param.size.x = USB_IMAGE_PIXELS; + snapshot_param.size.y = USB_IMAGE_PIXELS; snapshot_param.binning = 1; if (camera_snapshot_schedule(&cam_ctx, &snapshot_param, &snapshot_buffer, snapshot_captured_fn)) { snap_ready = false; @@ -189,6 +200,39 @@ static void take_snapshot_fn(void) { } } +unsigned usb_image_pos = 0; +#define MAX_TRANSFER (1023*64) + +static void send_image_step(void) { + unsigned size = USB_IMAGE_PIXELS*USB_IMAGE_PIXELS - usb_image_pos; + if (size > MAX_TRANSFER) size = MAX_TRANSFER; + + DCD_EP_Tx(&USB_OTG_dev, IMAGE_IN_EP, &snapshot_buffer_mem[usb_image_pos], size); + if (size == 0 || size % 64 != 0) { + // We sent a short packet at the end of the transfer. When it completes, we're done. + usb_image_pos = (unsigned) -1; + } else { + usb_image_pos += size; + } +} + +static void start_send_image(void) { + LEDOn(LED_COM); + usb_image_transfer_active = true; + usb_image_pos = 0; + DCD_EP_Flush(&USB_OTG_dev, IMAGE_IN_EP); + send_image_step(); +} + +void send_image_completed(void) { + if (usb_image_pos != (unsigned) -1) { + send_image_step(); + } else { + usb_image_transfer_active = false; + LEDOff(LED_COM); + } +} + void notify_changed_camera_parameters(void) { camera_reconfigure_general(&cam_ctx); } @@ -320,7 +364,7 @@ int main(void) if (snap_capture_success) { /* send the snapshot! */ LEDToggle(LED_COM); - mavlink_send_image(&snapshot_buffer); + start_send_image(); } } diff --git a/src/modules/flow/usb_composite.c b/src/modules/flow/usb_composite.c index 88083cd..2a26815 100644 --- a/src/modules/flow/usb_composite.c +++ b/src/modules/flow/usb_composite.c @@ -5,6 +5,7 @@ #define INTERFACE_CDC_CONTROL 0 #define INTERFACE_CDC_DATA 1 +#define INTERFACE_IMAGE 2 typedef struct ConfigDesc { USB_ConfigurationDescriptor Config; @@ -18,6 +19,9 @@ typedef struct ConfigDesc { USB_InterfaceDescriptor CDC_data_interface; USB_EndpointDescriptor CDC_out_endpoint; USB_EndpointDescriptor CDC_in_endpoint; + + USB_InterfaceDescriptor ImageInterface; + USB_EndpointDescriptor ImageInEndpoint; } __attribute__((packed)) ConfigDesc; const ConfigDesc configuration_descriptor = { @@ -25,7 +29,7 @@ const ConfigDesc configuration_descriptor = { .bLength = sizeof(USB_ConfigurationDescriptor), .bDescriptorType = USB_DTYPE_Configuration, .wTotalLength = sizeof(ConfigDesc), - .bNumInterfaces = 2, + .bNumInterfaces = 3, .bConfigurationValue = 1, .iConfiguration = 0, .bmAttributes = USB_CONFIG_ATTR_BUSPOWERED, @@ -96,6 +100,26 @@ const ConfigDesc configuration_descriptor = { .wMaxPacketSize = CDC_DATA_MAX_PACKET_SIZE, .bInterval = 0x0 }, + + .ImageInterface = { + .bLength = sizeof(USB_InterfaceDescriptor), + .bDescriptorType = USB_DTYPE_Interface, + .bInterfaceNumber = INTERFACE_IMAGE, + .bAlternateSetting = 0, + .bNumEndpoints = 1, + .bInterfaceClass = USB_CSCP_VendorSpecificClass, + .bInterfaceSubClass = 0x00, + .bInterfaceProtocol = 0x00, + .iInterface = 0, + }, + .ImageInEndpoint = { + .bLength = sizeof(USB_EndpointDescriptor), + .bDescriptorType = USB_DTYPE_Endpoint, + .bEndpointAddress = IMAGE_IN_EP, + .bmAttributes = (USB_EP_TYPE_BULK | ENDPOINT_ATTR_NO_SYNC | ENDPOINT_USAGE_DATA), + .wMaxPacketSize = IMAGE_IN_PACKET_SIZE, + .bInterval = 0x00 + }, }; static uint8_t usb_Init (void *pdev, uint8_t cfgidx); @@ -122,11 +146,15 @@ USBD_Class_cb_TypeDef custom_composite_cb = { }; static uint8_t usb_Init (void *pdev, uint8_t cfgidx) { - return USBD_CDC_cb.Init(pdev, cfgidx); + USBD_CDC_cb.Init(pdev, cfgidx); + DCD_EP_Open(pdev, IMAGE_IN_EP, IMAGE_IN_PACKET_SIZE, USB_OTG_EP_BULK); + return USBD_OK; } static uint8_t usb_DeInit (void *pdev, uint8_t cfgidx) { - return USBD_CDC_cb.DeInit(pdev, cfgidx); + USBD_CDC_cb.DeInit(pdev, cfgidx); + DCD_EP_Close(pdev, IMAGE_IN_EP); + return USBD_OK; } static uint8_t usb_Setup (void *pdev, USB_SETUP_REQ *req) { @@ -137,8 +165,15 @@ static uint8_t usb_EP0_RxReady (void *pdev) { return USBD_CDC_cb.EP0_RxReady(pdev); } +void send_image_completed(void); + static uint8_t usb_DataIn (void *pdev, uint8_t epnum) { - return USBD_CDC_cb.DataIn(pdev, epnum); + if (epnum == (CDC_IN_EP & 0x7f) || epnum == (CDC_CMD_EP & 0x7f)) { + return USBD_CDC_cb.DataIn(pdev, epnum); + } else if (epnum == (IMAGE_IN_EP & 0x7f)) { + send_image_completed(); + } + return USBD_OK; } static uint8_t usb_DataOut (void *pdev, uint8_t epnum) { From d33cf8d46080a78cab268f4095cc049f2126a93b Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Wed, 19 Aug 2015 12:40:56 -0700 Subject: [PATCH 070/120] Send metadata with USB image Timestamp is currently local time; should be replaced with UAVCAN time --- read_image.py | 15 ++++++++++++--- src/modules/flow/main.c | 20 ++++++++++++++++---- 2 files changed, 28 insertions(+), 7 deletions(-) diff --git a/read_image.py b/read_image.py index 7a5fd4b..baa2725 100644 --- a/read_image.py +++ b/read_image.py @@ -1,6 +1,7 @@ import usb.core, usb.util import numpy as np import matplotlib.pyplot as plt +import struct dev = usb.core.find(idVendor=0x26ac, idProduct=0x0015) endpoint = dev[0][(2,0)][0] @@ -13,16 +14,24 @@ plt.ion() plt.show() +HEADER_SIZE = 16 +READ_SIZE = HEADER_SIZE + SIZE*SIZE + while True: - data = endpoint.read(64 * (1 + (SIZE*SIZE) / 64), timeout=20000) - if len(data) != SIZE*SIZE: + data = endpoint.read(64 * (1 + (READ_SIZE) / 64), timeout=20000) # Round up to include the zero-length packet + + if len(data) != READ_SIZE: # We read a partial image from killing a previous instance of This # program in the middle of a transfer. We should be using an interface # alternate setting to enable and reset the endpoint. print "Ignoring partial buffer of", len(data) continue - image = np.frombuffer(data, dtype='uint8').reshape(SIZE, SIZE) + (flags, timestamp, exposure, reserved) = struct.unpack(' MAX_TRANSFER) size = MAX_TRANSFER; - DCD_EP_Tx(&USB_OTG_dev, IMAGE_IN_EP, &snapshot_buffer_mem[usb_image_pos], size); + DCD_EP_Tx(&USB_OTG_dev, IMAGE_IN_EP, &((uint8_t*)&usb_packet)[usb_image_pos], size); if (size == 0 || size % 64 != 0) { // We sent a short packet at the end of the transfer. When it completes, we're done. usb_image_pos = (unsigned) -1; @@ -220,6 +228,10 @@ static void start_send_image(void) { LEDOn(LED_COM); usb_image_transfer_active = true; usb_image_pos = 0; + usb_packet.flags = 0; + usb_packet.timestamp = snapshot_buffer.timestamp; // TODO: use UAVCAN timestamp instead of local timestamp + usb_packet.exposure = snapshot_buffer.param.exposure; + usb_packet.reserved = 0; DCD_EP_Flush(&USB_OTG_dev, IMAGE_IN_EP); send_image_step(); } @@ -253,7 +265,7 @@ static flow_klt_image flow_klt_images[2] __attribute__((section(".ccm"))); int main(void) { __enable_irq(); - snapshot_buffer = BuildCameraImageBuffer(snapshot_buffer_mem); + snapshot_buffer = BuildCameraImageBuffer(usb_packet.snapshot_buffer_mem); /* load settings and parameters */ global_data_reset_param_defaults(); From 317da1f8fa874cf3806f66c9919ca185b7774b69 Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Tue, 25 Aug 2015 18:15:49 -0700 Subject: [PATCH 071/120] Move LED control to src/drivers/boards On 2.0 HW, set LED color based on OF quality. --- CMakeLists.txt | 2 - src/drivers/boards/px4flow-v1/module.mk | 1 + src/drivers/boards/px4flow-v1/px4flow_init.c | 127 ----------------- .../boards/px4flow-v1/px4flow_led.c} | 57 +++++++- src/drivers/boards/px4flow-v2/px4flow_init.c | 128 ------------------ src/drivers/boards/px4flow-v2/px4flow_led.c | 106 +++++++-------- src/include/bsp/board.h | 110 +-------------- src/include/led.h | 69 ---------- src/modules/flow/camera.c | 4 - src/modules/flow/dcmi.c | 3 - src/modules/flow/i2c.c | 2 +- src/modules/flow/main.c | 30 +--- src/modules/flow/module.mk | 1 - src/modules/flow/timer.c | 3 - src/modules/uavcannode/uavcannode_main.cpp | 1 - 15 files changed, 114 insertions(+), 530 deletions(-) rename src/{modules/flow/led.c => drivers/boards/px4flow-v1/px4flow_led.c} (76%) delete mode 100644 src/include/led.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 707baf2..85148f7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -190,7 +190,6 @@ set(PX4FLOW_HDRS inc/gyro.h inc/i2c_frame.h inc/i2c.h - inc/led.h inc/main.h inc/mavlink_bridge_header.h inc/mt9v034.h @@ -218,7 +217,6 @@ set(PX4FLOW_SRC src/flow.c src/gyro.c src/i2c.c - src/led.c src/main.c src/mt9v034.c src/reset.c diff --git a/src/drivers/boards/px4flow-v1/module.mk b/src/drivers/boards/px4flow-v1/module.mk index 2714935..fdbed3c 100644 --- a/src/drivers/boards/px4flow-v1/module.mk +++ b/src/drivers/boards/px4flow-v1/module.mk @@ -8,6 +8,7 @@ ABS_BOOTLOADER_SRC := $(PX4_BOOTLOADER_BASE)src/ SRCS = \ px4flow_init.c \ + px4flow_led.c \ BOOTLOADER_SRC = \ $(ABS_BOOTLOADER_SRC)common/boot_app_shared.c \ diff --git a/src/drivers/boards/px4flow-v1/px4flow_init.c b/src/drivers/boards/px4flow-v1/px4flow_init.c index 170090f..95440f2 100644 --- a/src/drivers/boards/px4flow-v1/px4flow_init.c +++ b/src/drivers/boards/px4flow-v1/px4flow_init.c @@ -145,131 +145,4 @@ __EXPORT int board_reset(int status) } -/**************************************************************************** - * Name: board_led_initialize - * - * Description: - * This functions is called very early in initialization to perform board- - * specific initialization of LED-related resources. This includes such - * things as, for example, configure GPIO pins to drive the LEDs and also - * putting the LEDs in their correct initial state. - * - * NOTE: In most architectures, board_led_initialize() is called from - * board-specific initialization logic. But there are a few architectures - * where this initialization function is still called from common chip - * architecture logic. This interface is not, however, a common board - * interface in any event and, hence, the usage of the name - * board_led_initialize is deprecated. - * - * WARNING: This interface name will eventually be removed; do not use it - * in new board ports. New implementations should use the naming - * conventions for "Microprocessor-Specific Interfaces" or the "Board- - * Specific Interfaces" as described above. - * - * Input Parameters: - * None - * - * Returned Value: - * None - * - ****************************************************************************/ - -__EXPORT void board_led_initialize(void) -{ - -} - -/**************************************************************************** - * Name: board_led_on - * - * Description: - * Set the LED configuration into the ON condition for the state provided - * by the led parameter. This may be one of: - * - * LED_STARTED NuttX has been started - * LED_HEAPALLOCATE Heap has been allocated - * LED_IRQSENABLED Interrupts enabled - * LED_STACKCREATED Idle stack created - * LED_INIRQ In an interrupt - * LED_SIGNAL In a signal handler - * LED_ASSERTION An assertion failed - * LED_PANIC The system has crashed - * LED_IDLE MCU is in sleep mode - * - * Where these values are defined in a board-specific way in the standard - * board.h header file exported by every architecture. - * - * Input Parameters: - * led - Identifies the LED state to put in the ON state (which may or may - * not equate to turning an LED on) - * - * Returned Value: - * None - * - ****************************************************************************/ - -__EXPORT void board_led_on(int led) -{ -} - -/**************************************************************************** - * Name: board_led_off - * - * Description: - * Set the LED configuration into the OFF condition for the state provided - * by the led parameter. This may be one of: - * - * LED_INIRQ Leaving an interrupt - * LED_SIGNAL Leaving a signal handler - * LED_ASSERTION Recovering from an assertion failure - * LED_PANIC The system has crashed (blinking). - * LED_IDLE MCU is not in sleep mode - * - * Where these values are defined in a board-specific way in the standard - * board.h header file exported by every architecture. - * - * Input Parameters: - * led - Identifies the LED state to put in the OFF state (which may or may - * not equate to turning an LED off) - * - * Returned Value: - * None - * - ****************************************************************************/ - -__EXPORT void board_led_off(int led) -{ - -} - -/**************************************************************************** - * Name: board_crashdump - * - * Description: - * If CONFIG_BOARD_CRASHDUMP is selected then up_asseert will call out to - * board_crashdump prior to calling exit in the case of an assertion failure. - * Or in the case of a hardfault looping indefinitely. board_crashdump then - * has a chance to save the state of the machine. The provided - * board_crashdump should save as much information as it can about the cause - * of the fault and then most likely reset the system. - * - * N.B. There are limited system resources that can be used by the provided - * board_crashdump function. The tems from the fact that most critical/fatal - * crashes are because of a hard fault or during interrupt processing. - * In these cases, up_assert is running from the context of an interrupt - * handlerand it is impossible to use any device driver in this context. - * - * Also consider the following: Who knows what state the system is in? Is - * memory trashed? Is the Heap intact? Therefore all we can expect to do in - * board_crashdump is save the "machine state" in a place where on the next - * reset we can write it to more sophisticated storage in a sane operating - * environment. - * - ****************************************************************************/ - -__EXPORT void board_crashdump(uint32_t currentsp, void *tcb, uint8_t *filename, - int lineno) -{ - -} diff --git a/src/modules/flow/led.c b/src/drivers/boards/px4flow-v1/px4flow_led.c similarity index 76% rename from src/modules/flow/led.c rename to src/drivers/boards/px4flow-v1/px4flow_led.c index b5cf372..2a3e774 100644 --- a/src/modules/flow/led.c +++ b/src/drivers/boards/px4flow-v1/px4flow_led.c @@ -31,8 +31,40 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ +#include -#include "led.h" +#include +#include + +#include +#include +#include "stm32f4xx_gpio.h" +#include "stm32f4xx_rcc.h" +#include "stm32f4xx_tim.h" +#include "stm32f4xx_conf.h" +#include "stm32f4xx.h" + +typedef enum +{ + LED_ACT = 0, // Blue + LED_COM = 1, // Amber + LED_ERR = 2, // Red +} Led_TypeDef; + + +#define LEDn 3 + +#define LED_ACTIVITY_PIN GPIO_Pin_3 +#define LED_ACTIVITY_GPIO_PORT GPIOE +#define LED_ACTIVITY_GPIO_CLK RCC_AHB1Periph_GPIOE + +#define LED_BOOTLOADER_PIN GPIO_Pin_2 +#define LED_BOOTLOADER_GPIO_PORT GPIOE +#define LED_BOOTLOADER_GPIO_CLK RCC_AHB1Periph_GPIOE + +#define LED_TEST_PIN GPIO_Pin_7 +#define LED_TEST_GPIO_PORT GPIOE +#define LED_TEST_GPIO_CLK RCC_AHB1Periph_GPIOE /* LED GPIOs */ GPIO_TypeDef* LED_GPIO_PORTS[LEDn] = {LED_ACTIVITY_GPIO_PORT, LED_BOOTLOADER_GPIO_PORT, LED_TEST_GPIO_PORT}; @@ -48,7 +80,7 @@ const uint32_t LED_GPIO_CLKS[LEDn] = {LED_ACTIVITY_GPIO_CLK, LED_BOOTLOADER_GPIO * @arg LED_ERR * @retval None */ -void LEDInit(Led_TypeDef Led) +static void LEDInit(Led_TypeDef Led) { GPIO_InitTypeDef GPIO_InitStructure; @@ -73,7 +105,7 @@ void LEDInit(Led_TypeDef Led) * @arg LED_ERR * @retval None */ -void LEDOn(Led_TypeDef Led) +static void LEDOn(Led_TypeDef Led) { GPIO_ResetBits(LED_GPIO_PORTS[Led],LED_GPIO_PINS[Led]); } @@ -87,7 +119,7 @@ void LEDOn(Led_TypeDef Led) * @arg LED_ERR * @retval None */ -void LEDOff(Led_TypeDef Led) +static void LEDOff(Led_TypeDef Led) { GPIO_SetBits(LED_GPIO_PORTS[Led],LED_GPIO_PINS[Led]); } @@ -101,7 +133,22 @@ void LEDOff(Led_TypeDef Led) * @arg LED_ERR * @retval None */ -void LEDToggle(Led_TypeDef Led) +static void LEDToggle(Led_TypeDef Led) { GPIO_ToggleBits(LED_GPIO_PORTS[Led],LED_GPIO_PINS[Led]); } + +__EXPORT void board_led_initialize(void) +{ + LEDInit(LED_ACT); + LEDInit(LED_COM); + LEDInit(LED_ERR); +} + +__EXPORT void board_led_status_update(float quality) +{ + UNUSED(quality); + LEDToggle(LED_ACT); + LEDOff(LED_COM); + LEDOff(LED_ERR); +} diff --git a/src/drivers/boards/px4flow-v2/px4flow_init.c b/src/drivers/boards/px4flow-v2/px4flow_init.c index e9a1aeb..d7b4548 100644 --- a/src/drivers/boards/px4flow-v2/px4flow_init.c +++ b/src/drivers/boards/px4flow-v2/px4flow_init.c @@ -180,131 +180,3 @@ int board_get_serialnumber(uint8_t serial[BOARD_SERIALNUMBER_SIZE]) return BOARD_SERIALNUMBER_SIZE; } -/**************************************************************************** - * Name: board_led_initialize - * - * Description: - * This functions is called very early in initialization to perform board- - * specific initialization of LED-related resources. This includes such - * things as, for example, configure GPIO pins to drive the LEDs and also - * putting the LEDs in their correct initial state. - * - * NOTE: In most architectures, board_led_initialize() is called from - * board-specific initialization logic. But there are a few architectures - * where this initialization function is still called from common chip - * architecture logic. This interface is not, however, a common board - * interface in any event and, hence, the usage of the name - * board_led_initialize is deprecated. - * - * WARNING: This interface name will eventually be removed; do not use it - * in new board ports. New implementations should use the naming - * conventions for "Microprocessor-Specific Interfaces" or the "Board- - * Specific Interfaces" as described above. - * - * Input Parameters: - * None - * - * Returned Value: - * None - * - ****************************************************************************/ - -__EXPORT void board_led_initialize(void) -{ - -} - -/**************************************************************************** - * Name: board_led_on - * - * Description: - * Set the LED configuration into the ON condition for the state provided - * by the led parameter. This may be one of: - * - * LED_STARTED NuttX has been started - * LED_HEAPALLOCATE Heap has been allocated - * LED_IRQSENABLED Interrupts enabled - * LED_STACKCREATED Idle stack created - * LED_INIRQ In an interrupt - * LED_SIGNAL In a signal handler - * LED_ASSERTION An assertion failed - * LED_PANIC The system has crashed - * LED_IDLE MCU is in sleep mode - * - * Where these values are defined in a board-specific way in the standard - * board.h header file exported by every architecture. - * - * Input Parameters: - * led - Identifies the LED state to put in the ON state (which may or may - * not equate to turning an LED on) - * - * Returned Value: - * None - * - ****************************************************************************/ - -__EXPORT void board_led_on(int led) -{ - -} - -/**************************************************************************** - * Name: board_led_off - * - * Description: - * Set the LED configuration into the OFF condition for the state provided - * by the led parameter. This may be one of: - * - * LED_INIRQ Leaving an interrupt - * LED_SIGNAL Leaving a signal handler - * LED_ASSERTION Recovering from an assertion failure - * LED_PANIC The system has crashed (blinking). - * LED_IDLE MCU is not in sleep mode - * - * Where these values are defined in a board-specific way in the standard - * board.h header file exported by every architecture. - * - * Input Parameters: - * led - Identifies the LED state to put in the OFF state (which may or may - * not equate to turning an LED off) - * - * Returned Value: - * None - * - ****************************************************************************/ - -__EXPORT void board_led_off(int led) -{ - -} - -/**************************************************************************** - * Name: board_crashdump - * - * Description: - * If CONFIG_BOARD_CRASHDUMP is selected then up_asseert will call out to - * board_crashdump prior to calling exit in the case of an assertion failure. - * Or in the case of a hardfault looping indefinitely. board_crashdump then - * has a chance to save the state of the machine. The provided - * board_crashdump should save as much information as it can about the cause - * of the fault and then most likely reset the system. - * - * N.B. There are limited system resources that can be used by the provided - * board_crashdump function. The tems from the fact that most critical/fatal - * crashes are because of a hard fault or during interrupt processing. - * In these cases, up_assert is running from the context of an interrupt - * handlerand it is impossible to use any device driver in this context. - * - * Also consider the following: Who knows what state the system is in? Is - * memory trashed? Is the Heap intact? Therefore all we can expect to do in - * board_crashdump is save the "machine state" in a place where on the next - * reset we can write it to more sophisticated storage in a sane operating - * environment. - * - ****************************************************************************/ - -__EXPORT void board_crashdump(uint32_t currentsp, void *tcb, uint8_t *filename, - int lineno) -{ - -} diff --git a/src/drivers/boards/px4flow-v2/px4flow_led.c b/src/drivers/boards/px4flow-v2/px4flow_led.c index 269165a..872a57b 100644 --- a/src/drivers/boards/px4flow-v2/px4flow_led.c +++ b/src/drivers/boards/px4flow-v2/px4flow_led.c @@ -43,12 +43,45 @@ #include "stm32f4xx_tim.h" -#define TMR_FREQUENCY STM32_TIMCLK1 +const long fosc = STM32_TIMCLK1; +const long prescale = 128; + /* * PE[09] PE9/TIM1_CH1/FMC_D6 40 TIM1_CH1 R Miswired to B * PE[11] PE11/TIM1_CH2/8PI4_NSS/FMC_D8/LCD_G3 42 TIM1_CH2 G * PE[13] PE13/TIM1_CH3/SPI4_MISO/FMC_D10/LCD_DE 44 TIM1_CH2 B Miswired to R */ + + __EXPORT void board_led_initialize(void) { + RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE); + GPIO_InitTypeDef GPIO_InitStructure; + + GPIO_InitStructure.GPIO_Pin = (GPIO_Pin_9 | GPIO_Pin_11 | GPIO_Pin_13); + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + GPIO_Init(GPIOE, &GPIO_InitStructure); + + // Connect TIM1 pins to AF + GPIO_PinAFConfig(GPIOE, GPIO_PinSource9, GPIO_AF_TIM1); + GPIO_PinAFConfig(GPIOE, GPIO_PinSource11, GPIO_AF_TIM1); + GPIO_PinAFConfig(GPIOE, GPIO_PinSource13, GPIO_AF_TIM1); + + TIM1->EGR |= TIM_PSCReloadMode_Immediate; + TIM1->PSC = prescale; + TIM1->CR1 = TIM_CR1_ARPE | TIM_CR1_CEN; + + TIM1->CCMR1 = (TIM_OCMode_PWM1|TIM_CCMR1_OC1PE) | (TIM_OCMode_PWM1<<8|TIM_CCMR1_OC2PE); + TIM1->CCMR2 = (TIM_OCMode_PWM1|TIM_CCMR1_OC2PE); + + TIM1->CCER |= (TIM_CCER_CC1P|TIM_CCER_CC1E|TIM_CCER_CC2P|TIM_CCER_CC2E|TIM_CCER_CC3P|TIM_CCER_CC3E); + + TIM_CCPreloadControl(TIM1, ENABLE); + TIM_Cmd(TIM1, ENABLE); + TIM_CtrlPWMOutputs(TIM1, ENABLE); +} /**************************************************************************** * Name: board_led_rgb @@ -60,66 +93,33 @@ * red - intensity of the red led * green - intensity of the green led * blue - intensity of the blue led - * hz - number of flashes per second * * Returned Value: * None * ****************************************************************************/ - - -void board_led_rgb(uint16_t red, uint16_t green , uint16_t blue, - uint16_t hz) +static void board_led_rgb(uint16_t red, uint16_t green , uint16_t blue, uint16_t hz) { + const long p1s = fosc / prescale; + const long p0p5s = p1s / 2; - long fosc = TMR_FREQUENCY; - long prescale = 2048; - long p1s = fosc / prescale; - long p0p5s = p1s / 2; - static bool once = 0; - - if (!once) { - once = 1; - - - RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE); - GPIO_InitTypeDef GPIO_InitStructure; - - GPIO_InitStructure.GPIO_Pin = (GPIO_Pin_9 | GPIO_Pin_11 | GPIO_Pin_13); - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; - GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; - GPIO_Init(GPIOE, &GPIO_InitStructure); + long p2 = hz == 0 ? p1s : p1s / hz; + TIM1->ARR = p2; + p2 = hz == 0 ? p1s + 1 : p0p5s / hz; + TIM1->CCR3 = (red * p2) / 255; + TIM1->CCR2 = (green * p2) / 255; + TIM1->CCR1 = (blue * p2) / 255; - // Connect TIM1 pins to AF - GPIO_PinAFConfig(GPIOE, GPIO_PinSource9, GPIO_AF_TIM1); - GPIO_PinAFConfig(GPIOE, GPIO_PinSource11, GPIO_AF_TIM1); - GPIO_PinAFConfig(GPIOE, GPIO_PinSource13, GPIO_AF_TIM1); - - TIM1->EGR |= TIM_PSCReloadMode_Immediate; - TIM1->PSC = prescale; - TIM1->CR1 = TIM_CR1_ARPE | TIM_CR1_CEN; - - TIM1->CCMR1 = (TIM_OCMode_PWM1|TIM_CCMR1_OC1PE) | (TIM_OCMode_PWM1<<8|TIM_CCMR1_OC2PE); - TIM1->CCMR2 = (TIM_OCMode_PWM1|TIM_CCMR1_OC2PE); - - TIM1->CCER |= (TIM_CCER_CC1P|TIM_CCER_CC1E|TIM_CCER_CC2P|TIM_CCER_CC2E|TIM_CCER_CC3P|TIM_CCER_CC3E); - - TIM_CCPreloadControl(TIM1, ENABLE); - TIM_Cmd(TIM1, ENABLE); - TIM_CtrlPWMOutputs(TIM1, ENABLE); - } - - long p2 = hz == 0 ? p1s : p1s / hz; - TIM1->ARR = p2; - p2 = hz == 0 ? p1s + 1 : p0p5s / hz; - TIM1->CCR3 = (red * p2) / 255; - TIM1->CCR2 = (green * p2) / 255; - TIM1->CCR1 = (blue * p2) / 255; - - TIM_Cmd(TIM1, hz == 0 ? DISABLE : ENABLE); + TIM_Cmd(TIM1, hz == 0 ? DISABLE : ENABLE); +} +__EXPORT void board_led_status_update(float quality) { + float alpha = 0.3; + static float filter = 0; + filter = (1-alpha)*filter + alpha*quality; + uint8_t r = 255 * (1-filter); + uint8_t g = 255 * filter; + uint8_t b = 0; + board_led_rgb(r, g, b, 10000); } diff --git a/src/include/bsp/board.h b/src/include/bsp/board.h index 89a4bd9..2c5c410 100644 --- a/src/include/bsp/board.h +++ b/src/include/bsp/board.h @@ -210,114 +210,8 @@ int board_get_serialnumber(uint8_t serial[BOARD_SERIALNUMBER_SIZE]); void board_led_initialize(void); -/**************************************************************************** - * Name: board_led_on - * - * Description: - * Set the LED configuration into the ON condition for the state provided - * by the led parameter. This may be one of: - * - * LED_STARTED NuttX has been started - * LED_HEAPALLOCATE Heap has been allocated - * LED_IRQSENABLED Interrupts enabled - * LED_STACKCREATED Idle stack created - * LED_INIRQ In an interrupt - * LED_SIGNAL In a signal handler - * LED_ASSERTION An assertion failed - * LED_PANIC The system has crashed - * LED_IDLE MCU is in sleep mode - * - * Where these values are defined in a board-specific way in the standard - * board.h header file exported by every architecture. - * - * Input Parameters: - * led - Identifies the LED state to put in the ON state (which may or may - * not equate to turning an LED on) - * - * Returned Value: - * None - * - ****************************************************************************/ - -void board_led_on(int led); - -/**************************************************************************** - * Name: board_led_off - * - * Description: - * Set the LED configuration into the OFF condition for the state provided - * by the led parameter. This may be one of: - * - * LED_INIRQ Leaving an interrupt - * LED_SIGNAL Leaving a signal handler - * LED_ASSERTION Recovering from an assertion failure - * LED_PANIC The system has crashed (blinking). - * LED_IDLE MCU is not in sleep mode - * - * Where these values are defined in a board-specific way in the standard - * board.h header file exported by every architecture. - * - * Input Parameters: - * led - Identifies the LED state to put in the OFF state (which may or may - * not equate to turning an LED off) - * - * Returned Value: - * None - * - ****************************************************************************/ - -void board_led_off(int led); - -/**************************************************************************** - * Name: board_led_rgb - * - * Description: - * - * Input Parameters: - * - * red - intensity of the red led - * green - intensity of the green led - * blue - intensity of the blue led - * hz - number of flashes per second - * - * Returned Value: - * None - * - ****************************************************************************/ -#if defined(CONFIG_ARCH_BOARD_PX4FLOW_V1) -#define board_led_rgb(red, green , blue, hz) -#endif -#if defined(CONFIG_ARCH_BOARD_PX4FLOW_V2) -weak_function void board_led_rgb(uint16_t red, uint16_t green , uint16_t blue, - uint16_t hz); -#endif - -/**************************************************************************** - * Name: board_crashdump - * - * Description: - * If CONFIG_BOARD_CRASHDUMP is selected then up_asseert will call out to - * board_crashdump prior to calling exit in the case of an assertion failure. - * Or in the case of a hardfault looping indefinitely. board_crashdump then - * has a chance to save the state of the machine. The provided - * board_crashdump should save as much information as it can about the cause - * of the fault and then most likely reset the system. - * - * N.B. There are limited system resources that can be used by the provided - * board_crashdump function. The tems from the fact that most critical/fatal - * crashes are because of a hard fault or during interrupt processing. - * In these cases, up_assert is running from the context of an interrupt - * handlerand it is impossible to use any device driver in this context. - * - * Also consider the following: Who knows what state the system is in? Is - * memory trashed? Is the Heap intact? Therefore all we can expect to do in - * board_crashdump is save the "machine state" in a place where on the next - * reset we can write it to more sophisticated storage in a sane operating - * environment. - * - ****************************************************************************/ +/// Update the LED with the current status +void board_led_status_update(float quality); -void board_crashdump(uint32_t currentsp, void *tcb, uint8_t *filename, - int lineno); __END_DECLS #endif /* __INCLUDE_NUTTX_BOARD_H */ diff --git a/src/include/led.h b/src/include/led.h deleted file mode 100644 index e445cb1..0000000 --- a/src/include/led.h +++ /dev/null @@ -1,69 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#ifndef LED_H_ -#define LED_H_ - -#include "stm32f4xx_conf.h" -#include "stm32f4xx.h" - -typedef enum -{ - LED_ACT = 0, // Blue - LED_COM = 1, // Amber - LED_ERR = 2, // Red -} Led_TypeDef; - - -#define LEDn 3 - -#define LED_ACTIVITY_PIN GPIO_Pin_3 -#define LED_ACTIVITY_GPIO_PORT GPIOE -#define LED_ACTIVITY_GPIO_CLK RCC_AHB1Periph_GPIOE - -#define LED_BOOTLOADER_PIN GPIO_Pin_2 -#define LED_BOOTLOADER_GPIO_PORT GPIOE -#define LED_BOOTLOADER_GPIO_CLK RCC_AHB1Periph_GPIOE - -#define LED_TEST_PIN GPIO_Pin_7 -#define LED_TEST_GPIO_PORT GPIOE -#define LED_TEST_GPIO_CLK RCC_AHB1Periph_GPIOE - - -void LEDInit(Led_TypeDef Led); -void LEDOn(Led_TypeDef Led); -void LEDOff(Led_TypeDef Led); -void LEDToggle(Led_TypeDef Led); - - -#endif /* LED_H_ */ diff --git a/src/modules/flow/camera.c b/src/modules/flow/camera.c index 1c1d7e8..7f1e99a 100644 --- a/src/modules/flow/camera.c +++ b/src/modules/flow/camera.c @@ -37,7 +37,6 @@ #include "stm32f4xx_conf.h" #include "core_cmFunc.h" -#include "led.h" #include @@ -374,7 +373,6 @@ void camera_transport_frame_done_fn(void *usr, bool probably_infront_dma) { ctx->resync_discard_frame_count = fdc; /* re-initialize the transport twice */ if (fdc == 0 || fdc == 1) { - LEDOff(LED_ERR); ctx->transport->reset(ctx->transport->usr); } } else { @@ -406,8 +404,6 @@ void camera_transport_frame_done_fn(void *usr, bool probably_infront_dma) { ctx->seq_frame_receiving = false; /* initiate resynchronization: */ ctx->resync_discard_frame_count = 2; - - LEDOn(LED_ERR); } } else { ctx->frame_done_infront_count++; diff --git a/src/modules/flow/dcmi.c b/src/modules/flow/dcmi.c index e256925..b7b17c6 100644 --- a/src/modules/flow/dcmi.c +++ b/src/modules/flow/dcmi.c @@ -54,7 +54,6 @@ //#define CONFIG_USE_PROBES #include -//#include "led.h" struct _dcmi_transport_ctx; typedef struct _dcmi_transport_ctx dcmi_transport_ctx; @@ -174,7 +173,6 @@ void DMA2_Stream1_IRQHandler(void) { if (DMA_GetITStatus(DMA2_Stream1, DMA_IT_TCIF1) != RESET) { DMA_ClearITPendingBit(DMA2_Stream1, DMA_IT_TCIF1); /* get the buffer that has been completed: */ - //LEDOn(LED_ACT); void *buffer; if (DMA_GetCurrentMemoryTarget(DMA2_Stream1) == 0) { buffer = dcmi_dma_buffer_2; @@ -189,7 +187,6 @@ void DMA2_Stream1_IRQHandler(void) { ctx->transfer_done_cb(ctx->cb_usr, buffer, CONFIG_DCMI_DMA_BUFFER_SIZE); /* store the time when the function finished. */ ctx->last_dma_irq_t = get_boot_time_us(); - //LEDOff(LED_ACT); } } diff --git a/src/modules/flow/i2c.c b/src/modules/flow/i2c.c index ba10890..644a393 100644 --- a/src/modules/flow/i2c.c +++ b/src/modules/flow/i2c.c @@ -45,8 +45,8 @@ #include "stm32f4xx_i2c.h" #include "stm32f4xx_rcc.h" #include "stm32f4xx_gpio.h" +#include "stm32f4xx_conf.h" -#include "led.h" #include "i2c_frame.h" #include "gyro.h" #include "sonar.h" diff --git a/src/modules/flow/main.c b/src/modules/flow/main.c index 7b0a78b..938cc05 100644 --- a/src/modules/flow/main.c +++ b/src/modules/flow/main.c @@ -53,7 +53,6 @@ #include #include "settings.h" #include "utils.h" -#include "led.h" #include "filter.h" #include "result_accumulator.h" #include "flow.h" @@ -169,11 +168,6 @@ static void send_video_fn(void) { if (FLOAT_AS_BOOL(global_data.param[PARAM_USB_SEND_VIDEO]) && !FLOAT_AS_BOOL(global_data.param[PARAM_VIDEO_ONLY])) { mavlink_send_image(previous_image); - LEDToggle(LED_COM); - } - else if (!FLOAT_AS_BOOL(global_data.param[PARAM_USB_SEND_VIDEO])) - { - LEDOff(LED_COM); } } @@ -225,7 +219,6 @@ static void send_image_step(void) { } static void start_send_image(void) { - LEDOn(LED_COM); usb_image_transfer_active = true; usb_image_pos = 0; usb_packet.flags = 0; @@ -241,7 +234,6 @@ void send_image_completed(void) { send_image_step(); } else { usb_image_transfer_active = false; - LEDOff(LED_COM); } } @@ -271,22 +263,9 @@ int main(void) global_data_reset_param_defaults(); global_data_reset(); PROBE_INIT(); - /* init led */ - LEDInit(LED_ACT); - LEDInit(LED_COM); - LEDInit(LED_ERR); - LEDOff(LED_ACT); - LEDOff(LED_COM); - LEDOff(LED_ERR); - board_led_rgb(255,255,255, 1); - board_led_rgb( 0, 0,255, 0); - board_led_rgb( 0, 0, 0, 0); - board_led_rgb(255, 0, 0, 1); - board_led_rgb(255, 0, 0, 2); - board_led_rgb(255, 0, 0, 3); - board_led_rgb( 0,255, 0, 3); - board_led_rgb( 0, 0,255, 4); - + + board_led_initialize(); + /* enable FPU on Cortex-M4F core */ SCB_CPACR |= ((3UL << 10 * 2) | (3UL << 11 * 2)); /* set CP10 Full Access and set CP11 Full Access */ @@ -375,7 +354,6 @@ int main(void) snap_ready = true; if (snap_capture_success) { /* send the snapshot! */ - LEDToggle(LED_COM); start_send_image(); } } @@ -583,6 +561,8 @@ int main(void) int min_valid_ratio = global_data.param[PARAM_ALGORITHM_MIN_VALID_RATIO]; result_accumulator_calculate_output_flow(&mavlink_accumulator, min_valid_ratio, &output_flow); result_accumulator_calculate_output_flow_rad(&mavlink_accumulator, min_valid_ratio, &output_flow_rad); + + board_led_status_update(output_flow.quality / 255.0); // send flow mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], diff --git a/src/modules/flow/module.mk b/src/modules/flow/module.mk index 13d057b..b1e47e2 100644 --- a/src/modules/flow/module.mk +++ b/src/modules/flow/module.mk @@ -23,7 +23,6 @@ SRCS = $(ST_LIB)startup_stm32f4xx.s \ SRCS += main.c \ utils.c \ - led.c \ settings.c \ communication.c \ flow.c \ diff --git a/src/modules/flow/timer.c b/src/modules/flow/timer.c index 383d680..fa64fdb 100644 --- a/src/modules/flow/timer.c +++ b/src/modules/flow/timer.c @@ -41,7 +41,6 @@ #include "stm32f4xx.h" #include "timer.h" -#include "led.h" #define __INLINE inline #include "core_cmInstr.h" @@ -120,7 +119,6 @@ void timer_register(void (*timer_fn)(void), uint32_t period_ms) { } if (idx >= NTIMERS) { /* capture error */ - LEDOn(LED_ERR); while (1); } /* setup the info struct: */ @@ -163,7 +161,6 @@ void timer_init(void) if (SysTick_Config(ticks_per_ms)) /* set timer to trigger interrupt every 1 millisecond */ { /* capture clock error */ - LEDOn(LED_ERR); while (1); } } diff --git a/src/modules/uavcannode/uavcannode_main.cpp b/src/modules/uavcannode/uavcannode_main.cpp index 2e8b73d..6e313b2 100644 --- a/src/modules/uavcannode/uavcannode_main.cpp +++ b/src/modules/uavcannode/uavcannode_main.cpp @@ -220,7 +220,6 @@ void UavcanNode::cb_beginfirmware_update(const uavcan::ReceivedDataStructure Date: Thu, 27 Aug 2015 16:39:33 -0700 Subject: [PATCH 072/120] Move i2c struct filling into result_accumulator --- src/include/result_accumulator.h | 2 ++ src/modules/flow/i2c.c | 41 ++----------------------- src/modules/flow/result_accumulator.c | 44 +++++++++++++++++++++++++++ 3 files changed, 48 insertions(+), 39 deletions(-) diff --git a/src/include/result_accumulator.h b/src/include/result_accumulator.h index 9d86283..7012200 100644 --- a/src/include/result_accumulator.h +++ b/src/include/result_accumulator.h @@ -36,6 +36,7 @@ #include #include +#include "i2c_frame.h" typedef struct _result_accumulator_ctx { @@ -144,6 +145,7 @@ void result_accumulator_feed(result_accumulator_ctx *ctx, float dt, float x_rate void result_accumulator_calculate_output_flow(result_accumulator_ctx *ctx, uint16_t min_valid_data_count_percent, result_accumulator_output_flow *out); void result_accumulator_calculate_output_flow_rad(result_accumulator_ctx *ctx, uint16_t min_valid_data_count_percent, result_accumulator_output_flow_rad *out); void result_accumulator_calculate_output_flow_i2c(result_accumulator_ctx *ctx, uint16_t min_valid_data_count_percent, result_accumulator_output_flow_i2c *out); +void result_accumulator_fill_i2c_data(result_accumulator_ctx *ctx, i2c_frame* f, i2c_integral_frame* f_integral); /** Resets the accumulator to prepare it for the next accumulation phase. */ diff --git a/src/modules/flow/i2c.c b/src/modules/flow/i2c.c index ba10890..237072c 100644 --- a/src/modules/flow/i2c.c +++ b/src/modules/flow/i2c.c @@ -247,47 +247,10 @@ void update_TX_buffer(float dt, float x_rate, float y_rate, float z_rate, int16_ qual, pixel_flow_x, pixel_flow_y, rad_per_pixel, distance_valid, ground_distance, distance_age); - result_accumulator_output_flow output_flow; - result_accumulator_output_flow_i2c output_i2c; - int min_valid_ratio = global_data.param[PARAM_ALGORITHM_MIN_VALID_RATIO]; - result_accumulator_calculate_output_flow(&accumulator, min_valid_ratio, &output_flow); - result_accumulator_calculate_output_flow_i2c(&accumulator, min_valid_ratio, &output_i2c); - i2c_frame f; i2c_integral_frame f_integral; - - /* write the i2c_frame */ - f.frame_count = accumulator.last.frame_count; - f.pixel_flow_x_sum = output_flow.flow_x; - f.pixel_flow_y_sum = output_flow.flow_y; - f.flow_comp_m_x = floor(output_flow.flow_comp_m_x * 1000.0f + 0.5f); - f.flow_comp_m_y = floor(output_flow.flow_comp_m_y * 1000.0f + 0.5f); - f.qual = output_flow.quality; - f.ground_distance = output_i2c.ground_distance; - - f.gyro_x_rate = output_i2c.gyro_x; - f.gyro_y_rate = output_i2c.gyro_y; - f.gyro_z_rate = output_i2c.gyro_z; - f.gyro_range = getGyroRange(); - - if (output_i2c.time_delta_distance_us < 255 * 1000) { - f.sonar_timestamp = output_i2c.time_delta_distance_us / 1000; //convert to ms - } else { - f.sonar_timestamp = 255; - } - - /* write the i2c_integral_frame */ - f_integral.frame_count_since_last_readout = output_i2c.valid_frames; - f_integral.gyro_x_rate_integral = output_i2c.integrated_gyro_x; //mrad*10 - f_integral.gyro_y_rate_integral = output_i2c.integrated_gyro_y; //mrad*10 - f_integral.gyro_z_rate_integral = output_i2c.integrated_gyro_z; //mrad*10 - f_integral.pixel_flow_x_integral = output_i2c.rad_flow_x; //mrad*10 - f_integral.pixel_flow_y_integral = output_i2c.rad_flow_y; //mrad*10 - f_integral.integration_timespan = output_i2c.integration_time; //microseconds - f_integral.ground_distance = output_i2c.ground_distance; //mmeters - f_integral.sonar_timestamp = output_i2c.time_delta_distance_us; //microseconds - f_integral.qual = output_i2c.quality; //0-255 linear quality measurement 0=bad, 255=best - f_integral.gyro_temperature = output_i2c.temperature; //Temperature * 100 in centi-degrees Celsius + + result_accumulator_fill_i2c_data(&accumulator, &f, &f_integral); /* perform buffer swapping */ notpublishedIndexFrame1 = 1 - publishedIndexFrame1; // choose not the current published 1 buffer diff --git a/src/modules/flow/result_accumulator.c b/src/modules/flow/result_accumulator.c index 36c5d6b..11397ba 100644 --- a/src/modules/flow/result_accumulator.c +++ b/src/modules/flow/result_accumulator.c @@ -39,6 +39,8 @@ #include #include "result_accumulator.h" +#include "i2c_frame.h" +#include "settings.h" void result_accumulator_init(result_accumulator_ctx *ctx) { @@ -205,6 +207,48 @@ void result_accumulator_calculate_output_flow_i2c(result_accumulator_ctx *ctx, u } } +// TODO: why doesn't the above function write these structs directly? +void result_accumulator_fill_i2c_data(result_accumulator_ctx *ctx, i2c_frame* f, i2c_integral_frame* f_integral) { + result_accumulator_output_flow output_flow; + result_accumulator_output_flow_i2c output_i2c; + int min_valid_ratio = global_data.param[PARAM_ALGORITHM_MIN_VALID_RATIO]; + result_accumulator_calculate_output_flow(ctx, min_valid_ratio, &output_flow); + result_accumulator_calculate_output_flow_i2c(ctx, min_valid_ratio, &output_i2c); + + /* write the i2c_frame */ + f->frame_count = ctx->last.frame_count; + f->pixel_flow_x_sum = output_flow.flow_x; + f->pixel_flow_y_sum = output_flow.flow_y; + f->flow_comp_m_x = floor(output_flow.flow_comp_m_x * 1000.0f + 0.5f); + f->flow_comp_m_y = floor(output_flow.flow_comp_m_y * 1000.0f + 0.5f); + f->qual = output_flow.quality; + f->ground_distance = output_i2c.ground_distance; + + f->gyro_x_rate = output_i2c.gyro_x; + f->gyro_y_rate = output_i2c.gyro_y; + f->gyro_z_rate = output_i2c.gyro_z; + f->gyro_range = getGyroRange(); + + if (output_i2c.time_delta_distance_us < 255 * 1000) { + f->sonar_timestamp = output_i2c.time_delta_distance_us / 1000; //convert to ms + } else { + f->sonar_timestamp = 255; + } + + /* write the i2c_integral_frame */ + f_integral->frame_count_since_last_readout = output_i2c.valid_frames; + f_integral->gyro_x_rate_integral = output_i2c.integrated_gyro_x; //mrad*10 + f_integral->gyro_y_rate_integral = output_i2c.integrated_gyro_y; //mrad*10 + f_integral->gyro_z_rate_integral = output_i2c.integrated_gyro_z; //mrad*10 + f_integral->pixel_flow_x_integral = output_i2c.rad_flow_x; //mrad*10 + f_integral->pixel_flow_y_integral = output_i2c.rad_flow_y; //mrad*10 + f_integral->integration_timespan = output_i2c.integration_time; //microseconds + f_integral->ground_distance = output_i2c.ground_distance; //mmeters + f_integral->sonar_timestamp = output_i2c.time_delta_distance_us; //microseconds + f_integral->qual = output_i2c.quality; //0-255 linear quality measurement 0=bad, 255=best + f_integral->gyro_temperature = output_i2c.temperature; //Temperature * 100 in centi-degrees Celsius +} + void result_accumulator_reset(result_accumulator_ctx *ctx) { ctx->px_flow_x_accu = 0; From be74b0deac5d7243aca6edb07c7d505fc373337b Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Thu, 27 Aug 2015 16:50:02 -0700 Subject: [PATCH 073/120] Split i2c_slave (V1) and uavcan (V2) into separate modules --- .../baremetal/config_px4flow-v1_default.mk | 2 +- src/include/{legacy_i2c.h => fmu_comm.h} | 26 +++++--- src/include/i2c.h | 58 ------------------ src/modules/flow/camera.c | 7 +-- src/modules/flow/main.c | 28 ++------- src/modules/flow/module.mk | 1 - .../{flow/i2c.c => i2c_slave/i2c_slave.c} | 59 ++++++++----------- src/modules/i2c_slave/module.mk | 5 ++ src/modules/uavcannode/module.mk | 1 + src/modules/uavcannode/uavcannode.c | 41 +++++++++++++ 10 files changed, 99 insertions(+), 129 deletions(-) rename src/include/{legacy_i2c.h => fmu_comm.h} (72%) delete mode 100644 src/include/i2c.h rename src/modules/{flow/i2c.c => i2c_slave/i2c_slave.c} (91%) create mode 100644 src/modules/i2c_slave/module.mk create mode 100644 src/modules/uavcannode/uavcannode.c diff --git a/makefiles/baremetal/config_px4flow-v1_default.mk b/makefiles/baremetal/config_px4flow-v1_default.mk index f2e0bbc..0992b33 100644 --- a/makefiles/baremetal/config_px4flow-v1_default.mk +++ b/makefiles/baremetal/config_px4flow-v1_default.mk @@ -30,7 +30,7 @@ MODULES += drivers/boards/px4flow-v1 # # General system control # -##TODO EXPECT I2C - MODULES += modules/uavcannode +MODULES += modules/i2c_slave # C support MODULES += modules/libc diff --git a/src/include/legacy_i2c.h b/src/include/fmu_comm.h similarity index 72% rename from src/include/legacy_i2c.h rename to src/include/fmu_comm.h index d551912..22fffcc 100644 --- a/src/include/legacy_i2c.h +++ b/src/include/fmu_comm.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -30,13 +30,23 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ +#ifndef FMU_COMM_H +#define FMU_COMM_H -#pragma once +/// Abstraction over the FMU communications interface -- I2C slave or UAVCAN -#include -#include -#include +#include + +/// Call on boot to initialize +void fmu_comm_init(void); + +/// Call in the main loop periodically +void fmu_comm_run(void); + +/// Call every frame to update with new data +void fmu_comm_update(float 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); + +#endif -typedef struct legacy_12c_data_t { - uint64_t time_stamp_utc; -} legacy_12c_data_t; diff --git a/src/include/i2c.h b/src/include/i2c.h deleted file mode 100644 index 227be14..0000000 --- a/src/include/i2c.h +++ /dev/null @@ -1,58 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file i2c.h - * I2C communication functions. - * @author Thomas Boehm - */ - - -#ifndef I2C_H_ -#define I2C_H_ -#include -#include -#include - -#define I2C1_OWNADDRESS_1_BASE 0x42 //7bit base address -/** - * @brief Configures I2C1 for communication as a slave (default behaviour for STM32F) - */ - -void i2c_init(void); -void update_TX_buffer(float 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, legacy_12c_data_t *pd); -char i2c_get_ownaddress1(void); -#endif /* I2C_H_ */ - diff --git a/src/modules/flow/camera.c b/src/modules/flow/camera.c index 1c1d7e8..c125fd1 100644 --- a/src/modules/flow/camera.c +++ b/src/modules/flow/camera.c @@ -34,6 +34,7 @@ #include "camera.h" #include "timer.h" +#include "fmu_comm.h" #include "stm32f4xx_conf.h" #include "core_cmFunc.h" @@ -41,8 +42,6 @@ #include -#include - #define abs(x) ({ \ typeof(x) __x = (x); \ if (__x < 0) __x = -__x;\ @@ -542,8 +541,8 @@ int camera_img_stream_get_buffers(camera_ctx *ctx, camera_image_buffer *buffers[ if (wait_for_new) { /* wait until a new frame is available: */ while(!ctx->new_frame_arrived) { - //TODO: camera_img_stream_get_buffers is calling uavcan_run() - uavcan_run(); + //TODO: camera_img_stream_get_buffers is calling fmu_comm_run() + fmu_comm_run(); } } if (camera_img_stream_get_buffers_idx(ctx, bidx, count, wait_for_new)) { diff --git a/src/modules/flow/main.c b/src/modules/flow/main.c index 7b0a78b..c4d47a3 100644 --- a/src/modules/flow/main.c +++ b/src/modules/flow/main.c @@ -61,7 +61,7 @@ #include "dcmi.h" #include "mt9v034.h" #include "gyro.h" -#include "i2c.h" +#include "fmu_comm.h" #include "usart.h" #include "sonar.h" #include "communication.h" @@ -326,8 +326,7 @@ int main(void) /* usart config*/ usart_init(); - /* i2c config*/ - i2c_init(); + fmu_comm_init(); /* sonar config*/ float sonar_distance_filtered = 0.0f; // distance in meter @@ -357,17 +356,13 @@ int main(void) uint32_t last_frame_index = 0; - uavcan_start(); - /* main loop */ while (1) { /* check timers */ timer_check(); - PROBE_1(false); - uavcan_run(); - PROBE_1(true); + fmu_comm_run(); if (snap_capture_done) { snap_capture_done = false; @@ -528,25 +523,12 @@ int main(void) { ground_distance = sonar_distance_raw; } - - uavcan_define_export(i2c_data, legacy_12c_data_t, ccm); - uavcan_define_export(range_data, range_data_t, ccm); - uavcan_timestamp_export(i2c_data); - uavcan_assign(range_data.time_stamp_utc, i2c_data.time_stamp_utc); /* update I2C transmit buffer */ - update_TX_buffer(frame_dt, + fmu_comm_update(frame_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()), uavcan_use_export(i2c_data)); - - PROBE_2(false); - uavcan_publish(range, 40, range_data); - PROBE_2(true); - - PROBE_3(false); - uavcan_publish(flow, 40, i2c_data); - PROBE_3(true); + distance_valid, ground_distance, get_time_delta_us(get_sonar_measure_time())); /* accumulate the results */ result_accumulator_feed(&mavlink_accumulator, frame_dt, diff --git a/src/modules/flow/module.mk b/src/modules/flow/module.mk index 13d057b..7462cba 100644 --- a/src/modules/flow/module.mk +++ b/src/modules/flow/module.mk @@ -37,7 +37,6 @@ SRCS += main.c \ usbd_cdc_vcp.c \ usbd_desc.c \ usbd_usr.c \ - i2c.c \ reset.c \ sonar_mode_filter.c \ camera.c \ diff --git a/src/modules/flow/i2c.c b/src/modules/i2c_slave/i2c_slave.c similarity index 91% rename from src/modules/flow/i2c.c rename to src/modules/i2c_slave/i2c_slave.c index 237072c..b68b07a 100644 --- a/src/modules/flow/i2c.c +++ b/src/modules/i2c_slave/i2c_slave.c @@ -30,17 +30,13 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ - -/** - * @file i2c.c - * Definition of i2c frames. - * @author Thomas Boehm - * @author James Goppert - */ + +#include +#include +#include "fmu_comm.h" #include "px4_config.h" #include "px4_macros.h" -#include "i2c.h" #include "stm32f4xx.h" #include "stm32f4xx_i2c.h" #include "stm32f4xx_rcc.h" @@ -53,13 +49,11 @@ #include "main.h" #include "result_accumulator.h" -#include "mavlink_bridge_header.h" -#include +#define I2C1_OWNADDRESS_1_BASE 0x42 //7bit base address /* prototypes */ void I2C1_EV_IRQHandler(void); void I2C1_ER_IRQHandler(void); -char readI2CAddressOffset(void); static char offset = 0; uint8_t dataRX = 0; @@ -73,8 +67,21 @@ uint8_t readout_done_frame1 = 1; uint8_t readout_done_frame2 = 1; uint8_t stop_accumulation = 0; -void i2c_init() { +static char readI2CAddressOffset(void) { + //read 3bit address offset of 7 bit address + offset = 0x00; + offset = GPIO_ReadInputData(GPIOC ) >> 13; //bit 0 + offset = offset | ((GPIO_ReadInputData(GPIOC ) >> 14) << 1); //bit 1 + offset = offset | ((GPIO_ReadInputData(GPIOC ) >> 15) << 2); //bit 2 + offset = (~offset) & 0x07; + return offset; +} + +static char i2c_get_ownaddress1(void) { + return (I2C1_OWNADDRESS_1_BASE + readI2CAddressOffset()) << 1; //add offset to base and shift 1 bit to generate valid 7 bit address +} +__EXPORT void fmu_comm_init(void) { I2C_DeInit(I2C1 ); //Deinit and reset the I2C to avoid it locking up I2C_SoftwareResetCmd(I2C1, ENABLE); I2C_SoftwareResetCmd(I2C1, DISABLE); @@ -136,6 +143,8 @@ void i2c_init() { I2C_Cmd(I2C1, ENABLE); } +__EXPORT void fmu_comm_run(void) {} + void I2C1_EV_IRQHandler(void) { //uint8_t dataRX; @@ -225,9 +234,9 @@ void I2C1_ER_IRQHandler(void) { } -void update_TX_buffer(float dt, float x_rate, float y_rate, float z_rate, int16_t gyro_temp, +__EXPORT void fmu_comm_update(float 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, legacy_12c_data_t *pd) { + bool distance_valid, float ground_distance, uint32_t distance_age) { static result_accumulator_ctx accumulator; static bool initialized = false; @@ -249,19 +258,15 @@ void update_TX_buffer(float dt, float x_rate, float y_rate, float z_rate, int16_ i2c_frame f; i2c_integral_frame f_integral; - + result_accumulator_fill_i2c_data(&accumulator, &f, &f_integral); /* perform buffer swapping */ notpublishedIndexFrame1 = 1 - publishedIndexFrame1; // choose not the current published 1 buffer notpublishedIndexFrame2 = 1 - publishedIndexFrame2; // choose not the current published 2 buffer - // HACK!! To get the data -TODO(TODO:Tom Please fix this); - uavcan_export(&pd->frame, &f, I2C_FRAME_SIZE); - uavcan_export(&pd->integral_frame, &f_integral, I2C_INTEGRAL_FRAME_SIZE); - // fill I2C transmitbuffer1 with frame1 values + // fill I2C transmitbuffer1 with frame1 values memcpy(&(txDataFrame1[notpublishedIndexFrame1]), &f, I2C_FRAME_SIZE); @@ -279,17 +284,3 @@ TODO(TODO:Tom Please fix this); publishedIndexFrame2 = 1 - publishedIndexFrame2; } } - -char readI2CAddressOffset(void) { - //read 3bit address offset of 7 bit address - offset = 0x00; - offset = GPIO_ReadInputData(GPIOC ) >> 13; //bit 0 - offset = offset | ((GPIO_ReadInputData(GPIOC ) >> 14) << 1); //bit 1 - offset = offset | ((GPIO_ReadInputData(GPIOC ) >> 15) << 2); //bit 2 - offset = (~offset) & 0x07; - return offset; -} - -char i2c_get_ownaddress1(void) { - return (I2C1_OWNADDRESS_1_BASE + readI2CAddressOffset()) << 1; //add offset to base and shift 1 bit to generate valid 7 bit address -} diff --git a/src/modules/i2c_slave/module.mk b/src/modules/i2c_slave/module.mk new file mode 100644 index 0000000..4753321 --- /dev/null +++ b/src/modules/i2c_slave/module.mk @@ -0,0 +1,5 @@ +DEFAULT_VISIBILITY=default + +SRCS += i2c_slave.c + +$(info SRCS=$(SRCS)) diff --git a/src/modules/uavcannode/module.mk b/src/modules/uavcannode/module.mk index cbb9558..7c9a3b8 100644 --- a/src/modules/uavcannode/module.mk +++ b/src/modules/uavcannode/module.mk @@ -50,6 +50,7 @@ MODULE_STACKSIZE = 1024 # Main SRCS += uavcannode_main.cpp \ + uavcannode.c \ # diff --git a/src/modules/uavcannode/uavcannode.c b/src/modules/uavcannode/uavcannode.c new file mode 100644 index 0000000..8592734 --- /dev/null +++ b/src/modules/uavcannode/uavcannode.c @@ -0,0 +1,41 @@ +#include +#include +#include +#include +#include "i2c_frame.h" +#include "hrt.h" +#include "fmu_comm.h" +#include "result_accumulator.h" +#include "uavcan_if.h" + +static result_accumulator_ctx accumulator; +static legacy_12c_data_t i2c_data; +static range_data_t range_data; + +__EXPORT void fmu_comm_init() { + uavcannode_main(true); + result_accumulator_init(&accumulator); +} + +__EXPORT void fmu_comm_run() { + uavcannode_run(); +} + +__EXPORT void fmu_comm_update(float 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) { + range_data.time_stamp_utc = i2c_data.time_stamp_utc = hrt_absolute_time(); + + /* feed the accumulator and recalculate */ + result_accumulator_feed(&accumulator, 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); + + if (accumulator.frame_count >= 40) { + result_accumulator_fill_i2c_data(&accumulator, &i2c_data.frame, &i2c_data.integral_frame); + // TODO: fill range_data + uavcannode_publish_flow(&i2c_data); + uavcannode_publish_range(&range_data); + result_accumulator_reset(&accumulator); + } +} \ No newline at end of file From be2bb496b87a9d9b16b7c02406c3b64a1a4196ea Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Mon, 31 Aug 2015 16:27:13 -0700 Subject: [PATCH 074/120] Hack: Clear extra I2C RxNE interrupts --- src/modules/lidar/lidar.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/lidar/lidar.c b/src/modules/lidar/lidar.c index 7ad742a..adce342 100644 --- a/src/modules/lidar/lidar.c +++ b/src/modules/lidar/lidar.c @@ -322,6 +322,8 @@ __EXPORT void I2C1_EV_IRQHandler(void) default: break; } + } else if (I2C1->SR2 == 0 && I2C1->SR1 == 0x40) { + I2C_ReceiveData(I2C1); } } From 15be42a52086fa26b4048273472e0504deab11c8 Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Mon, 31 Aug 2015 16:44:30 -0700 Subject: [PATCH 075/120] Send ground distance even if flow data is invalid --- src/modules/flow/result_accumulator.c | 25 ++++++++++--------------- 1 file changed, 10 insertions(+), 15 deletions(-) diff --git a/src/modules/flow/result_accumulator.c b/src/modules/flow/result_accumulator.c index 01182df..e475edc 100644 --- a/src/modules/flow/result_accumulator.c +++ b/src/modules/flow/result_accumulator.c @@ -121,8 +121,6 @@ void result_accumulator_calculate_output_flow(result_accumulator_ctx *ctx, uint1 out->flow_comp_m_y = 0; } out->quality = ctx->min_quality; - /* averaging the distance is no use */ - out->ground_distance = ctx->last.ground_distance; } else { /* not enough valid data */ out->flow_x = 0; @@ -130,8 +128,9 @@ void result_accumulator_calculate_output_flow(result_accumulator_ctx *ctx, uint1 out->flow_comp_m_x = 0; out->flow_comp_m_y = 0; out->quality = 0; - out->ground_distance = -1; } + /* averaging the distance is no use */ + out->ground_distance = ctx->last.ground_distance; } void result_accumulator_calculate_output_flow_rad(result_accumulator_ctx *ctx, uint16_t min_valid_data_count_percent, result_accumulator_output_flow_rad *out) @@ -147,8 +146,6 @@ void result_accumulator_calculate_output_flow_rad(result_accumulator_ctx *ctx, u out->quality = ctx->min_quality; /* averaging the distance and temperature is no use */ out->temperature = ctx->last.temperature; - out->time_delta_distance_us = ctx->last.distance_age; - out->ground_distance = ctx->last.ground_distance; } else { /* not enough valid data */ out->integration_time = 0; @@ -159,9 +156,9 @@ void result_accumulator_calculate_output_flow_rad(result_accumulator_ctx *ctx, u out->integrated_zgyro = 0; out->quality = 0; out->temperature = ctx->last.temperature; - out->time_delta_distance_us = 0; - out->ground_distance = -1; } + out->time_delta_distance_us = ctx->last.distance_age; + out->ground_distance = ctx->last.ground_distance; } void result_accumulator_calculate_output_flow_i2c(result_accumulator_ctx *ctx, uint16_t min_valid_data_count_percent, result_accumulator_output_flow_i2c *out) @@ -181,12 +178,6 @@ void result_accumulator_calculate_output_flow_i2c(result_accumulator_ctx *ctx, u out->gyro_z = floor(ctx->gyro_z_accu * (100.0f / ctx->valid_time) + 0.5f); out->quality = ctx->min_quality; out->temperature = ctx->last.temperature; - out->time_delta_distance_us = ctx->last.distance_age; - if (ctx->last.ground_distance >= 0) { - out->ground_distance = floor(ctx->last.ground_distance * 1000.0f + 0.5f); - } else { - out->ground_distance = 0; - } } else { /* not enough valid data */ out->frame_count = ctx->frame_count; @@ -202,7 +193,11 @@ void result_accumulator_calculate_output_flow_i2c(result_accumulator_ctx *ctx, u out->gyro_z = 0; out->quality = 0; out->temperature = ctx->last.temperature; - out->time_delta_distance_us = 0; + } + out->time_delta_distance_us = ctx->last.distance_age; + if (ctx->last.ground_distance >= 0) { + out->ground_distance = floor(ctx->last.ground_distance * 1000.0f + 0.5f); + } else { out->ground_distance = 0; } } @@ -263,7 +258,7 @@ void result_accumulator_reset(result_accumulator_ctx *ctx) ctx->gyro_x_accu = 0; ctx->gyro_y_accu = 0; ctx->gyro_z_accu = 0; - + ctx->min_quality = 255; ctx->valid_data_count = 0; From 9cf78c372c01ee423666b97ab8ebf4366e4e9679 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Tue, 1 Sep 2015 17:16:00 +0200 Subject: [PATCH 076/120] implemented checks in camera buffer management. --- src/camera.c | 32 +++++++++++++++++++++----------- 1 file changed, 21 insertions(+), 11 deletions(-) diff --git a/src/camera.c b/src/camera.c index bf5ab42..a0863f5 100644 --- a/src/camera.c +++ b/src/camera.c @@ -134,11 +134,10 @@ static void uint32_t_memcpy(uint32_t *dst, const uint32_t *src, size_t count) { } } -#pragma GCC diagnostic push -#pragma GCC diagnostic warning "-Warray-bounds" -static void camera_buffer_fifo_remove_front(camera_ctx *ctx, int *out, size_t count) { +static bool camera_buffer_fifo_remove_front(camera_ctx *ctx, int *out, size_t count) { size_t bc = ctx->buf_avail_count; size_t i; + if (count > bc) return false; // read out: for (i = 0; i < count; ++i) { *out++ = ctx->buf_avail[i]; @@ -148,18 +147,20 @@ static void camera_buffer_fifo_remove_front(camera_ctx *ctx, int *out, size_t co ctx->buf_avail[i - count] = ctx->buf_avail[i]; } ctx->buf_avail_count = bc - count; + return true; } -#pragma GCC diagnostic pop -static void camera_buffer_fifo_remove_back(camera_ctx *ctx, int *out, size_t count) { +static bool camera_buffer_fifo_remove_back(camera_ctx *ctx, int *out, size_t count) { size_t bc = ctx->buf_avail_count; size_t i; + if (count > bc) return false; // read out: for (i = bc - count; i < bc; ++i) { *out++ = ctx->buf_avail[i]; } // reduce count: ctx->buf_avail_count = bc - count; + return true; } static void camera_buffer_fifo_push_at(camera_ctx *ctx, size_t pos, const int *in, size_t count) { @@ -310,11 +311,14 @@ void camera_transport_transfer_done_fn(void *usr, const void *buffer, size_t siz ctx->cur_frame_param.p.size.y == ctx->img_stream_param.p.size.y) { if (img_data_valid) { // get least recently used buffer from the available buffers: - camera_buffer_fifo_remove_back(ctx, &ctx->cur_frame_target_buf_idx, 1); - ctx->cur_frame_target_buf = &ctx->buffers[ctx->cur_frame_target_buf_idx]; - /* if there are no more buffers in the fifo reset the flag again: */ - if (ctx->buf_avail_count == 0) { - ctx->new_frame_arrived = false; + if (camera_buffer_fifo_remove_back(ctx, &ctx->cur_frame_target_buf_idx, 1)) { + ctx->cur_frame_target_buf = &ctx->buffers[ctx->cur_frame_target_buf_idx]; + /* if there are no more buffers in the fifo reset the flag again: */ + if (ctx->buf_avail_count == 0) { + ctx->new_frame_arrived = false; + } + } else { + ctx->cur_frame_target_buf_idx = -1; } } } @@ -505,7 +509,13 @@ void camera_snapshot_acknowledge(camera_ctx *ctx) { static bool camera_img_stream_get_buffers_idx(camera_ctx *ctx, int bidx[], size_t count, bool reset_new_frm) { int i; __disable_irq(); - camera_buffer_fifo_remove_front(ctx, bidx, count); + if (!camera_buffer_fifo_remove_front(ctx, bidx, count)) { + if (reset_new_frm) { + ctx->new_frame_arrived = false; + } + __enable_irq(); + return false; + } /* check that the buffers are in consecutive order: */ bool consecutive = true; uint32_t exp_frame = ctx->buffers[bidx[0]].frame_number - 1; From 2b0eef840cb6774326b2039143753d047cb9f9c5 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Thu, 3 Sep 2015 10:25:17 +0200 Subject: [PATCH 077/120] fixed updating meta data of KLT image. now the preprocessing is only done for new frames. --- src/main.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main.c b/src/main.c index 6bec320..140c2e9 100644 --- a/src/main.c +++ b/src/main.c @@ -388,6 +388,7 @@ int main(void) } } klt_preprocess_image(frames[i]->buffer, klt_images[i]); + klt_images[i]->meta = frames[i]->frame_number; } } } From b530c547b30ae29e185b818732c1a73f83f7259f Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Thu, 3 Sep 2015 11:25:29 +0200 Subject: [PATCH 078/120] fixed buffer_count check in camera_init. --- src/camera.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/camera.c b/src/camera.c index a0863f5..4fd4d38 100644 --- a/src/camera.c +++ b/src/camera.c @@ -59,7 +59,7 @@ bool camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const c ctx->sensor = sensor; ctx->transport = transport; // check parameter: - if (buffer_count > CONFIG_CAMERA_MAX_BUFFER_COUNT && buffer_count < 2) { + if (buffer_count > CONFIG_CAMERA_MAX_BUFFER_COUNT || buffer_count < 2) { return false; } uint32_t img_size = (uint32_t)img_param->size.x * (uint32_t)img_param->size.y; From f882d3d597eaa9caf5fa7a8b52a661fd1cbd7509 Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Thu, 3 Sep 2015 11:18:41 -0700 Subject: [PATCH 079/120] Explict check for index-out-of-bounds warning --- src/modules/flow/camera.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/flow/camera.c b/src/modules/flow/camera.c index 785009d..f4b79c0 100644 --- a/src/modules/flow/camera.c +++ b/src/modules/flow/camera.c @@ -143,7 +143,7 @@ static bool camera_buffer_fifo_remove_front(camera_ctx *ctx, int *out, size_t co *out++ = ctx->buf_avail[i]; } // close gap: - for (i = count; i < bc; ++i) { + for (i = count; i < bc && i < CONFIG_CAMERA_MAX_BUFFER_COUNT; ++i) { ctx->buf_avail[i - count] = ctx->buf_avail[i]; } ctx->buf_avail_count = bc - count; From e5c4c783d814cb957b76a0b77a3ef84df73b6fcb Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Tue, 8 Sep 2015 10:15:51 -0700 Subject: [PATCH 080/120] Fix uninitialized variable and float comparison in exposure adjustment --- src/modules/flow/mt9v034.c | 14 ++++---------- 1 file changed, 4 insertions(+), 10 deletions(-) diff --git a/src/modules/flow/mt9v034.c b/src/modules/flow/mt9v034.c index f2f03a1..2f01393 100644 --- a/src/modules/flow/mt9v034.c +++ b/src/modules/flow/mt9v034.c @@ -206,8 +206,6 @@ bool mt9v034_update_exposure_param(void *usr, uint32_t exposure, float gain) { return false; } - bool update_exposure; - bool update_gain; int context_idx; if (ctx->do_switch_context) { context_idx = ctx->desired_context; @@ -258,25 +256,21 @@ bool mt9v034_update_exposure_param(void *usr, uint32_t exposure, float gain) { fine_sw_total = 260; } uint32_t real_exposure = coarse_sw_total * (uint32_t)row_time + fine_sw_total; - if (ctx_param->exposure != real_exposure) { - update_exposure = true; - } + bool update_exposure = (ctx_param->exposure != real_exposure); /** * Analog Gain * [6:0] Analog Gain: Range 16 - 64 * [15] Force 0.75X: 0 = Disabled. */ - uint16_t analog_gain = gain * 16; + uint16_t analog_gain = gain * 16 + 0.5f; /* limit: */ if (analog_gain < 16) { analog_gain = 16; } else if (analog_gain > 64) { analog_gain = 64; } + bool update_gain = ((uint16_t)(ctx_param->analog_gain * 16 + 0.05f) != analog_gain); float real_analog_gain = analog_gain * 0.0625f; - if (!FLOAT_EQ_FLOAT(ctx_param->analog_gain, real_analog_gain)) { - update_gain = true; - } uint16_t sw_for_blanking = coarse_sw_total; if (sw_for_blanking < ctx_param->p.size.y) { @@ -664,7 +658,7 @@ static bool mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, analog_gain = 64; } float real_analog_gain = analog_gain * 0.0625f; - if (ctx_param->analog_gain != real_analog_gain) { + if ((uint16_t)(ctx_param->analog_gain * 16 + 0.05f) != analog_gain) { update_gain = true; } From 334c6b89095286380444afa1479f093f1fdafb41 Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Tue, 8 Sep 2015 10:16:50 -0700 Subject: [PATCH 081/120] Include UAVCAN timestamp and range data in USB packet --- read_image.py | 6 ++--- src/modules/flow/main.c | 56 +++++++++++++++++++++-------------------- 2 files changed, 32 insertions(+), 30 deletions(-) diff --git a/read_image.py b/read_image.py index baa2725..68e1a46 100644 --- a/read_image.py +++ b/read_image.py @@ -22,15 +22,15 @@ data = endpoint.read(64 * (1 + (READ_SIZE) / 64), timeout=20000) # Round up to include the zero-length packet if len(data) != READ_SIZE: - # We read a partial image from killing a previous instance of This + # We read a partial image from killing a previous instance of this # program in the middle of a transfer. We should be using an interface # alternate setting to enable and reset the endpoint. print "Ignoring partial buffer of", len(data) continue - (flags, timestamp, exposure, reserved) = struct.unpack(' //#define CONFIG_USE_PROBES @@ -110,10 +111,9 @@ static camera_ctx cam_ctx; static camera_img_param img_stream_param; typedef struct __attribute__((packed)) { - uint32_t flags; - uint32_t timestamp; + uint64_t timestamp; uint32_t exposure; - uint32_t reserved; + uint32_t ground_distance_mm; uint8_t snapshot_buffer_mem[USB_IMAGE_PIXELS * USB_IMAGE_PIXELS]; } usb_packet_format; @@ -225,13 +225,16 @@ static void send_image_step(void) { } } -static void start_send_image(void) { +static void start_send_image(float ground_distance_m) { usb_image_transfer_active = true; usb_image_pos = 0; - usb_packet.flags = 0; - usb_packet.timestamp = snapshot_buffer.timestamp; // TODO: use UAVCAN timestamp instead of local timestamp +#if defined(CONFIG_ARCH_BOARD_PX4FLOW_V2) + usb_packet.timestamp = hrt_absolute_time(); +#else + usb_packet.timestamp = 0; +#endif usb_packet.exposure = snapshot_buffer.param.exposure; - usb_packet.reserved = 0; + usb_packet.ground_distance_mm = ground_distance_m * 1000.0f; DCD_EP_Flush(&USB_OTG_dev, IMAGE_IN_EP); send_image_step(); } @@ -349,13 +352,31 @@ int main(void) fmu_comm_run(); + distance_valid = distance_read(&distance_filtered, &distance_raw); + /* reset to zero for invalid distances */ + if (!distance_valid) { + distance_filtered = 0.0f; + distance_raw = 0.0f; + } + + /* decide which distance to use */ + float ground_distance = 0.0f; + if(FLOAT_AS_BOOL(global_data.param[PARAM_SONAR_FILTERED])) + { + ground_distance = distance_filtered; + } + else + { + ground_distance = distance_raw; + } + if (snap_capture_done) { snap_capture_done = false; camera_snapshot_acknowledge(&cam_ctx); snap_ready = true; if (snap_capture_success) { /* send the snapshot! */ - start_send_image(); + start_send_image(ground_distance); } } @@ -371,13 +392,6 @@ int main(void) float x_rate = y_rate_sensor; // change x and y rates float y_rate = - x_rate_sensor; float z_rate = z_rate_sensor; // z is correct - - distance_valid = distance_read(&distance_filtered, &distance_raw); - /* reset to zero for invalid distances */ - if (!distance_valid) { - distance_filtered = 0.0f; - distance_raw = 0.0f; - } bool use_klt = !FLOAT_EQ_INT(global_data.param[PARAM_ALGORITHM_CHOICE], 0); @@ -495,18 +509,6 @@ int main(void) /* return the image buffers */ camera_img_stream_return_buffers(&cam_ctx, frames, 2); - - /* decide which distance to use */ - float ground_distance = 0.0f; - - if(FLOAT_AS_BOOL(global_data.param[PARAM_SONAR_FILTERED])) - { - ground_distance = distance_filtered; - } - else - { - ground_distance = distance_raw; - } /* update I2C transmit buffer */ fmu_comm_update(frame_dt, From 4a98fe2ad6295b2fd6d6577fb5ac086d6358a051 Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Wed, 9 Sep 2015 16:53:03 -0700 Subject: [PATCH 082/120] Fill range data, and send UAVCAN messages at proper rate --- src/modules/uavcannode/uavcannode.c | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/src/modules/uavcannode/uavcannode.c b/src/modules/uavcannode/uavcannode.c index 8592734..984f4dc 100644 --- a/src/modules/uavcannode/uavcannode.c +++ b/src/modules/uavcannode/uavcannode.c @@ -21,6 +21,16 @@ __EXPORT void fmu_comm_run() { uavcannode_run(); } +static void fill_range_data(result_accumulator_ctx *acc, range_data_t *range) { + range->roll = range->pitch = range->yaw = 0; + range->orientation_defined = 0; + range->field_of_view = 0; + range->sensor_id = 0; + range->sensor_type = SENSOR_TYPE_LIDAR; + range->reading_type = (acc->last.ground_distance >= 0) ? READING_TYPE_VALID_RANGE : READING_TYPE_UNDEFINED; + range->range = acc->last.ground_distance; +} + __EXPORT void fmu_comm_update(float 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) { @@ -31,11 +41,11 @@ __EXPORT void fmu_comm_update(float dt, float x_rate, float y_rate, float z_rate qual, pixel_flow_x, pixel_flow_y, rad_per_pixel, distance_valid, ground_distance, distance_age); - if (accumulator.frame_count >= 40) { + if (accumulator.frame_count % 32 == 0) { result_accumulator_fill_i2c_data(&accumulator, &i2c_data.frame, &i2c_data.integral_frame); - // TODO: fill range_data - uavcannode_publish_flow(&i2c_data); + fill_range_data(&accumulator, &range_data); uavcannode_publish_range(&range_data); + uavcannode_publish_flow(&i2c_data); result_accumulator_reset(&accumulator); } } \ No newline at end of file From 143eb2123a8c57f1e908f5865d41674d3de5c322 Mon Sep 17 00:00:00 2001 From: Tom Date: Wed, 16 Sep 2015 12:39:40 -0700 Subject: [PATCH 083/120] Replace optical_flow.LegacyRawSample with optical_flow.RawSample --- src/include/uavcan_if.h | 6 +-- .../optical_flow/1110.LegacyRawSample.uavcan | 6 --- .../flow/optical_flow/1110.RawSample.uavcan | 19 ++++++++++ .../flow/optical_flow/I2CFrame.uavcan | 17 --------- .../flow/optical_flow/I2CFrameIntegral.uavcan | 16 -------- src/modules/uavcannode/uavcannode.c | 4 +- src/modules/uavcannode/uavcannode_main.cpp | 37 +++++++------------ src/modules/uavcannode/uavcannode_main.hpp | 6 +-- 8 files changed, 40 insertions(+), 71 deletions(-) delete mode 100644 src/modules/uavcannode/dsdl/uavcan/threedr/equipment/flow/optical_flow/1110.LegacyRawSample.uavcan create mode 100644 src/modules/uavcannode/dsdl/uavcan/threedr/equipment/flow/optical_flow/1110.RawSample.uavcan delete mode 100644 src/modules/uavcannode/dsdl/uavcan/threedr/equipment/flow/optical_flow/I2CFrame.uavcan delete mode 100644 src/modules/uavcannode/dsdl/uavcan/threedr/equipment/flow/optical_flow/I2CFrameIntegral.uavcan diff --git a/src/include/uavcan_if.h b/src/include/uavcan_if.h index e142433..3f5ffb8 100644 --- a/src/include/uavcan_if.h +++ b/src/include/uavcan_if.h @@ -41,11 +41,11 @@ #include "hrt.h" -typedef struct legacy_12c_data_t { +typedef struct legacy_i2c_data_t { uint64_t time_stamp_utc; i2c_frame frame; i2c_integral_frame integral_frame; -} legacy_12c_data_t; +} legacy_i2c_data_t; typedef enum range_sensor_type_t { @@ -77,7 +77,7 @@ typedef struct range_data_t { __BEGIN_DECLS __EXPORT int uavcannode_run(void); -__EXPORT int uavcannode_publish_flow(legacy_12c_data_t *pdata); +__EXPORT int uavcannode_publish_flow(legacy_i2c_data_t *pdata); __EXPORT int uavcannode_publish_range(range_data_t *pdata); __EXPORT int uavcannode_main(bool start_not_stop); __END_DECLS diff --git a/src/modules/uavcannode/dsdl/uavcan/threedr/equipment/flow/optical_flow/1110.LegacyRawSample.uavcan b/src/modules/uavcannode/dsdl/uavcan/threedr/equipment/flow/optical_flow/1110.LegacyRawSample.uavcan deleted file mode 100644 index d65cea2..0000000 --- a/src/modules/uavcannode/dsdl/uavcan/threedr/equipment/flow/optical_flow/1110.LegacyRawSample.uavcan +++ /dev/null @@ -1,6 +0,0 @@ -# Current time. -# Note that the data type "uavcan.Timestamp" is defined by the UAVCAN specification. -uavcan.Timestamp time - -I2CFrame frame -I2CFrameIntegral integral diff --git a/src/modules/uavcannode/dsdl/uavcan/threedr/equipment/flow/optical_flow/1110.RawSample.uavcan b/src/modules/uavcannode/dsdl/uavcan/threedr/equipment/flow/optical_flow/1110.RawSample.uavcan new file mode 100644 index 0000000..6037479 --- /dev/null +++ b/src/modules/uavcannode/dsdl/uavcan/threedr/equipment/flow/optical_flow/1110.RawSample.uavcan @@ -0,0 +1,19 @@ +# Raw (unfused) optical flow sample + +uavcan.Timestamp time + +uint8 sensor_id + +# Lateral flow (displacement) in radians & delta angle over timestep +float16[2] flow_integral_xy_radians +float16[3] gyro_rate_integral_xyz_radians +# The time period over which the flow & gyro data was collected +uint21 integration_time_usec +# The maximum velocity (rad/s) for which the flow data can be trusted +float16 max_axis_velocity_radians_sec +# The percentage (0 to 1) of tiles for which the optical flow feature tracker could find a displacement +float16 samples_matched_pct +# Gyro temperature +float16 gyro_temperature_celsius +# Gyro covariance matrix +float16[<=9] gyro_rate_integral_xyz_covariance # Radians^2 / Radians diff --git a/src/modules/uavcannode/dsdl/uavcan/threedr/equipment/flow/optical_flow/I2CFrame.uavcan b/src/modules/uavcannode/dsdl/uavcan/threedr/equipment/flow/optical_flow/I2CFrame.uavcan deleted file mode 100644 index a20cd65..0000000 --- a/src/modules/uavcannode/dsdl/uavcan/threedr/equipment/flow/optical_flow/I2CFrame.uavcan +++ /dev/null @@ -1,17 +0,0 @@ -# -# Nested Type -# Legacy I2C -# - -uint16 frame_count -int16 pixel_flow_x_sum -int16 pixel_flow_y_sum -int16 flow_comp_m_x -int16 flow_comp_m_y -int16 qual -int16 gyro_x_rate -int16 gyro_y_rate -int16 gyro_z_rate -uint8 gyro_range -uint8 sonarimestamp -int16 ground_distance diff --git a/src/modules/uavcannode/dsdl/uavcan/threedr/equipment/flow/optical_flow/I2CFrameIntegral.uavcan b/src/modules/uavcannode/dsdl/uavcan/threedr/equipment/flow/optical_flow/I2CFrameIntegral.uavcan deleted file mode 100644 index cad6dd9..0000000 --- a/src/modules/uavcannode/dsdl/uavcan/threedr/equipment/flow/optical_flow/I2CFrameIntegral.uavcan +++ /dev/null @@ -1,16 +0,0 @@ -# -# Nested Type -# Legacy I2C Integral -# - -uint16 frame_count_since_last_readout -int16 pixel_flow_x_integral -int16 pixel_flow_y_integral -int16 gyro_x_rate_integral -int16 gyro_y_rate_integral -int16 gyro_z_rate_integral -uint32 integration_timespan -uint32 distance_timestamp -uint16 ground_distance -int16 gyro_temperature -uint8 qual diff --git a/src/modules/uavcannode/uavcannode.c b/src/modules/uavcannode/uavcannode.c index 984f4dc..6af5acd 100644 --- a/src/modules/uavcannode/uavcannode.c +++ b/src/modules/uavcannode/uavcannode.c @@ -9,7 +9,7 @@ #include "uavcan_if.h" static result_accumulator_ctx accumulator; -static legacy_12c_data_t i2c_data; +static legacy_i2c_data_t i2c_data; static range_data_t range_data; __EXPORT void fmu_comm_init() { @@ -48,4 +48,4 @@ __EXPORT void fmu_comm_update(float dt, float x_rate, float y_rate, float z_rate uavcannode_publish_flow(&i2c_data); result_accumulator_reset(&accumulator); } -} \ No newline at end of file +} diff --git a/src/modules/uavcannode/uavcannode_main.cpp b/src/modules/uavcannode/uavcannode_main.cpp index d119149..44107b7 100644 --- a/src/modules/uavcannode/uavcannode_main.cpp +++ b/src/modules/uavcannode/uavcannode_main.cpp @@ -279,31 +279,20 @@ int UavcanNode::publish(range_data_t *pdata) _range_pulisher.broadcast(m); return PX4_OK; } -int UavcanNode::publish(legacy_12c_data_t *pdata) +int UavcanNode::publish(legacy_i2c_data_t *pdata) { - ::threedr::equipment::flow::optical_flow::LegacyRawSample r; + ::threedr::equipment::flow::optical_flow::RawSample r; r.time.usec = pdata->time_stamp_utc; - r.frame.frame_count = pdata->frame.frame_count; - r.frame.pixel_flow_x_sum = pdata->frame.pixel_flow_x_sum; - r.frame.pixel_flow_y_sum = pdata->frame.pixel_flow_y_sum; - r.frame.flow_comp_m_x = pdata->frame.flow_comp_m_x; - r.frame.flow_comp_m_y = pdata->frame.flow_comp_m_y; - r.frame.qual = pdata->frame.qual; - r.frame.gyro_x_rate = pdata->frame.gyro_x_rate; - r.frame.gyro_y_rate = pdata->frame.gyro_y_rate; - r.frame.gyro_z_rate = pdata->frame.gyro_z_rate; - r.frame.gyro_range = pdata->frame.gyro_range; - r.integral.frame_count_since_last_readout = pdata->integral_frame.frame_count_since_last_readout; - r.integral.pixel_flow_x_integral = pdata->integral_frame.pixel_flow_x_integral; - r.integral.pixel_flow_y_integral = pdata->integral_frame.pixel_flow_y_integral; - r.integral.gyro_x_rate_integral = pdata->integral_frame.gyro_x_rate_integral; - r.integral.gyro_y_rate_integral = pdata->integral_frame.gyro_y_rate_integral; - r.integral.gyro_z_rate_integral = pdata->integral_frame.gyro_z_rate_integral; - r.integral.integration_timespan = pdata->integral_frame.integration_timespan; - r.integral.distance_timestamp = pdata->integral_frame.distance_timestamp; - r.integral.ground_distance = pdata->integral_frame.ground_distance; - r.integral.gyro_temperature = pdata->integral_frame.gyro_temperature; - r.integral.qual = pdata->integral_frame.qual; + r.flow_integral_xy_radians[0] = static_cast(pdata->integral_frame.pixel_flow_x_integral) / 10000.0f; + r.flow_integral_xy_radians[1] = static_cast(pdata->integral_frame.pixel_flow_y_integral) / 10000.0f; + r.gyro_rate_integral_xyz_radians[0] = static_cast(pdata->integral_frame.gyro_x_rate_integral) / 10000.0f; + r.gyro_rate_integral_xyz_radians[1] = static_cast(pdata->integral_frame.gyro_y_rate_integral) / 10000.0f; + r.gyro_rate_integral_xyz_radians[2] = static_cast(pdata->integral_frame.gyro_z_rate_integral) / 10000.0f; + r.integration_time_usec = pdata->integral_frame.integration_timespan; + //FIXME: r.max_axis_velocity_radians_sec + r.samples_matched_pct = pdata->integral_frame.qual / 255.0f; + r.gyro_temperature_celsius = pdata->integral_frame.gyro_temperature / 100.0f; + //FIXME: r.gyro_rate_integral_xyz_covariance _flow_pulisher.broadcast(r); return PX4_OK; @@ -393,7 +382,7 @@ TODO(Need non vol Paramter sotrage) return UavcanNode::start(node_id, bitrate); } -__EXPORT int uavcannode_publish_flow(legacy_12c_data_t *pdata) +__EXPORT int uavcannode_publish_flow(legacy_i2c_data_t *pdata) { UavcanNode *const inst = UavcanNode::instance(); diff --git a/src/modules/uavcannode/uavcannode_main.hpp b/src/modules/uavcannode/uavcannode_main.hpp index e88b66e..939a710 100644 --- a/src/modules/uavcannode/uavcannode_main.hpp +++ b/src/modules/uavcannode/uavcannode_main.hpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include #include "uavcan_if.h" @@ -104,7 +104,7 @@ class UavcanNode static UavcanNode *instance() { return _instance; } int run(); - int publish(legacy_12c_data_t *pdata); + int publish(legacy_i2c_data_t *pdata); int publish(range_data_t *pdata); /* The bit rate that can be passed back to the bootloader */ @@ -129,7 +129,7 @@ class UavcanNode uavcan::ServiceServer _fw_update_listner; uavcan::GlobalTimeSyncSlave _time_sync_slave; - uavcan::Publisher<::threedr::equipment::flow::optical_flow::LegacyRawSample> _flow_pulisher; + uavcan::Publisher<::threedr::equipment::flow::optical_flow::RawSample> _flow_pulisher; uavcan::Publisher _range_pulisher; void cb_beginfirmware_update(const uavcan::ReceivedDataStructure &req, uavcan::ServiceResponseDataStructure &rsp); From 96976a983b57e7ba2e0ad0dcd4e7fa5af02094f2 Mon Sep 17 00:00:00 2001 From: Tom Date: Wed, 16 Sep 2015 12:54:59 -0700 Subject: [PATCH 084/120] Fix parsing issue on old compilers --- src/modules/uavcannode/uavcannode_main.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcannode/uavcannode_main.hpp b/src/modules/uavcannode/uavcannode_main.hpp index 939a710..479c9a2 100644 --- a/src/modules/uavcannode/uavcannode_main.hpp +++ b/src/modules/uavcannode/uavcannode_main.hpp @@ -129,7 +129,7 @@ class UavcanNode uavcan::ServiceServer _fw_update_listner; uavcan::GlobalTimeSyncSlave _time_sync_slave; - uavcan::Publisher<::threedr::equipment::flow::optical_flow::RawSample> _flow_pulisher; + uavcan::Publisher< ::threedr::equipment::flow::optical_flow::RawSample> _flow_pulisher; uavcan::Publisher _range_pulisher; void cb_beginfirmware_update(const uavcan::ReceivedDataStructure &req, uavcan::ServiceResponseDataStructure &rsp); From 97e3c211abd0de21ba5c233c7e7ce682247dae8c Mon Sep 17 00:00:00 2001 From: Tom Date: Wed, 16 Sep 2015 13:39:14 -0700 Subject: [PATCH 085/120] Add sensor id to uavcan message --- src/modules/uavcannode/uavcannode_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uavcannode/uavcannode_main.cpp b/src/modules/uavcannode/uavcannode_main.cpp index 44107b7..3da4a2b 100644 --- a/src/modules/uavcannode/uavcannode_main.cpp +++ b/src/modules/uavcannode/uavcannode_main.cpp @@ -283,6 +283,7 @@ int UavcanNode::publish(legacy_i2c_data_t *pdata) { ::threedr::equipment::flow::optical_flow::RawSample r; r.time.usec = pdata->time_stamp_utc; + r.sensor_id = 0; r.flow_integral_xy_radians[0] = static_cast(pdata->integral_frame.pixel_flow_x_integral) / 10000.0f; r.flow_integral_xy_radians[1] = static_cast(pdata->integral_frame.pixel_flow_y_integral) / 10000.0f; r.gyro_rate_integral_xyz_radians[0] = static_cast(pdata->integral_frame.gyro_x_rate_integral) / 10000.0f; From a205c4081ba2a619852888d6b62c2138afa8caff Mon Sep 17 00:00:00 2001 From: Tom Date: Fri, 18 Sep 2015 11:45:58 -0700 Subject: [PATCH 086/120] Integrate Pavel's suggestions to optical_flow.RawSample --- .../equipment/flow/optical_flow/1110.RawSample.uavcan | 8 +++----- src/modules/uavcannode/uavcannode_main.cpp | 5 ++--- 2 files changed, 5 insertions(+), 8 deletions(-) diff --git a/src/modules/uavcannode/dsdl/uavcan/threedr/equipment/flow/optical_flow/1110.RawSample.uavcan b/src/modules/uavcannode/dsdl/uavcan/threedr/equipment/flow/optical_flow/1110.RawSample.uavcan index 6037479..7387aba 100644 --- a/src/modules/uavcannode/dsdl/uavcan/threedr/equipment/flow/optical_flow/1110.RawSample.uavcan +++ b/src/modules/uavcannode/dsdl/uavcan/threedr/equipment/flow/optical_flow/1110.RawSample.uavcan @@ -1,18 +1,16 @@ # Raw (unfused) optical flow sample -uavcan.Timestamp time - -uint8 sensor_id +uavcan.Timestamp timestamp # Lateral flow (displacement) in radians & delta angle over timestep float16[2] flow_integral_xy_radians float16[3] gyro_rate_integral_xyz_radians # The time period over which the flow & gyro data was collected -uint21 integration_time_usec +uint24 integration_time_usec # The maximum velocity (rad/s) for which the flow data can be trusted float16 max_axis_velocity_radians_sec # The percentage (0 to 1) of tiles for which the optical flow feature tracker could find a displacement -float16 samples_matched_pct +uint8 samples_matched_pct # Gyro temperature float16 gyro_temperature_celsius # Gyro covariance matrix diff --git a/src/modules/uavcannode/uavcannode_main.cpp b/src/modules/uavcannode/uavcannode_main.cpp index 3da4a2b..5ceff40 100644 --- a/src/modules/uavcannode/uavcannode_main.cpp +++ b/src/modules/uavcannode/uavcannode_main.cpp @@ -282,8 +282,7 @@ int UavcanNode::publish(range_data_t *pdata) int UavcanNode::publish(legacy_i2c_data_t *pdata) { ::threedr::equipment::flow::optical_flow::RawSample r; - r.time.usec = pdata->time_stamp_utc; - r.sensor_id = 0; + r.timestamp.usec = pdata->time_stamp_utc; r.flow_integral_xy_radians[0] = static_cast(pdata->integral_frame.pixel_flow_x_integral) / 10000.0f; r.flow_integral_xy_radians[1] = static_cast(pdata->integral_frame.pixel_flow_y_integral) / 10000.0f; r.gyro_rate_integral_xyz_radians[0] = static_cast(pdata->integral_frame.gyro_x_rate_integral) / 10000.0f; @@ -291,7 +290,7 @@ int UavcanNode::publish(legacy_i2c_data_t *pdata) r.gyro_rate_integral_xyz_radians[2] = static_cast(pdata->integral_frame.gyro_z_rate_integral) / 10000.0f; r.integration_time_usec = pdata->integral_frame.integration_timespan; //FIXME: r.max_axis_velocity_radians_sec - r.samples_matched_pct = pdata->integral_frame.qual / 255.0f; + r.samples_matched_pct = pdata->integral_frame.qual; r.gyro_temperature_celsius = pdata->integral_frame.gyro_temperature / 100.0f; //FIXME: r.gyro_rate_integral_xyz_covariance _flow_pulisher.broadcast(r); From 9ea5d3c297c5c3767f0786975d255542e485d678 Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Thu, 17 Sep 2015 15:35:39 -0700 Subject: [PATCH 087/120] Remove unused implementation of itoa It has the arguments in non-standard order, which GCC 4.9 complains about. Fixes #30. --- src/include/utils.h | 1 - src/modules/flow/dcmi.c | 1 - src/modules/flow/main.c | 1 - src/modules/flow/utils.c | 26 -------------------------- src/modules/lidar/lidar.c | 1 - src/modules/sonar/sonar.c | 1 - 6 files changed, 31 deletions(-) diff --git a/src/include/utils.h b/src/include/utils.h index 92134b9..a95632e 100644 --- a/src/include/utils.h +++ b/src/include/utils.h @@ -42,6 +42,5 @@ */ char* ftoa(float f); void ltoa(char *buf, unsigned long i, int base); -void itoa(char *buf, unsigned int i, int base); #endif /* UTILS_H_ */ diff --git a/src/modules/flow/dcmi.c b/src/modules/flow/dcmi.c index b7b17c6..ec1c270 100644 --- a/src/modules/flow/dcmi.c +++ b/src/modules/flow/dcmi.c @@ -40,7 +40,6 @@ #include "no_warnings.h" #include "mavlink_bridge_header.h" #include -#include "utils.h" #include "dcmi.h" #include "timer.h" #include "stm32f4xx_gpio.h" diff --git a/src/modules/flow/main.c b/src/modules/flow/main.c index ccfe314..c44434a 100644 --- a/src/modules/flow/main.c +++ b/src/modules/flow/main.c @@ -51,7 +51,6 @@ #include "mavlink_bridge_header.h" #include #include "settings.h" -#include "utils.h" #include "filter.h" #include "result_accumulator.h" #include "flow.h" diff --git a/src/modules/flow/utils.c b/src/modules/flow/utils.c index 3252f23..d8ad615 100644 --- a/src/modules/flow/utils.c +++ b/src/modules/flow/utils.c @@ -157,29 +157,3 @@ void ltoa(char *buf, unsigned long i, int base) } strcpy(buf, s); } - -void itoa(char *buf, unsigned int i, int base) -{ - char *s; - const int len = 10; - int rem; - char rev[len+1]; - - if (i == 0) - s = "0"; - else - { - rev[len] = 0; - s = &rev[len]; - while (i) - { - rem = i % base; - if (rem < 10) - *--s = rem + '0'; - else if (base == 16) - *--s = "abcdef"[rem - 10]; - i /= base; - } - } - strcpy(buf, s); -} diff --git a/src/modules/lidar/lidar.c b/src/modules/lidar/lidar.c index adce342..a7c909d 100644 --- a/src/modules/lidar/lidar.c +++ b/src/modules/lidar/lidar.c @@ -35,7 +35,6 @@ #include "distance.h" #include "usbd_cdc_vcp.h" #include "main.h" -#include "utils.h" #include "distance_mode_filter.h" #include "stm32f4xx.h" diff --git a/src/modules/sonar/sonar.c b/src/modules/sonar/sonar.c index 97ee813..20881b8 100644 --- a/src/modules/sonar/sonar.c +++ b/src/modules/sonar/sonar.c @@ -46,7 +46,6 @@ #include "stm32f4xx_tim.h" #include "stm32f4xx_dma.h" #include "misc.h" -#include "utils.h" #include "usart.h" #include "settings.h" #include "distance.h" From 12b268681a7a79f5601e32b8c79d9dc9cd71f73f Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Thu, 17 Sep 2015 15:53:33 -0700 Subject: [PATCH 088/120] Remove nearly-unused mavlink debug channel and associated integer conversions --- src/include/utils.h | 46 ------- src/modules/flow/communication.c | 13 -- src/modules/flow/debug.c | 201 ------------------------------- src/modules/flow/flow.c | 1 - src/modules/flow/main.c | 2 - src/modules/flow/module.mk | 2 - src/modules/flow/utils.c | 159 ------------------------ 7 files changed, 424 deletions(-) delete mode 100644 src/include/utils.h delete mode 100644 src/modules/flow/debug.c delete mode 100644 src/modules/flow/utils.c diff --git a/src/include/utils.h b/src/include/utils.h deleted file mode 100644 index a95632e..0000000 --- a/src/include/utils.h +++ /dev/null @@ -1,46 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * -> Code from CodeForge.com - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#ifndef UTILS_H_ -#define UTILS_H_ - -#include - -/** -* @brief Float to Ascii string -*/ -char* ftoa(float f); -void ltoa(char *buf, unsigned long i, int base); - -#endif /* UTILS_H_ */ diff --git a/src/modules/flow/communication.c b/src/modules/flow/communication.c index 07e536a..06b34e9 100644 --- a/src/modules/flow/communication.c +++ b/src/modules/flow/communication.c @@ -45,7 +45,6 @@ #include "mt9v034.h" #include "dcmi.h" #include "gyro.h" -#include "debug.h" #include "communication.h" #include "timer.h" @@ -241,22 +240,10 @@ void handle_mavlink_message(mavlink_channel_t chan, { notify_changed_camera_parameters(); } - /* handle calibration on/off */ - else if(i == PARAM_VIDEO_ONLY) - { - if(FLOAT_AS_BOOL(global_data.param[PARAM_VIDEO_ONLY])) - debug_string_message_buffer("Calibration Mode On"); - else - debug_string_message_buffer("Calibration Mode Off"); - } else if(i == PARAM_GYRO_SENSITIVITY_DPS) { l3gd20_config(); } - else - { - debug_int_message_buffer("Parameter received, param id =", i); - } /* report back new value */ mavlink_msg_param_value_send(MAVLINK_COMM_0, diff --git a/src/modules/flow/debug.c b/src/modules/flow/debug.c deleted file mode 100644 index 03f3b79..0000000 --- a/src/modules/flow/debug.c +++ /dev/null @@ -1,201 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include "no_warnings.h" -#include "mavlink_bridge_header.h" -#include -#include "utils.h" -#include "settings.h" -#include "debug.h" - -typedef enum -{ - DEBUG_STRING, - DEBUG_INT, - DEBUG_FLOAT -} DebugMsgType_TypeDef; - -/* send buffers */ -static DebugMsgType_TypeDef m_debug_buf_type[DEBUG_COUNT]; -static char const * m_debug_buf_pointer[DEBUG_COUNT]; //string pointer buffer -static int32_t m_debug_buf_int[DEBUG_COUNT]; -static float m_debug_buf_float[DEBUG_COUNT]; - -static uint8_t m_debug_index_write = 0; -static uint8_t m_debug_index_read = 0; -static uint16_t m_debug_count = 0; -static bool m_debug_was_full = false; - -/** - * @brief Add a debug message to the send buffer - * @note This function is interrupt service routine (ISR) SAFE, as it only access RAM. - * - * @param string Message - * @return 1 if successfully added, 0 else - */ -static uint8_t debug_message_buffer(const char* string) -{ - if (m_debug_index_read - m_debug_index_write == 1 || (m_debug_index_read - == 0 && m_debug_index_write == DEBUG_COUNT - 1)) - { - /* buffer full, can't send */ - m_debug_was_full = true; - return 0; - } - m_debug_index_write = (m_debug_index_write + 1) % DEBUG_COUNT; - m_debug_count++; - - /* buffer pointer to string in program code */ - m_debug_buf_pointer[m_debug_index_write] = string; - - return 1; -} - -/** - * @brief Add a string debug message to the send buffer - * - * @param string Message - * @return 1 if successfully added, 0 else - */ -uint8_t debug_string_message_buffer(const char* string) -{ - if (debug_message_buffer(string)) - { - /* Could write, save message to buffer */ - m_debug_buf_type[m_debug_index_write] = DEBUG_STRING; - return 1; - } - else - { - /* Could not write, do nothing */ - return 0; - } -} - -/** - * @brief Add a string-integer debug message to the send buffers - * - * @param string Message - * @return 1 if successfully added, 0 else - */ -uint8_t debug_int_message_buffer(const char* string, int32_t num) -{ - if (debug_message_buffer(string)) - { - /* Could write, save integer to buffer */ - m_debug_buf_int[m_debug_index_write] = num; - m_debug_buf_type[m_debug_index_write] = DEBUG_INT; - return 1; - } - else - { - /* Could not write, do nothing */ - return 0; - } -} - -/** - * @brief Add a string-float debug message to the send buffers - * - * @param string Message - * @return 1 if successfully added, 0 else - */ -uint8_t debug_float_message_buffer(const char* string, float num) -{ - if (debug_message_buffer(string)) - { - /* Could write, save float to buffer */ - m_debug_buf_float[m_debug_index_write] = num; - m_debug_buf_type[m_debug_index_write] = DEBUG_FLOAT; - return 1; - } - else - { - /* Could not write, do nothing */ - return 0; - } -} - -/** - * @brief Send one of the buffered messages - */ -void debug_message_send_one(void) -{ - if (m_debug_index_write == m_debug_index_read) - { - /* buffer empty */ - return; - } - m_debug_index_read = (m_debug_index_read + 1) % DEBUG_COUNT; - - char msg[DEBUG_MAX_LEN] = {}; - - switch(m_debug_buf_type[m_debug_index_read]) - { - case(DEBUG_STRING): - strncpy(msg, m_debug_buf_pointer[m_debug_index_read], DEBUG_MAX_LEN); - break; - - case(DEBUG_INT): - strncat(msg, m_debug_buf_pointer[m_debug_index_read], DEBUG_MAX_LEN); - strncat(msg, " ", DEBUG_MAX_LEN); - strncat(msg, ftoa(m_debug_buf_int[m_debug_index_read]), DEBUG_MAX_LEN); - msg[strlen(msg) - 2] = '\0'; // TODO workaround: cut ".0" of float - break; - - case(DEBUG_FLOAT): - strncat(msg, m_debug_buf_pointer[m_debug_index_read], DEBUG_MAX_LEN); - strncat(msg, " ", DEBUG_MAX_LEN); - strncat(msg, ftoa(m_debug_buf_float[m_debug_index_read]), DEBUG_MAX_LEN); - break; - - default: - return; - } - - - if(m_debug_was_full) - { - msg[0] = '+'; - m_debug_was_full = false; - } - - msg[DEBUG_MAX_LEN - 1] = '\0'; //enforce string termination - - mavlink_msg_statustext_send(MAVLINK_COMM_0, 0, msg); - - if (FLOAT_AS_BOOL(global_data.param[PARAM_USB_SEND_DEBUG])) - mavlink_msg_statustext_send(MAVLINK_COMM_2, 0, msg); -} - diff --git a/src/modules/flow/flow.c b/src/modules/flow/flow.c index 6db20d5..be3d831 100644 --- a/src/modules/flow/flow.c +++ b/src/modules/flow/flow.c @@ -45,7 +45,6 @@ #include #include "flow.h" #include "dcmi.h" -#include "debug.h" #include "timer.h" #define __INLINE inline diff --git a/src/modules/flow/main.c b/src/modules/flow/main.c index c44434a..5329540 100644 --- a/src/modules/flow/main.c +++ b/src/modules/flow/main.c @@ -62,7 +62,6 @@ #include "usart.h" #include "distance.h" #include "communication.h" -#include "debug.h" #include "usbd_cdc_core.h" #include "usbd_usr.h" #include "usbd_desc.h" @@ -178,7 +177,6 @@ static void send_video_fn(void) { } static void send_params_fn(void) { - debug_message_send_one(); communication_parameter_send(); } diff --git a/src/modules/flow/module.mk b/src/modules/flow/module.mk index da4fd10..ef22337 100644 --- a/src/modules/flow/module.mk +++ b/src/modules/flow/module.mk @@ -22,7 +22,6 @@ SRCS = $(ST_LIB)startup_stm32f4xx.s \ SRCS += main.c \ - utils.c \ settings.c \ communication.c \ flow.c \ @@ -30,7 +29,6 @@ SRCS += main.c \ mt9v034.c \ gyro.c \ usart.c \ - debug.c \ usb_bsp.c \ usbd_cdc_vcp.c \ usbd_desc.c \ diff --git a/src/modules/flow/utils.c b/src/modules/flow/utils.c deleted file mode 100644 index d8ad615..0000000 --- a/src/modules/flow/utils.c +++ /dev/null @@ -1,159 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * -> Code from CodeForge.com - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include "utils.h" -#include - -typedef union { -long L; -float F; -} LF_t; - -char *ftoa(float f) //, int *status) -{ - long mantissa, int_part, frac_part; - short exp2; - LF_t x; - char *p; - static char outbuf[15]; - - //*status = 0; -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wfloat-equal" - if (f == 0.0f) - { -#pragma GCC diagnostic pop - outbuf[0] = '0'; - outbuf[1] = '.'; - outbuf[2] = '0'; - outbuf[3] = 0; - return outbuf; - } - x.F = f; - - exp2 = (unsigned char)(x.L >> 23) - 127; - mantissa = (x.L & 0xFFFFFF) | 0x800000; - frac_part = 0; - int_part = 0; - - if (exp2 >= 31) - { - //*status = _FTOA_TOO_LARGE; - return 0; - } - else if (exp2 < -23) - { - //*status = _FTOA_TOO_SMALL; - return 0; - } - else if (exp2 >= 23) - int_part = mantissa << (exp2 - 23); - else if (exp2 >= 0) - { - int_part = mantissa >> (23 - exp2); - frac_part = (mantissa << (exp2 + 1)) & 0xFFFFFF; - } - else /* if (exp2 < 0) */ - frac_part = (mantissa & 0xFFFFFF) >> -(exp2 + 1); - - p = outbuf; - - if (x.L < 0) - *p++ = '-'; - - if (int_part == 0) - *p++ = '0'; - else - { - ltoa(p, int_part, 10); - while (*p) - p++; - } - *p++ = '.'; - - if (frac_part == 0) - *p++ = '0'; - else - { - char m, max; - - max = sizeof (outbuf) - (p - outbuf) - 1; - if (max > 7) - max = 7; - /* print BCD */ - for (m = 0; m < max; m++) - { - /* frac_part *= 10; */ - frac_part = (frac_part << 3) + (frac_part << 1); - - *p++ = (frac_part >> 24) + '0'; - frac_part &= 0xFFFFFF; - } - /* delete ending zeroes */ - for (--p; p[0] == '0' && p[-1] != '.'; --p) - ; - ++p; - } - *p = 0; - - return outbuf; -} - - -void ltoa(char *buf, unsigned long i, int base) -{ - char *s; - #define LEN 25 - int rem; - char rev[LEN+1]; - - if (i == 0) - s = "0"; - else - { - rev[LEN] = 0; - s = &rev[LEN]; - while (i) - { - rem = i % base; - if (rem < 10) - *--s = rem + '0'; - else if (base == 16) - *--s = "abcdef"[rem - 10]; - i /= base; - } - } - strcpy(buf, s); -} From 2274c7fb292313c1c6ef480b942f49b634608a42 Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Fri, 18 Sep 2015 15:58:26 -0700 Subject: [PATCH 089/120] Remove unused image filtering --- src/include/filter.h | 45 -------------------- src/include/settings.h | 1 - src/modules/flow/filter.c | 85 ------------------------------------- src/modules/flow/main.c | 6 --- src/modules/flow/module.mk | 1 - src/modules/flow/settings.c | 4 -- 6 files changed, 142 deletions(-) delete mode 100644 src/include/filter.h delete mode 100644 src/modules/flow/filter.c diff --git a/src/include/filter.h b/src/include/filter.h deleted file mode 100644 index a85cba5..0000000 --- a/src/include/filter.h +++ /dev/null @@ -1,45 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#ifndef FILTER_H_ -#define FILTER_H_ - -#include - -/** - * @brief Filters the image to improve the flow processing - * @note It uses a temporary buffer on the stack to work on the image. - */ -void filter_image(uint8_t *image, uint16_t width); - -#endif /* FILTER_H_ */ diff --git a/src/include/settings.h b/src/include/settings.h index b988a44..be2d499 100644 --- a/src/include/settings.h +++ b/src/include/settings.h @@ -111,7 +111,6 @@ enum global_param_id_t PARAM_IMAGE_INTERVAL, PARAM_ALGORITHM_CHOICE, - PARAM_ALGORITHM_IMAGE_FILTER, PARAM_ALGORITHM_CORNER_KAPPA, PARAM_ALGORITHM_OUTLIER_THR_BLOCK, PARAM_ALGORITHM_OUTLIER_THR_KLT, diff --git a/src/modules/flow/filter.c b/src/modules/flow/filter.c deleted file mode 100644 index d48af93..0000000 --- a/src/modules/flow/filter.c +++ /dev/null @@ -1,85 +0,0 @@ - -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Petri Tanskanen - * Lorenz Meier - * Samuel Zihlmann - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include -#include -#include - -#include "stm32f4xx_conf.h" -#include "core_cm4_simd.h" - -#include "filter.h" - -void filter_image(uint8_t *image, uint16_t width) { - uint16_t ext_width = width + 2; - /* first we make a copy of the image and extend it beyond the edges by 1 pixel */ - uint8_t img_ext[ext_width * ext_width]; - #define IMG_PX(x, y) (*(image + width * (y) + (x))) - #define EXT_PX(x, y) (*(img_ext + ext_width * (y) + (x))) - int y, x; - /* copy the lines from the middle: */ - for (y = 0; y < width; y++) { - memcpy(&EXT_PX(1, y + 1), &IMG_PX(0, y), width); - } - /* top and bottom: */ - memcpy(&EXT_PX(1, 0), &IMG_PX(0, 0), width); - memcpy(&EXT_PX(1, width + 1), &IMG_PX(0, width - 1), width); - /* left and right: */ - for (y = 0; y < ext_width; y++) { - EXT_PX(0, y) = EXT_PX(1, y); - EXT_PX(ext_width - 1, y) = EXT_PX(ext_width - 2, y); - } - /* now compute with the following kernel: - * -1 -1 -1 - * -1 8 -1 - * -1 -1 -1 */ - for (y = 0; y < width; y++) { - for (x = 0; x < width; x++) { - int16_t sum = (uint16_t)EXT_PX(x, y + 0) + (uint16_t)EXT_PX(x + 1, y + 0) + (uint16_t)EXT_PX(x + 2, y + 0) + - (uint16_t)EXT_PX(x, y + 1) + + (uint16_t)EXT_PX(x + 2, y + 1) + - (uint16_t)EXT_PX(x, y + 2) + (uint16_t)EXT_PX(x + 1, y + 2) + (uint16_t)EXT_PX(x + 2, y + 2); - sum -= (((uint16_t)EXT_PX(x + 1, y + 1)) << 3); - /* the result will be between -255 * 8 to 255 * 8: */ - /* amplification could be done here. */ - /* clip the result to -128 to 127: */ - if (sum < -128) sum = -128; - else if (sum > 127) sum = 127; - IMG_PX(x, y) = sum + 128; - } - } -} diff --git a/src/modules/flow/main.c b/src/modules/flow/main.c index 5329540..1e7bcd5 100644 --- a/src/modules/flow/main.c +++ b/src/modules/flow/main.c @@ -51,7 +51,6 @@ #include "mavlink_bridge_header.h" #include #include "settings.h" -#include "filter.h" #include "result_accumulator.h" #include "flow.h" #include "timer.h" @@ -412,11 +411,6 @@ int main(void) bool used_klt_image[2] = {false, false}; for (i = 0; i < 2; ++i) { if (frames[i]->frame_number != frames[i]->meta) { - // the image is new. apply pre-processing: - /* filter the new image */ - if (FLOAT_AS_BOOL(global_data.param[PARAM_ALGORITHM_IMAGE_FILTER])) { - filter_image(frames[i]->buffer, frames[i]->param.p.size.x); - } /* update meta data to mark it as an up-to date image: */ frames[i]->meta = frames[i]->frame_number; } else { diff --git a/src/modules/flow/module.mk b/src/modules/flow/module.mk index ef22337..1b70646 100644 --- a/src/modules/flow/module.mk +++ b/src/modules/flow/module.mk @@ -36,7 +36,6 @@ SRCS += main.c \ reset.c \ distance_mode_filter.c \ camera.c \ - filter.c \ result_accumulator.c \ timer.c \ usb_composite.c \ diff --git a/src/modules/flow/settings.c b/src/modules/flow/settings.c index 49547c3..4cdeae6 100644 --- a/src/modules/flow/settings.c +++ b/src/modules/flow/settings.c @@ -123,10 +123,6 @@ void global_data_reset_param_defaults(void){ strcpy(global_data.param_name[PARAM_ALGORITHM_CHOICE], "ALG_CHOICE"); global_data.param_access[PARAM_ALGORITHM_CHOICE] = READ_WRITE; - global_data.param[PARAM_ALGORITHM_IMAGE_FILTER] = 0; - strcpy(global_data.param_name[PARAM_ALGORITHM_IMAGE_FILTER], "ALG_IMG_FILT"); - global_data.param_access[PARAM_ALGORITHM_IMAGE_FILTER] = READ_WRITE; - global_data.param[PARAM_ALGORITHM_CORNER_KAPPA] = 0.06; strcpy(global_data.param_name[PARAM_ALGORITHM_CORNER_KAPPA], "ALG_CORN_KAPPA"); global_data.param_access[PARAM_ALGORITHM_CORNER_KAPPA] = READ_WRITE; From 44c414775562496f5eca1b9aeb3bf7739e0aeac7 Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Mon, 21 Sep 2015 11:46:18 -0700 Subject: [PATCH 090/120] mt9v034: write reserved registers with recommended values from Rev G datasheet This eliminates the noise seen on every 4th row --- src/modules/flow/mt9v034.c | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/modules/flow/mt9v034.c b/src/modules/flow/mt9v034.c index 2f01393..ccf7d7d 100644 --- a/src/modules/flow/mt9v034.c +++ b/src/modules/flow/mt9v034.c @@ -434,6 +434,16 @@ static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx, bool initial) { */ ctx->pixel_frame_line_ctrl_reg = 0x0000; mt9v034_WriteReg(MTV_PIXEL_FRAME_LINE_CTRL_REG, ctx->pixel_frame_line_ctrl_reg); + + + // Write reserved registers per Rev G datasheet table 8 recommendations + mt9v034_WriteReg(0x13, 0x2D2E); + mt9v034_WriteReg(0x20, 0x03C7); + mt9v034_WriteReg(0x24, 0x001B); + mt9v034_WriteReg(0x2B, 0x0003); + mt9v034_WriteReg(0x2F, 0x0003); + + } /* settings that are the same for both contexts: */ From 344c5c05d2f66dfb36d66feb76590e3c65bcd560 Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Mon, 21 Sep 2015 15:13:54 -0700 Subject: [PATCH 091/120] uavcan: Refactor data handling Move the entire uavcan module into C++ so DSDL structs can be filled directly, without going through intermediate C structs. --- src/include/uavcan_if.h | 112 --------------- src/modules/flow/main.c | 1 - src/modules/i2c_slave/i2c_slave.c | 1 - src/modules/uavcannode/module.mk | 1 - src/modules/uavcannode/uavcannode.c | 51 ------- src/modules/uavcannode/uavcannode_main.cpp | 159 ++++++++++----------- src/modules/uavcannode/uavcannode_main.hpp | 8 +- 7 files changed, 75 insertions(+), 258 deletions(-) delete mode 100644 src/include/uavcan_if.h delete mode 100644 src/modules/uavcannode/uavcannode.c diff --git a/src/include/uavcan_if.h b/src/include/uavcan_if.h deleted file mode 100644 index 3f5ffb8..0000000 --- a/src/include/uavcan_if.h +++ /dev/null @@ -1,112 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -#include -#include -#include -#include -#include "i2c_frame.h" -#include "hrt.h" - - -typedef struct legacy_i2c_data_t { - uint64_t time_stamp_utc; - i2c_frame frame; - i2c_integral_frame integral_frame; -} legacy_i2c_data_t; - - -typedef enum range_sensor_type_t { - SENSOR_TYPE_UNDEFINED = 0, - SENSOR_TYPE_SONAR = 1, - SENSOR_TYPE_LIDAR = 2, - SENSOR_TYPE_RADAR = 3 -} range_sensor_type_t; - -typedef enum range_sensor_reding_type_t { - READING_TYPE_UNDEFINED = 0, // Range is unknown - READING_TYPE_VALID_RANGE = 1, // Range field contains valid distance - READING_TYPE_TOO_CLOSE = 2, // Range field contains min range for the sensor - READING_TYPE_TOO_FAR = 3, // Range field contains max range for the sensor -} range_sensor_reding_type_t; - -typedef struct range_data_t { - uint64_t time_stamp_utc; - int8_t roll; - int8_t pitch; - int8_t yaw; - bool orientation_defined; - uint8_t sensor_id; - float field_of_view; - range_sensor_type_t sensor_type; - range_sensor_reding_type_t reading_type; - float range; -} range_data_t; - -__BEGIN_DECLS -__EXPORT int uavcannode_run(void); -__EXPORT int uavcannode_publish_flow(legacy_i2c_data_t *pdata); -__EXPORT int uavcannode_publish_range(range_data_t *pdata); -__EXPORT int uavcannode_main(bool start_not_stop); -__END_DECLS - - -#if defined(CONFIG_ARCH_BOARD_PX4FLOW_V2) -#define uavcan_start() uavcannode_main(true) -#define uavcan_stop() uavcannode_main(false) -#define uavcan_run() uavcannode_run() -#define uavcan_publish(what, rate, name) if (++name##_cnt >= rate) { \ - name##_cnt = 0; \ - (void) uavcannode_publish_##what(&name); \ - } -#define uavcan_export memcpy -#define uavcan_define_export(name, type, section) type name; \ - static uint8_t name##_cnt = 0; - -#define uavcan_use_export(name) &name -#define uavcan_timestamp_export(name) name.time_stamp_utc = hrt_absolute_time() -#define uavcan_assign(lhs, rhs) lhs = rhs - -#else -#define uavcan_start() -#define uavcan_stop() -#define uavcan_run() -#define uavcan_publish(what, rate, name) -#define uavcan_export(d,s,l) -#define uavcan_define_export(name, type, section) -#define uavcan_use_export(name) NULL -#define uavcan_timestamp_export(name) -#define uavcan_assign(lhs, rhs) -#endif diff --git a/src/modules/flow/main.c b/src/modules/flow/main.c index 1e7bcd5..4493cdf 100644 --- a/src/modules/flow/main.c +++ b/src/modules/flow/main.c @@ -67,7 +67,6 @@ #include "usbd_cdc_vcp.h" #include "main.h" #include "hrt.h" -#include #include //#define CONFIG_USE_PROBES diff --git a/src/modules/i2c_slave/i2c_slave.c b/src/modules/i2c_slave/i2c_slave.c index 78612c3..c46ce7d 100644 --- a/src/modules/i2c_slave/i2c_slave.c +++ b/src/modules/i2c_slave/i2c_slave.c @@ -32,7 +32,6 @@ ****************************************************************************/ #include -#include #include "fmu_comm.h" #include "px4_config.h" diff --git a/src/modules/uavcannode/module.mk b/src/modules/uavcannode/module.mk index 7c9a3b8..cbb9558 100644 --- a/src/modules/uavcannode/module.mk +++ b/src/modules/uavcannode/module.mk @@ -50,7 +50,6 @@ MODULE_STACKSIZE = 1024 # Main SRCS += uavcannode_main.cpp \ - uavcannode.c \ # diff --git a/src/modules/uavcannode/uavcannode.c b/src/modules/uavcannode/uavcannode.c deleted file mode 100644 index 6af5acd..0000000 --- a/src/modules/uavcannode/uavcannode.c +++ /dev/null @@ -1,51 +0,0 @@ -#include -#include -#include -#include -#include "i2c_frame.h" -#include "hrt.h" -#include "fmu_comm.h" -#include "result_accumulator.h" -#include "uavcan_if.h" - -static result_accumulator_ctx accumulator; -static legacy_i2c_data_t i2c_data; -static range_data_t range_data; - -__EXPORT void fmu_comm_init() { - uavcannode_main(true); - result_accumulator_init(&accumulator); -} - -__EXPORT void fmu_comm_run() { - uavcannode_run(); -} - -static void fill_range_data(result_accumulator_ctx *acc, range_data_t *range) { - range->roll = range->pitch = range->yaw = 0; - range->orientation_defined = 0; - range->field_of_view = 0; - range->sensor_id = 0; - range->sensor_type = SENSOR_TYPE_LIDAR; - range->reading_type = (acc->last.ground_distance >= 0) ? READING_TYPE_VALID_RANGE : READING_TYPE_UNDEFINED; - range->range = acc->last.ground_distance; -} - -__EXPORT void fmu_comm_update(float 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) { - range_data.time_stamp_utc = i2c_data.time_stamp_utc = hrt_absolute_time(); - - /* feed the accumulator and recalculate */ - result_accumulator_feed(&accumulator, 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); - - if (accumulator.frame_count % 32 == 0) { - result_accumulator_fill_i2c_data(&accumulator, &i2c_data.frame, &i2c_data.integral_frame); - fill_range_data(&accumulator, &range_data); - uavcannode_publish_range(&range_data); - uavcannode_publish_flow(&i2c_data); - result_accumulator_reset(&accumulator); - } -} diff --git a/src/modules/uavcannode/uavcannode_main.cpp b/src/modules/uavcannode/uavcannode_main.cpp index 5ceff40..ad74ae2 100644 --- a/src/modules/uavcannode/uavcannode_main.cpp +++ b/src/modules/uavcannode/uavcannode_main.cpp @@ -40,6 +40,15 @@ #include "boot_app_shared.h" #include +#include +#include + +extern "C" { + #include "settings.h" + #include "hrt.h" + #include "fmu_comm.h" + #include "result_accumulator.h" +} #define FW_GIT STRINGIFY(GIT_VERSION) @@ -263,36 +272,14 @@ class RestartRequestHandler: public uavcan::IRestartRequestHandler -int UavcanNode::publish(range_data_t *pdata) +int UavcanNode::publish(::uavcan::equipment::range_sensor::Measurement &m) { - ::uavcan::equipment::range_sensor::Measurement m; - m.timestamp.usec = pdata->time_stamp_utc; - m.sensor_id = pdata->sensor_id; - m.beam_orientation_in_body_frame.fixed_axis_roll_pitch_yaw[0] = pdata->roll * m.beam_orientation_in_body_frame.ANGLE_MULTIPLIER; - m.beam_orientation_in_body_frame.fixed_axis_roll_pitch_yaw[1] = pdata->pitch * m.beam_orientation_in_body_frame.ANGLE_MULTIPLIER; - m.beam_orientation_in_body_frame.fixed_axis_roll_pitch_yaw[2] = pdata->yaw * m.beam_orientation_in_body_frame.ANGLE_MULTIPLIER; - m.beam_orientation_in_body_frame.orientation_defined = true; - m.field_of_view = pdata->field_of_view; - m.sensor_type = pdata->sensor_type; - m.reading_type= pdata->reading_type; - m.range = pdata->range; _range_pulisher.broadcast(m); return PX4_OK; } -int UavcanNode::publish(legacy_i2c_data_t *pdata) + +int UavcanNode::publish(::threedr::equipment::flow::optical_flow::RawSample &r) { - ::threedr::equipment::flow::optical_flow::RawSample r; - r.timestamp.usec = pdata->time_stamp_utc; - r.flow_integral_xy_radians[0] = static_cast(pdata->integral_frame.pixel_flow_x_integral) / 10000.0f; - r.flow_integral_xy_radians[1] = static_cast(pdata->integral_frame.pixel_flow_y_integral) / 10000.0f; - r.gyro_rate_integral_xyz_radians[0] = static_cast(pdata->integral_frame.gyro_x_rate_integral) / 10000.0f; - r.gyro_rate_integral_xyz_radians[1] = static_cast(pdata->integral_frame.gyro_y_rate_integral) / 10000.0f; - r.gyro_rate_integral_xyz_radians[2] = static_cast(pdata->integral_frame.gyro_z_rate_integral) / 10000.0f; - r.integration_time_usec = pdata->integral_frame.integration_timespan; - //FIXME: r.max_axis_velocity_radians_sec - r.samples_matched_pct = pdata->integral_frame.qual; - r.gyro_temperature_celsius = pdata->integral_frame.gyro_temperature / 100.0f; - //FIXME: r.gyro_rate_integral_xyz_covariance _flow_pulisher.broadcast(r); return PX4_OK; @@ -332,16 +319,6 @@ UavcanNode::teardown() return 0; } - -/* - * App entry point - */ -static void print_usage() -{ - PX4_INFO("usage: \n" - "\tuavcannode {start|status|stop|arm|disarm}"); -} - static int uavcannode_start() { board_app_initialize(); @@ -382,69 +359,77 @@ TODO(Need non vol Paramter sotrage) return UavcanNode::start(node_id, bitrate); } -__EXPORT int uavcannode_publish_flow(legacy_i2c_data_t *pdata) -{ - - UavcanNode *const inst = UavcanNode::instance(); +static result_accumulator_ctx accumulator; - if (!inst) { - PX4_ERR( "application not running"); - return 1; - } - return inst->publish(pdata); +__EXPORT void fmu_comm_init() { + result_accumulator_init(&accumulator); + if (!UavcanNode::instance()) { + uavcannode_start(); + } } -__EXPORT int uavcannode_publish_range(range_data_t *pdata) -{ +__EXPORT void fmu_comm_run() { + UavcanNode *const inst = UavcanNode::instance(); - UavcanNode *const inst = UavcanNode::instance(); + if (!inst) { + PX4_ERR( "application not running"); + return; + } - if (!inst) { - PX4_ERR( "application not running"); - return 1; - } - return inst->publish(pdata); + inst->run(); } -__EXPORT int uavcannode_run() -{ - UavcanNode *const inst = UavcanNode::instance(); - - if (!inst) { - PX4_ERR( "application not running"); - return 1; - } - return inst->run(); - -} +__EXPORT void fmu_comm_update(float 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) { + /* feed the accumulator and recalculate */ + result_accumulator_feed(&accumulator, 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); -int uavcannode_main(bool start_not_stop) -{ - if (start_not_stop) { + if (accumulator.frame_count % 32 == 0) { + UavcanNode *const inst = UavcanNode::instance(); - if (UavcanNode::instance()) { - PX4_ERR("already started"); - return 1; + if (!inst) { + PX4_ERR( "UAVCAN not running"); + return; } - return uavcannode_start(); - } - - /* commands below require the app to be started */ - UavcanNode *const inst = UavcanNode::instance(); - - if (!inst) { - PX4_ERR( "application not running"); - return 1; - } - - - if (!start_not_stop) { - inst->stop(); - return 0; + uint64_t timestamp = hrt_absolute_time(); + + result_accumulator_output_flow_rad flow_rad; + result_accumulator_calculate_output_flow_rad(&accumulator, global_data.param[PARAM_ALGORITHM_MIN_VALID_RATIO], &flow_rad); + ::threedr::equipment::flow::optical_flow::RawSample r; + r.timestamp.usec = timestamp; + r.flow_integral_xy_radians[0] = flow_rad.integrated_x; + r.flow_integral_xy_radians[1] = flow_rad.integrated_y; + r.gyro_rate_integral_xyz_radians[0] = flow_rad.integrated_xgyro; + r.gyro_rate_integral_xyz_radians[1] = flow_rad.integrated_ygyro; + r.gyro_rate_integral_xyz_radians[2] = flow_rad.integrated_zgyro; + r.integration_time_usec = flow_rad.integration_time; + //FIXME: r.max_axis_velocity_radians_sec + r.samples_matched_pct = flow_rad.quality; + r.gyro_temperature_celsius = flow_rad.temperature / 100.0f; + //FIXME: r.gyro_rate_integral_xyz_covariance + inst->publish(r); + + ::uavcan::equipment::range_sensor::Measurement m; + m.timestamp.usec = timestamp; + m.sensor_id = 0; + m.beam_orientation_in_body_frame.fixed_axis_roll_pitch_yaw[0] = 0 * m.beam_orientation_in_body_frame.ANGLE_MULTIPLIER; + m.beam_orientation_in_body_frame.fixed_axis_roll_pitch_yaw[1] = 0 * m.beam_orientation_in_body_frame.ANGLE_MULTIPLIER; + m.beam_orientation_in_body_frame.fixed_axis_roll_pitch_yaw[2] = 0 * m.beam_orientation_in_body_frame.ANGLE_MULTIPLIER; + m.beam_orientation_in_body_frame.orientation_defined = true; + m.field_of_view = 0; + m.sensor_type = ::uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_LIDAR; + m.reading_type = (accumulator.last.ground_distance >= 0) + ? ::uavcan::equipment::range_sensor::Measurement::READING_TYPE_VALID_RANGE + : ::uavcan::equipment::range_sensor::Measurement::READING_TYPE_UNDEFINED; + m.range = accumulator.last.ground_distance; + inst->publish(m); + + result_accumulator_reset(&accumulator); } - - print_usage(); - return 1; } + diff --git a/src/modules/uavcannode/uavcannode_main.hpp b/src/modules/uavcannode/uavcannode_main.hpp index 479c9a2..cdea8e6 100644 --- a/src/modules/uavcannode/uavcannode_main.hpp +++ b/src/modules/uavcannode/uavcannode_main.hpp @@ -41,8 +41,6 @@ #include #include -#include "uavcan_if.h" - /** * @file uavcan_main.hpp * @@ -103,9 +101,9 @@ class UavcanNode static UavcanNode *instance() { return _instance; } - int run(); - int publish(legacy_i2c_data_t *pdata); - int publish(range_data_t *pdata); + int run(); + int publish(::threedr::equipment::flow::optical_flow::RawSample&); + int publish(uavcan::equipment::range_sensor::Measurement&); /* The bit rate that can be passed back to the bootloader */ From d04c5e7c4f07e3062b288bd501d0064c7d1b9182 Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Mon, 21 Sep 2015 15:17:41 -0700 Subject: [PATCH 092/120] uavcan: fix typos --- src/modules/uavcannode/uavcannode_main.cpp | 12 ++++++------ src/modules/uavcannode/uavcannode_main.hpp | 6 +++--- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/modules/uavcannode/uavcannode_main.cpp b/src/modules/uavcannode/uavcannode_main.cpp index ad74ae2..46c85e4 100644 --- a/src/modules/uavcannode/uavcannode_main.cpp +++ b/src/modules/uavcannode/uavcannode_main.cpp @@ -102,10 +102,10 @@ UavcanNode *UavcanNode::_instance; UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : active_bitrate(0), _node(can_driver, system_clock), - _fw_update_listner(_node), + _fw_update_listener(_node), _time_sync_slave(_node), - _flow_pulisher(_node), - _range_pulisher(_node), + _flow_publisher(_node), + _range_publisher(_node), _reset_timer(_node) { @@ -246,7 +246,7 @@ int UavcanNode::init(uavcan::NodeID node_id) fill_node_info(); - const int srv_start_res = _fw_update_listner.start(BeginFirmwareUpdateCallBack(this, + const int srv_start_res = _fw_update_listener.start(BeginFirmwareUpdateCallBack(this, &UavcanNode::cb_beginfirmware_update)); if (srv_start_res < 0) { @@ -274,13 +274,13 @@ class RestartRequestHandler: public uavcan::IRestartRequestHandler int UavcanNode::publish(::uavcan::equipment::range_sensor::Measurement &m) { - _range_pulisher.broadcast(m); + _range_publisher.broadcast(m); return PX4_OK; } int UavcanNode::publish(::threedr::equipment::flow::optical_flow::RawSample &r) { - _flow_pulisher.broadcast(r); + _flow_publisher.broadcast(r); return PX4_OK; } diff --git a/src/modules/uavcannode/uavcannode_main.hpp b/src/modules/uavcannode/uavcannode_main.hpp index cdea8e6..92ba97a 100644 --- a/src/modules/uavcannode/uavcannode_main.hpp +++ b/src/modules/uavcannode/uavcannode_main.hpp @@ -125,10 +125,10 @@ class UavcanNode uavcan::ServiceResponseDataStructure &)> BeginFirmwareUpdateCallBack; - uavcan::ServiceServer _fw_update_listner; + uavcan::ServiceServer _fw_update_listener; uavcan::GlobalTimeSyncSlave _time_sync_slave; - uavcan::Publisher< ::threedr::equipment::flow::optical_flow::RawSample> _flow_pulisher; - uavcan::Publisher _range_pulisher; + uavcan::Publisher< ::threedr::equipment::flow::optical_flow::RawSample> _flow_publisher; + uavcan::Publisher _range_publisher; void cb_beginfirmware_update(const uavcan::ReceivedDataStructure &req, uavcan::ServiceResponseDataStructure &rsp); From 286063af72048d0bd1925b464e62211634f5579e Mon Sep 17 00:00:00 2001 From: Tom Date: Tue, 22 Sep 2015 11:38:18 -0700 Subject: [PATCH 093/120] Changes to optical_flow.RawSample: fix comments, nuke covariances --- .../threedr/equipment/flow/optical_flow/1110.RawSample.uavcan | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/modules/uavcannode/dsdl/uavcan/threedr/equipment/flow/optical_flow/1110.RawSample.uavcan b/src/modules/uavcannode/dsdl/uavcan/threedr/equipment/flow/optical_flow/1110.RawSample.uavcan index 7387aba..eec44ce 100644 --- a/src/modules/uavcannode/dsdl/uavcan/threedr/equipment/flow/optical_flow/1110.RawSample.uavcan +++ b/src/modules/uavcannode/dsdl/uavcan/threedr/equipment/flow/optical_flow/1110.RawSample.uavcan @@ -9,9 +9,7 @@ float16[3] gyro_rate_integral_xyz_radians uint24 integration_time_usec # The maximum velocity (rad/s) for which the flow data can be trusted float16 max_axis_velocity_radians_sec -# The percentage (0 to 1) of tiles for which the optical flow feature tracker could find a displacement +# The percentage (scaled from 0 to 255) of tiles for which the optical flow feature tracker could find a displacement uint8 samples_matched_pct # Gyro temperature float16 gyro_temperature_celsius -# Gyro covariance matrix -float16[<=9] gyro_rate_integral_xyz_covariance # Radians^2 / Radians From 0f9e158fad4dac7edeade9e68911919473602b13 Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Thu, 24 Sep 2015 14:37:14 -0700 Subject: [PATCH 094/120] get_boot_time_us: check if there is a preempted systick interrupt pending Reported by Simon Laube: The usage of get_boot_time_us() in the camera interrupt handler caused issues where sometimes it would return a timestamp that is 1ms in the past. The result is that some frames deltas would be reported as 1ms too short while the next frame after such an incident would be 1ms too long. --- src/modules/flow/timer.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/flow/timer.c b/src/modules/flow/timer.c index fa64fdb..ff89990 100644 --- a/src/modules/flow/timer.c +++ b/src/modules/flow/timer.c @@ -187,6 +187,7 @@ uint32_t get_boot_time_us(void) do { // read the value: val_us_base = boot_time_us_base; + if (SCB->ICSR & SCB_ICSR_PENDSTSET_Msk) val_us_base += 1000; val_tick = SysTick->VAL; // make sure it did not roll over in the mean-time: } while(SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk); From 8a82ff273e2294e3b94cf480aae33e92808d1adc Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Thu, 24 Sep 2015 14:40:20 -0700 Subject: [PATCH 095/120] Revert "mt9v034: write reserved registers with recommended values from Rev G datasheet" This reverts commit 44c414775562496f5eca1b9aeb3bf7739e0aeac7. These register changes make it more succeptible to the full-frame corruption. --- src/modules/flow/mt9v034.c | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/src/modules/flow/mt9v034.c b/src/modules/flow/mt9v034.c index ccf7d7d..2f01393 100644 --- a/src/modules/flow/mt9v034.c +++ b/src/modules/flow/mt9v034.c @@ -434,16 +434,6 @@ static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx, bool initial) { */ ctx->pixel_frame_line_ctrl_reg = 0x0000; mt9v034_WriteReg(MTV_PIXEL_FRAME_LINE_CTRL_REG, ctx->pixel_frame_line_ctrl_reg); - - - // Write reserved registers per Rev G datasheet table 8 recommendations - mt9v034_WriteReg(0x13, 0x2D2E); - mt9v034_WriteReg(0x20, 0x03C7); - mt9v034_WriteReg(0x24, 0x001B); - mt9v034_WriteReg(0x2B, 0x0003); - mt9v034_WriteReg(0x2F, 0x0003); - - } /* settings that are the same for both contexts: */ From 38d8b3b707790902740dd0ed0f0fff7dc7aff3ad Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Mon, 28 Sep 2015 11:03:20 -0700 Subject: [PATCH 096/120] mt9v034: Refactor to reduce duplicate code --- src/modules/flow/mt9v034.c | 213 ++++++++++--------------------------- 1 file changed, 54 insertions(+), 159 deletions(-) diff --git a/src/modules/flow/mt9v034.c b/src/modules/flow/mt9v034.c index 2f01393..f37d4d6 100644 --- a/src/modules/flow/mt9v034.c +++ b/src/modules/flow/mt9v034.c @@ -127,8 +127,20 @@ const camera_sensor_interface *mt9v034_get_sensor_interface() { return &mt9v034_sensor_interface; } -uint32_t mt9v034_get_clks_per_row(uint16_t width, int binning) { - width = width * binning; +static uint16_t mt9v034_hor_blanking(uint16_t raw_width, int binning) { + /* + * Horizontal Blanking + * Number of blank columns in a row. + * Minimum horizontal blanking is 61 for normal mode, + * 71 for column bin 2 mode, and 91 for column bin 4 mode. + * + * The MT9V034 uses column parallel analog-digital converters, thus short row timing is not possible. + * The minimum total row time is 690 columns (horizontal width + horizontal blanking). The minimum + * horizontal blanking is 61( / 71 / 91). When the window width is set below 627, horizontal blanking + * must be increased. + * + * In old code for bin 4 and 64 pixels it was 350 + 91. + */ uint16_t hor_blanking = 61; /* init to minimum: */ switch (binning) { @@ -137,9 +149,15 @@ uint32_t mt9v034_get_clks_per_row(uint16_t width, int binning) { case 4: hor_blanking = 91; break; } /* increase if window is smaller: */ - if (width < 690 - hor_blanking) { - hor_blanking = 690 - width; + if (raw_width < 690 - hor_blanking) { + hor_blanking = 690 - raw_width; } + return hor_blanking; +} + +uint32_t mt9v034_get_clks_per_row(uint16_t width, int binning) { + width = width * binning; + uint16_t hor_blanking = mt9v034_hor_blanking(width, binning); return width + hor_blanking; } @@ -199,50 +217,10 @@ bool mt9v034_prepare_update_param(void *usr, const camera_img_param_ex *img_para return true; } -bool mt9v034_update_exposure_param(void *usr, uint32_t exposure, float gain) { - mt9v034_sensor_ctx *ctx = (mt9v034_sensor_ctx *)usr; - /* check whether i2c is in use: */ - if (ctx->seq_i2c_in_use) { - return false; - } - - int context_idx; - if (ctx->do_switch_context) { - context_idx = ctx->desired_context; - } else { - context_idx = ctx->cur_context; - } +static bool mt9v034_configure_exposure_param(mt9v034_sensor_ctx *ctx, int context_idx, uint32_t exposure, float gain, bool full_refresh) { camera_img_param_ex *ctx_param = &ctx->context_p[context_idx]; - /* image dimensions */ - uint16_t width = ctx_param->p.size.x * ctx_param->p.binning; - - /* - * Horizontal Blanking - * Number of blank columns in a row. - * Minimum horizontal blanking is 61 for normal mode, - * 71 for column bin 2 mode, and 91 for column bin 4 mode. - * - * The MT9V034 uses column parallel analog-digital converters, thus short row timing is not possible. - * The minimum total row time is 690 columns (horizontal width + horizontal blanking). The minimum - * horizontal blanking is 61( / 71 / 91). When the window width is set below 627, horizontal blanking - * must be increased. - * - * In old code for bin 4 and 64 pixels it was 350 + 91. - */ - uint16_t hor_blanking = 61; - /* init to minimum: */ - switch (ctx_param->p.binning) { - case 1: hor_blanking = 61; break; - case 2: hor_blanking = 71; break; - case 4: hor_blanking = 91; break; - } - /* increase if window is smaller: */ - if (width < 690 - hor_blanking) { - hor_blanking = 690 - width; - } - - uint16_t row_time = hor_blanking + width; + uint16_t row_time = mt9v034_get_clks_per_row(ctx_param->p.size.x, ctx_param->p.binning); /* * Coarse Shutter Width Total @@ -269,7 +247,7 @@ bool mt9v034_update_exposure_param(void *usr, uint32_t exposure, float gain) { } else if (analog_gain > 64) { analog_gain = 64; } - bool update_gain = ((uint16_t)(ctx_param->analog_gain * 16 + 0.05f) != analog_gain); + bool update_gain = ((uint16_t)(ctx_param->analog_gain * 16 + 0.5f) != analog_gain); float real_analog_gain = analog_gain * 0.0625f; uint16_t sw_for_blanking = coarse_sw_total; @@ -293,27 +271,27 @@ bool mt9v034_update_exposure_param(void *usr, uint32_t exposure, float gain) { switch (context_idx) { case 0: - if (update_exposure || DEBUG_TEST_WORSTCASE_LATENCY) { + if (full_refresh || update_exposure || DEBUG_TEST_WORSTCASE_LATENCY) { mt9v034_WriteReg(MTV_COARSE_SW_TOTAL_REG_A, coarse_sw_total); mt9v034_WriteReg(MTV_FINE_SW_TOTAL_REG_A, fine_sw_total); } - if (update_gain || DEBUG_TEST_WORSTCASE_LATENCY) { + if (full_refresh || update_gain || DEBUG_TEST_WORSTCASE_LATENCY) { mt9v034_WriteReg(MTV_ANALOG_GAIN_CTRL_REG_A, analog_gain); } - if (ver_blanking != ctx->ver_blnk_ctx_a_reg || DEBUG_TEST_WORSTCASE_LATENCY) { + if (full_refresh || ver_blanking != ctx->ver_blnk_ctx_a_reg || DEBUG_TEST_WORSTCASE_LATENCY) { ctx->ver_blnk_ctx_a_reg = ver_blanking; mt9v034_WriteReg(MTV_VER_BLANKING_REG_A, ctx->ver_blnk_ctx_a_reg); } break; case 1: - if (update_exposure || DEBUG_TEST_WORSTCASE_LATENCY) { + if (full_refresh || update_exposure || DEBUG_TEST_WORSTCASE_LATENCY) { mt9v034_WriteReg(MTV_COARSE_SW_TOTAL_REG_B, coarse_sw_total); mt9v034_WriteReg(MTV_FINE_SW_TOTAL_REG_B, fine_sw_total); } - if (update_gain || DEBUG_TEST_WORSTCASE_LATENCY) { + if (full_refresh || update_gain || DEBUG_TEST_WORSTCASE_LATENCY) { mt9v034_WriteReg(MTV_ANALOG_GAIN_CTRL_REG_B, analog_gain); } - if (ver_blanking != ctx->ver_blnk_ctx_b_reg || DEBUG_TEST_WORSTCASE_LATENCY) { + if (full_refresh || ver_blanking != ctx->ver_blnk_ctx_b_reg || DEBUG_TEST_WORSTCASE_LATENCY) { ctx->ver_blnk_ctx_b_reg = ver_blanking; mt9v034_WriteReg(MTV_VER_BLANKING_REG_B, ctx->ver_blnk_ctx_b_reg); } @@ -327,6 +305,23 @@ bool mt9v034_update_exposure_param(void *usr, uint32_t exposure, float gain) { return true; } +bool mt9v034_update_exposure_param(void *usr, uint32_t exposure, float gain) { + mt9v034_sensor_ctx *ctx = (mt9v034_sensor_ctx *)usr; + /* check whether i2c is in use: */ + if (ctx->seq_i2c_in_use) { + return false; + } + + int context_idx; + if (ctx->do_switch_context) { + context_idx = ctx->desired_context; + } else { + context_idx = ctx->cur_context; + } + + return mt9v034_configure_exposure_param(ctx, context_idx, exposure, gain, false); +} + void mt9v034_restore_previous_param(void *usr) { mt9v034_sensor_ctx *ctx = (mt9v034_sensor_ctx *)usr; /* switch back to previous context: */ @@ -579,8 +574,6 @@ static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx, bool initial) { static bool mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, const camera_img_param_ex *img_param, bool full_refresh) { bool update_size = full_refresh; bool update_binning = full_refresh; - bool update_exposure= full_refresh; - bool update_gain = full_refresh; if (context_idx < 0 || context_idx >= 2) return false; camera_img_param_ex *ctx_param = &ctx->context_p[context_idx]; /* check which sets of parameters we need to update: */ @@ -602,84 +595,8 @@ static bool mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, uint16_t col_start = 1 + (752 - width) / 2; uint16_t row_start = 4 + (480 - height) / 2; - - /* - * Horizontal Blanking - * Number of blank columns in a row. - * Minimum horizontal blanking is 61 for normal mode, - * 71 for column bin 2 mode, and 91 for column bin 4 mode. - * - * The MT9V034 uses column parallel analog-digital converters, thus short row timing is not possible. - * The minimum total row time is 690 columns (horizontal width + horizontal blanking). The minimum - * horizontal blanking is 61( / 71 / 91). When the window width is set below 627, horizontal blanking - * must be increased. - * - * In old code for bin 4 and 64 pixels it was 350 + 91. - */ - uint16_t hor_blanking = 61; - /* init to minimum: */ - switch (img_param->p.binning) { - case 1: hor_blanking = 61; break; - case 2: hor_blanking = 71; break; - case 4: hor_blanking = 91; break; - } - /* increase if window is smaller: */ - if (width < 690 - hor_blanking) { - hor_blanking = 690 - width; - } - - uint16_t row_time = hor_blanking + width; - - /* - * Coarse Shutter Width Total - * Total integration time in number of rows. This value is used only when AEC is disabled. - * Default: 0x01E0 - */ - uint16_t coarse_sw_total = img_param->exposure / row_time; - uint16_t fine_sw_total = img_param->exposure % row_time; - /* ensure minimum: */ - if (coarse_sw_total < 1 && fine_sw_total < 260) { - fine_sw_total = 260; - } - uint32_t real_exposure = coarse_sw_total * row_time + fine_sw_total; - if (ctx_param->exposure != real_exposure) { - update_exposure = true; - } - /** - * Analog Gain - * [6:0] Analog Gain: Range 16 - 64 - * [15] Force 0.75X: 0 = Disabled. - */ - uint16_t analog_gain = img_param->analog_gain * 16 + 0.5f; - /* limit: */ - if (analog_gain < 16) { - analog_gain = 16; - } else if (analog_gain > 64) { - analog_gain = 64; - } - float real_analog_gain = analog_gain * 0.0625f; - if ((uint16_t)(ctx_param->analog_gain * 16 + 0.05f) != analog_gain) { - update_gain = true; - } - - uint16_t sw_for_blanking = coarse_sw_total; - if (sw_for_blanking < img_param->p.size.y) { - sw_for_blanking = img_param->p.size.y; - } - /* - * Vertical Blanking - * Number of blank rows in a frame. V-Blank value must meet the following minimums: - * Linear Mode: - * V-Blank (min) = SW_total - R0x08 + 7 - * R0x08 (Coarse Shutter Width 1) - * If manual exposure then SW_total = R0x0B. (Coarse Shutter Width Total) - * If auto-exposure mode then SW_total = R0xAD. (Maximum Coarse Shutter Width) - * - * If Auto-Knee Point enabled, then V-Blank (min) = (t2 + t3 + 7). - * t2 = Tint_tot * (1/2) ^ T2 Ratio -> 4 - * t3 = Tint_tot * (1/2) ^ T3 Ratio -> 1 - */ - uint16_t ver_blanking = (sw_for_blanking >> 4) + (sw_for_blanking >> 6) + 7; + + uint16_t hor_blanking = mt9v034_hor_blanking(width, img_param->p.binning); /* * Read Mode @@ -709,17 +626,6 @@ static bool mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, mt9v034_WriteReg(MTV_COLUMN_START_REG_A, col_start); mt9v034_WriteReg(MTV_ROW_START_REG_A, row_start); } - if (ver_blanking != ctx->ver_blnk_ctx_a_reg || full_refresh || DEBUG_TEST_WORSTCASE_LATENCY) { - ctx->ver_blnk_ctx_a_reg = ver_blanking; - mt9v034_WriteReg(MTV_VER_BLANKING_REG_A, ctx->ver_blnk_ctx_a_reg); - } - if (update_exposure || DEBUG_TEST_WORSTCASE_LATENCY) { - mt9v034_WriteReg(MTV_COARSE_SW_TOTAL_REG_A, coarse_sw_total); - mt9v034_WriteReg(MTV_FINE_SW_TOTAL_REG_A, fine_sw_total); - } - if (update_gain || DEBUG_TEST_WORSTCASE_LATENCY) { - mt9v034_WriteReg(MTV_ANALOG_GAIN_CTRL_REG_A, analog_gain); - } break; case 1: if (update_size || update_binning || DEBUG_TEST_WORSTCASE_LATENCY) { @@ -730,25 +636,14 @@ static bool mt9v034_configure_context(mt9v034_sensor_ctx *ctx, int context_idx, mt9v034_WriteReg(MTV_COLUMN_START_REG_B, col_start); mt9v034_WriteReg(MTV_ROW_START_REG_B, row_start); } - if (ver_blanking != ctx->ver_blnk_ctx_b_reg || full_refresh || DEBUG_TEST_WORSTCASE_LATENCY) { - ctx->ver_blnk_ctx_b_reg = ver_blanking; - mt9v034_WriteReg(MTV_VER_BLANKING_REG_B, ctx->ver_blnk_ctx_b_reg); - } - if (update_exposure || DEBUG_TEST_WORSTCASE_LATENCY) { - mt9v034_WriteReg(MTV_COARSE_SW_TOTAL_REG_B, coarse_sw_total); - mt9v034_WriteReg(MTV_FINE_SW_TOTAL_REG_B, fine_sw_total); - } - if (update_gain || DEBUG_TEST_WORSTCASE_LATENCY) { - mt9v034_WriteReg(MTV_ANALOG_GAIN_CTRL_REG_B, analog_gain); - } break; } - + /* update the current settings: */ - *ctx_param = *img_param; - ctx_param->exposure = real_exposure; - ctx_param->analog_gain = real_analog_gain; + ctx_param->p = img_param->p; + mt9v034_configure_exposure_param(ctx, context_idx, img_param->exposure, img_param->analog_gain, full_refresh); + return true; } From 070d84a65d676bc2229eed45005776ed9b31852a Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Tue, 29 Sep 2015 11:24:13 -0700 Subject: [PATCH 097/120] lidarlite: mark sample invalid and reset i2c on timeout --- src/modules/lidar/lidar.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/lidar/lidar.c b/src/modules/lidar/lidar.c index a7c909d..c50d8d3 100644 --- a/src/modules/lidar/lidar.c +++ b/src/modules/lidar/lidar.c @@ -175,20 +175,12 @@ static bool i2c_read(uint8_t address, uint8_t *data, uint8_t length) /* Data from the LIDAR */ float sample, sample_filter; -uint8_t swver = 0, hwver = 0; uint32_t measure_time = 0; bool sample_valid = false; __EXPORT void distance_init(void) { i2c_init_master(); - uint8_t out[1] = { REG_HWVER }; - uint8_t in[1] = { 0 }; - if(i2c_write(LIDAR_ADDRESS, out, 1) && i2c_read(LIDAR_ADDRESS, in, 1)) - hwver = in[0]; - out[0] = REG_SWVER; in[0] = 0; - if(i2c_write(LIDAR_ADDRESS, out, 1) && i2c_read(LIDAR_ADDRESS, in, 1)) - swver = in[0]; /* Run interrupt-based drivers after config */ I2C_ITConfig(I2C1, I2C_IT_EVT, ENABLE); @@ -222,6 +214,13 @@ __EXPORT void distance_trigger(void) { if(!i2c_started) return; + + if (ld_state != IDLE) { + // Previous reading did not complete + distance_init(); + sample_valid = false; + } + ld_state = WRITE; ld_reg = REG_CONTROL; ld_buffer[0] = BITS_ACQUIRE_WITH_CORRECTION; @@ -332,6 +331,7 @@ __EXPORT void I2C1_ER_IRQHandler(void) { I2C1->SR1 &= 0x00FF; ld_state = IDLE; } + sample_valid = false; } static void lidar_process(void) From 748bf1eadac292c5669424e610148344e0c52625 Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Wed, 30 Sep 2015 13:34:46 -0700 Subject: [PATCH 098/120] Refactor result_accumulator_feed --- src/include/fmu_comm.h | 5 +- src/include/result_accumulator.h | 53 ++++-------- src/modules/flow/main.c | 34 ++++---- src/modules/flow/result_accumulator.c | 96 +++++++++------------- src/modules/i2c_slave/i2c_slave.c | 8 +- src/modules/uavcannode/uavcannode_main.cpp | 12 +-- 6 files changed, 86 insertions(+), 122 deletions(-) diff --git a/src/include/fmu_comm.h b/src/include/fmu_comm.h index 22fffcc..9999f19 100644 --- a/src/include/fmu_comm.h +++ b/src/include/fmu_comm.h @@ -36,6 +36,7 @@ /// Abstraction over the FMU communications interface -- I2C slave or UAVCAN #include +#include "result_accumulator.h" /// Call on boot to initialize void fmu_comm_init(void); @@ -44,9 +45,7 @@ void fmu_comm_init(void); void fmu_comm_run(void); /// Call every frame to update with new data -void fmu_comm_update(float 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); +void fmu_comm_update(const result_accumulator_frame* frame); #endif diff --git a/src/include/result_accumulator.h b/src/include/result_accumulator.h index 7012200..d7c9499 100644 --- a/src/include/result_accumulator.h +++ b/src/include/result_accumulator.h @@ -38,25 +38,21 @@ #include #include "i2c_frame.h" +typedef struct _result_accumulator_frame { + float dt; ///< The time delta of 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) + int16_t gyro_temp; ///< Temperature * 100 in centi-degrees Celsius + uint8_t qual; ///< The quality output of the flow algorithm. + float rad_per_pixel; ///< Pixel to radian conversion factor. + float pixel_flow_x; ///< The measured x-flow in the current image in pixel. Sensor linear motion along the positive X axis induces a negative flow. + float pixel_flow_y; ///< The measured y-flow in the current image in pixel. Sensor linear motion along the positive Y axis induces a negative flow. + float ground_distance; ///< The measured distance to the ground in meter. + uint32_t distance_age; ///< Age of the distance measurement in us. +} result_accumulator_frame; typedef struct _result_accumulator_ctx { - struct _last { - uint32_t frame_count; ///< Frame counter. - float dt; ///< The time delta of 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) - int16_t temperature; ///< Temperature * 100 in centi-degrees Celsius - uint8_t qual; ///< The quality output of the flow algorithm. - float pixel_flow_x; ///< The measured x-flow in the current image in pixel. Sensor linear motion along the positive X axis induces a negative flow. - float pixel_flow_y; ///< The measured y-flow in the current image in pixel. Sensor linear motion along the positive Y axis induces a negative flow. - float flow_x_rad; ///< Flow in radians around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) - float flow_y_rad; ///< Flow in radians around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) - float flow_x_m; ///< The measured x-flow in the current image in meters. - float flow_y_m; ///< The measured x-flow in the current image in meters. - 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; @@ -73,6 +69,9 @@ typedef struct _result_accumulator_ctx { float gyro_x_accu; float gyro_y_accu; float gyro_z_accu; + int16_t last_gyro_temp; ///< Temperature * 100 in centi-degrees Celsius + float last_ground_distance; ///< The measured distance to the ground in meter. + uint32_t last_distance_age; ///< Age of the distance measurement in us. } result_accumulator_ctx; @@ -120,25 +119,7 @@ typedef struct _result_accumulator_output_flow_i2c { /** Initializes the result accumulator. */ 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 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) - * @param gyro_temp The current gyro temperature in centi-degrees Celsius - * @param qual The quality output of the flow algorithm. - * @param pixel_flow_x The measured x-flow in the current image in pixel. - * @param pixel_flow_y The measured y-flow in the current image in pixel. - * @param rad_per_pixel Pixel to radian conversion factor. - * @param distance_valid True when the distance sensor reports that its distance is valid. - * @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, - 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); +void result_accumulator_feed(result_accumulator_ctx *ctx, const result_accumulator_frame* frame); /** Recalculates the output values of the result_accumulator. Call this before using any of the output values. */ diff --git a/src/modules/flow/main.c b/src/modules/flow/main.c index 4493cdf..40df8a8 100644 --- a/src/modules/flow/main.c +++ b/src/modules/flow/main.c @@ -109,7 +109,7 @@ static camera_img_param img_stream_param; typedef struct __attribute__((packed)) { uint64_t timestamp; uint32_t exposure; - uint32_t ground_distance_mm; + int32_t ground_distance_mm; uint8_t snapshot_buffer_mem[USB_IMAGE_PIXELS * USB_IMAGE_PIXELS]; } usb_packet_format; @@ -350,12 +350,12 @@ int main(void) distance_valid = distance_read(&distance_filtered, &distance_raw); /* reset to zero for invalid distances */ if (!distance_valid) { - distance_filtered = 0.0f; - distance_raw = 0.0f; + distance_filtered = -1.0f; + distance_raw = -1.0f; } /* decide which distance to use */ - float ground_distance = 0.0f; + float ground_distance; if(FLOAT_AS_BOOL(global_data.param[PARAM_SONAR_FILTERED])) { ground_distance = distance_filtered; @@ -499,18 +499,24 @@ int main(void) /* return the image buffers */ camera_img_stream_return_buffers(&cam_ctx, frames, 2); + + result_accumulator_frame frame_data = { + .dt = frame_dt, + .x_rate = x_rate, + .y_rate = y_rate, + .z_rate = z_rate, + .gyro_temp = gyro_temp, + .qual = qual, + .pixel_flow_x = pixel_flow_x, + .pixel_flow_y = pixel_flow_y, + .rad_per_pixel = 1.0f / focal_length_px, + .ground_distance = ground_distance, + .distance_age = get_time_delta_us(get_distance_measure_time()) + }; /* update I2C transmit buffer */ - fmu_comm_update(frame_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_distance_measure_time())); - - /* accumulate the results */ - result_accumulator_feed(&mavlink_accumulator, frame_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_distance_measure_time())); + fmu_comm_update(&frame_data); + result_accumulator_feed(&mavlink_accumulator, &frame_data); uint32_t computaiton_time_us = get_time_delta_us(start_computations); diff --git a/src/modules/flow/result_accumulator.c b/src/modules/flow/result_accumulator.c index e475edc..d4d1d42 100644 --- a/src/modules/flow/result_accumulator.c +++ b/src/modules/flow/result_accumulator.c @@ -48,60 +48,46 @@ void result_accumulator_init(result_accumulator_ctx *ctx) memset(ctx, 0, sizeof(result_accumulator_ctx)); /* set non-zero initial values */ ctx->min_quality = 255; - ctx->last.ground_distance = -1; + 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, - 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.x_rate = x_rate; - ctx->last.y_rate = y_rate; - ctx->last.z_rate = z_rate; - ctx->last.temperature = gyro_temp; - ctx->last.qual = qual; - ctx->last.pixel_flow_x = pixel_flow_x; - ctx->last.pixel_flow_y = pixel_flow_y; - ctx->last.flow_x_rad = pixel_flow_y * rad_per_pixel; - ctx->last.flow_y_rad = - pixel_flow_x * rad_per_pixel; - ctx->last.ground_distance = distance_valid ? ground_distance : -1; - ctx->last.distance_age = distance_age; - if(ctx->last.ground_distance >= 0) { - ctx->last.flow_x_m = ctx->last.flow_x_rad * ctx->last.ground_distance; - ctx->last.flow_y_m = ctx->last.flow_y_rad * ctx->last.ground_distance; - }else{ - ctx->last.flow_x_m = 0; - ctx->last.flow_y_m = 0; - } +void result_accumulator_feed(result_accumulator_ctx *ctx, const result_accumulator_frame* frame) +{ + ctx->last_ground_distance = frame->ground_distance; + ctx->last_distance_age = frame->distance_age; + ctx->last_gyro_temp = frame->gyro_temp; + /* accumulate the values */ - if (qual > 0) { - ctx->px_flow_x_accu += ctx->last.pixel_flow_x; - ctx->px_flow_y_accu += ctx->last.pixel_flow_y; - ctx->rad_flow_x_accu += ctx->last.flow_x_rad; - ctx->rad_flow_y_accu += ctx->last.flow_y_rad; - - ctx->gyro_x_accu += x_rate * dt; - ctx->gyro_y_accu += y_rate * dt; - ctx->gyro_z_accu += z_rate * dt; + if (frame->qual > 0) { + ctx->px_flow_x_accu += frame->pixel_flow_x; + ctx->px_flow_y_accu += frame->pixel_flow_y; + + float flow_x_rad = frame->pixel_flow_y * frame->rad_per_pixel; + float flow_y_rad = - frame->pixel_flow_x * frame->rad_per_pixel; + ctx->rad_flow_x_accu += flow_x_rad; + ctx->rad_flow_y_accu += flow_y_rad; + + ctx->gyro_x_accu += frame->x_rate * frame->dt; + ctx->gyro_y_accu += frame->y_rate * frame->dt; + ctx->gyro_z_accu += frame->z_rate * frame->dt; + /* the quality is the minimum of all quality measurements */ - if (qual < ctx->min_quality || ctx->valid_data_count == 0) { - ctx->min_quality = qual; + if (frame->qual < ctx->min_quality || ctx->valid_data_count == 0) { + ctx->min_quality = frame->qual; } + ctx->valid_data_count++; - ctx->valid_time += dt; - if(ctx->last.ground_distance >= 0){ - ctx->m_flow_x_accu += ctx->last.flow_x_m; - ctx->m_flow_y_accu += ctx->last.flow_y_m; - ctx->valid_dist_time += dt; + ctx->valid_time += frame->dt; + + if(ctx->last_ground_distance >= 0){ + ctx->m_flow_x_accu += flow_x_rad * ctx->last_ground_distance; + ctx->m_flow_y_accu += flow_y_rad * ctx->last_ground_distance; + ctx->valid_dist_time += frame->dt; } } ctx->data_count++; ctx->frame_count++; - ctx->full_time += dt; - - ctx->last.frame_count = ctx->frame_count; + ctx->full_time += frame->dt; } void result_accumulator_calculate_output_flow(result_accumulator_ctx *ctx, uint16_t min_valid_data_count_percent, result_accumulator_output_flow *out) @@ -130,7 +116,7 @@ void result_accumulator_calculate_output_flow(result_accumulator_ctx *ctx, uint1 out->quality = 0; } /* averaging the distance is no use */ - out->ground_distance = ctx->last.ground_distance; + out->ground_distance = ctx->last_ground_distance; } void result_accumulator_calculate_output_flow_rad(result_accumulator_ctx *ctx, uint16_t min_valid_data_count_percent, result_accumulator_output_flow_rad *out) @@ -145,7 +131,7 @@ void result_accumulator_calculate_output_flow_rad(result_accumulator_ctx *ctx, u out->integrated_zgyro = ctx->gyro_z_accu; out->quality = ctx->min_quality; /* averaging the distance and temperature is no use */ - out->temperature = ctx->last.temperature; + out->temperature = ctx->last_gyro_temp; } else { /* not enough valid data */ out->integration_time = 0; @@ -155,10 +141,10 @@ void result_accumulator_calculate_output_flow_rad(result_accumulator_ctx *ctx, u out->integrated_ygyro = 0; out->integrated_zgyro = 0; out->quality = 0; - out->temperature = ctx->last.temperature; + out->temperature = ctx->last_gyro_temp; } - out->time_delta_distance_us = ctx->last.distance_age; - out->ground_distance = ctx->last.ground_distance; + out->time_delta_distance_us = ctx->last_distance_age; + out->ground_distance = ctx->last_ground_distance; } void result_accumulator_calculate_output_flow_i2c(result_accumulator_ctx *ctx, uint16_t min_valid_data_count_percent, result_accumulator_output_flow_i2c *out) @@ -177,7 +163,7 @@ void result_accumulator_calculate_output_flow_i2c(result_accumulator_ctx *ctx, u out->gyro_y = floor(ctx->gyro_y_accu * (100.0f / ctx->valid_time) + 0.5f); out->gyro_z = floor(ctx->gyro_z_accu * (100.0f / ctx->valid_time) + 0.5f); out->quality = ctx->min_quality; - out->temperature = ctx->last.temperature; + out->temperature = ctx->last_gyro_temp; } else { /* not enough valid data */ out->frame_count = ctx->frame_count; @@ -192,11 +178,11 @@ void result_accumulator_calculate_output_flow_i2c(result_accumulator_ctx *ctx, u out->gyro_y = 0; out->gyro_z = 0; out->quality = 0; - out->temperature = ctx->last.temperature; + out->temperature = ctx->last_gyro_temp; } - out->time_delta_distance_us = ctx->last.distance_age; - if (ctx->last.ground_distance >= 0) { - out->ground_distance = floor(ctx->last.ground_distance * 1000.0f + 0.5f); + out->time_delta_distance_us = ctx->last_distance_age; + if (ctx->last_ground_distance >= 0) { + out->ground_distance = floor(ctx->last_ground_distance * 1000.0f + 0.5f); } else { out->ground_distance = 0; } @@ -211,7 +197,7 @@ void result_accumulator_fill_i2c_data(result_accumulator_ctx *ctx, i2c_frame* f, result_accumulator_calculate_output_flow_i2c(ctx, min_valid_ratio, &output_i2c); /* write the i2c_frame */ - f->frame_count = ctx->last.frame_count; + f->frame_count = ctx->frame_count; f->pixel_flow_x_sum = output_flow.flow_x; f->pixel_flow_y_sum = output_flow.flow_y; f->flow_comp_m_x = floor(output_flow.flow_comp_m_x * 1000.0f + 0.5f); diff --git a/src/modules/i2c_slave/i2c_slave.c b/src/modules/i2c_slave/i2c_slave.c index c46ce7d..d6fa4b5 100644 --- a/src/modules/i2c_slave/i2c_slave.c +++ b/src/modules/i2c_slave/i2c_slave.c @@ -232,9 +232,7 @@ void I2C1_ER_IRQHandler(void) { } -__EXPORT void fmu_comm_update(float 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) { +__EXPORT void fmu_comm_update(const result_accumulator_frame* frame_data) { static result_accumulator_ctx accumulator; static bool initialized = false; @@ -250,9 +248,7 @@ __EXPORT void fmu_comm_update(float dt, float x_rate, float y_rate, float z_rate } /* feed the accumulator and recalculate */ - result_accumulator_feed(&accumulator, 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); + result_accumulator_feed(&accumulator, frame_data); i2c_frame f; i2c_integral_frame f_integral; diff --git a/src/modules/uavcannode/uavcannode_main.cpp b/src/modules/uavcannode/uavcannode_main.cpp index 46c85e4..826188a 100644 --- a/src/modules/uavcannode/uavcannode_main.cpp +++ b/src/modules/uavcannode/uavcannode_main.cpp @@ -379,14 +379,10 @@ __EXPORT void fmu_comm_run() { inst->run(); } -__EXPORT void fmu_comm_update(float 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) { +__EXPORT void fmu_comm_update(const result_accumulator_frame* frame_data) { /* feed the accumulator and recalculate */ - result_accumulator_feed(&accumulator, 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); + result_accumulator_feed(&accumulator, frame_data); if (accumulator.frame_count % 32 == 0) { UavcanNode *const inst = UavcanNode::instance(); @@ -423,10 +419,10 @@ __EXPORT void fmu_comm_update(float dt, float x_rate, float y_rate, float z_rate m.beam_orientation_in_body_frame.orientation_defined = true; m.field_of_view = 0; m.sensor_type = ::uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_LIDAR; - m.reading_type = (accumulator.last.ground_distance >= 0) + m.reading_type = (accumulator.last_ground_distance >= 0) ? ::uavcan::equipment::range_sensor::Measurement::READING_TYPE_VALID_RANGE : ::uavcan::equipment::range_sensor::Measurement::READING_TYPE_UNDEFINED; - m.range = accumulator.last.ground_distance; + m.range = accumulator.last_ground_distance; inst->publish(m); result_accumulator_reset(&accumulator); From 59c05f38fbbbea2cb144492f32cd72ab7dc7e305 Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Wed, 30 Sep 2015 15:28:50 -0700 Subject: [PATCH 099/120] Change result accumulator to handle dropped frame time correctly This only affects the legacy data in pixels / accumulation period. Based on 04f365d by Simon Laube The full_time variable of the accumulator did not include the time of the dropped frames. This is fixed now. Accumulator calculations relying on the full_time variable are now correct event when frames are dropped. (for example when taking a snapshot) The full_time variable is currently only used by the old flow message which uses pixels as the output format. It is used to scale the measured pixel values to appear like we did not drop any frames. This scaling is correct if we assume that the speed remains constant during the accumulation period. --- src/include/result_accumulator.h | 1 + src/modules/flow/main.c | 9 +++++++-- src/modules/flow/result_accumulator.c | 2 +- 3 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/include/result_accumulator.h b/src/include/result_accumulator.h index d7c9499..b895ecc 100644 --- a/src/include/result_accumulator.h +++ b/src/include/result_accumulator.h @@ -40,6 +40,7 @@ typedef struct _result_accumulator_frame { 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) diff --git a/src/modules/flow/main.c b/src/modules/flow/main.c index 40df8a8..9e483c1 100644 --- a/src/modules/flow/main.c +++ b/src/modules/flow/main.c @@ -339,6 +339,8 @@ int main(void) uint32_t last_frame_index = 0; + uint32_t last_processed_frame_timestamp = get_boot_time_us(); + /* main loop */ while (1) { @@ -447,8 +449,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); @@ -502,6 +506,7 @@ int main(void) result_accumulator_frame frame_data = { .dt = frame_dt, + .dropped_dt = dropped_dt, .x_rate = x_rate, .y_rate = y_rate, .z_rate = z_rate, diff --git a/src/modules/flow/result_accumulator.c b/src/modules/flow/result_accumulator.c index d4d1d42..66950d1 100644 --- a/src/modules/flow/result_accumulator.c +++ b/src/modules/flow/result_accumulator.c @@ -87,7 +87,7 @@ void result_accumulator_feed(result_accumulator_ctx *ctx, const result_accumulat } ctx->data_count++; ctx->frame_count++; - ctx->full_time += frame->dt; + ctx->full_time += frame->dt + frame->dropped_dt; } void result_accumulator_calculate_output_flow(result_accumulator_ctx *ctx, uint16_t min_valid_data_count_percent, result_accumulator_output_flow *out) From 50f460fd9af2c6522bf6e7356496a027235a98ea Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Mon, 31 Aug 2015 13:45:14 +0200 Subject: [PATCH 100/120] Implement maximum velocity calculation algorithm. --- src/include/flow.h | 17 +++++++++++ src/include/result_accumulator.h | 7 ++++- src/modules/flow/flow.c | 14 +++++++++ src/modules/flow/main.c | 42 +++++++++++++++++++-------- src/modules/flow/result_accumulator.c | 27 +++++++++++++++++ 5 files changed, 94 insertions(+), 13 deletions(-) diff --git a/src/include/flow.h b/src/include/flow.h index 258fc0c..d62aa12 100644 --- a/src/include/flow.h +++ b/src/include/flow.h @@ -52,6 +52,11 @@ typedef struct _flow_klt_image { uint32_t meta; } flow_klt_image; +typedef struct _flow_capability { + float max_x_px_frame; + float max_y_px_frame; +} flow_capability; + /** * @brief Computes pixel flow from image1 to image2 * Searches the corresponding position in the new image (image2) of max. 64 pixels from the old image (image1). @@ -67,6 +72,12 @@ typedef struct _flow_klt_image { uint16_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate, flow_raw_result *out, uint16_t max_out); +/* + * Returns the capability of the flow algorithm. + * @param cap Receives the capability. + */ +void get_flow_capability(flow_capability *cap); + /** * Preprocesses the image for use with compute_klt. * This will add the pyramid levels. @@ -91,6 +102,12 @@ void klt_preprocess_image(uint8_t *image, flow_klt_image *klt_image); uint16_t compute_klt(flow_klt_image *image1, flow_klt_image *image2, float x_rate, float y_rate, float z_rate, flow_raw_result *out, uint16_t max_out); +/* + * Returns the capability of the KLT algorithm. + * @param cap Receives the capability. + */ +void get_flow_klt_capability(flow_capability *cap); + /** * * @brief Extracts pixel flow from the result vector diff --git a/src/include/result_accumulator.h b/src/include/result_accumulator.h index b895ecc..2babcaf 100644 --- a/src/include/result_accumulator.h +++ b/src/include/result_accumulator.h @@ -37,6 +37,7 @@ #include #include #include "i2c_frame.h" +#include "flow.h" typedef struct _result_accumulator_frame { float dt; ///< The time delta of this sample. @@ -51,6 +52,7 @@ typedef struct _result_accumulator_frame { float pixel_flow_y; ///< The measured y-flow in the current image in pixel. Sensor linear motion along the positive Y axis induces a negative flow. float ground_distance; ///< The measured distance to the ground in meter. uint32_t distance_age; ///< Age of the distance measurement in us. + flow_capability flow_cap; } result_accumulator_frame; typedef struct _result_accumulator_ctx { @@ -73,9 +75,12 @@ typedef struct _result_accumulator_ctx { int16_t last_gyro_temp; ///< Temperature * 100 in centi-degrees Celsius float last_ground_distance; ///< The measured distance to the ground in meter. uint32_t last_distance_age; ///< Age of the distance measurement in us. + float flow_cap_mvx_rad; /**< The maximum velocity that could be measured by all datasets together in one accumulation period. + * This is the minimum of all max velocities. In rad / s. */ + float flow_cap_mvy_rad; /**< The maximum velocity that could be measured by all datasets together in one accumulation period. + * This is the minimum of all max velocities. In rad / s. */ } result_accumulator_ctx; - typedef struct _result_accumulator_output_flow { int16_t flow_x; ///< Flow in pixels * 10 in x-sensor direction (dezi-pixels) int16_t flow_y; ///< Flow in pixels * 10 in y-sensor direction (dezi-pixels) diff --git a/src/modules/flow/flow.c b/src/modules/flow/flow.c index be3d831..9c9e307 100644 --- a/src/modules/flow/flow.c +++ b/src/modules/flow/flow.c @@ -390,6 +390,13 @@ static inline uint32_t compute_sad_8x8(uint8_t *image1, uint8_t *image2, uint16_ return acc; } +void get_flow_capability(flow_capability *cap) { + const uint16_t search_size = SEARCH_SIZE; + + cap->max_x_px_frame = search_size + 0.5; + cap->max_y_px_frame = search_size + 0.5; +} + uint16_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate, flow_raw_result *out, uint16_t max_out) { @@ -539,6 +546,13 @@ void klt_preprocess_image(uint8_t *image, flow_klt_image *klt_image) { } } +void get_flow_klt_capability(flow_capability *cap) { + uint16_t topPyrStep = 1 << (PYR_LVLS - 1); + + cap->max_x_px_frame = topPyrStep * 2; + cap->max_y_px_frame = topPyrStep * 2; +} + uint16_t compute_klt(flow_klt_image *image1, flow_klt_image *image2, float x_rate, float y_rate, float z_rate, flow_raw_result *out, uint16_t max_out) { diff --git a/src/modules/flow/main.c b/src/modules/flow/main.c index 9e483c1..230a65e 100644 --- a/src/modules/flow/main.c +++ b/src/modules/flow/main.c @@ -377,9 +377,6 @@ int main(void) } } - /* calculate focal_length in pixel */ - const float focal_length_px = (global_data.param[PARAM_FOCAL_LENGTH_MM]) / (4.0f * 0.006f); //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled - /* new gyroscope data */ float x_rate_sensor, y_rate_sensor, z_rate_sensor; int16_t gyro_temp; @@ -452,19 +449,39 @@ int main(void) 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); - float z_rate_fr = - z_rate * frame_dt; - /* compute optical flow in pixels */ + /* calculate focal_length in pixel */ + const float focal_length_px = (global_data.param[PARAM_FOCAL_LENGTH_MM]) / + ((float)frames[0]->param.p.binning * 0.006f); // pixel-size: 6um + + /* extract the raw flow from the images: */ flow_raw_result flow_rslt[32]; uint16_t flow_rslt_count = 0; + /* make sure both images are taken with same binning mode: */ + if (frames[0]->param.p.binning == frames[1]->param.p.binning) { + /* 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); + float z_rate_fr = - z_rate * frame_dt; + + /* compute optical flow in pixels */ + if (!use_klt) { + flow_rslt_count = compute_flow(frames[1]->buffer, frames[0]->buffer, x_rate_px, y_rate_px, z_rate_fr, + flow_rslt, sizeof(flow_rslt) / sizeof(flow_rslt[0])); + } else { + flow_rslt_count = compute_klt(klt_images[1], klt_images[0], x_rate_px, y_rate_px, z_rate_fr, + flow_rslt, sizeof(flow_rslt) / sizeof(flow_rslt[0])); + } + } else { + /* no result for this frame. */ + flow_rslt_count = 0; + } + /* determine capability: */ + flow_capability flow_rslt_cap; if (!use_klt) { - flow_rslt_count = compute_flow(frames[1]->buffer, frames[0]->buffer, x_rate_px, y_rate_px, z_rate_fr, flow_rslt, 32); + get_flow_capability(&flow_rslt_cap); } else { - flow_rslt_count = compute_klt(klt_images[1], klt_images[0], x_rate_px, y_rate_px, z_rate_fr, flow_rslt, 32); + get_flow_klt_capability(&flow_rslt_cap); } /* calculate flow value from the raw results */ @@ -516,7 +533,8 @@ int main(void) .pixel_flow_y = pixel_flow_y, .rad_per_pixel = 1.0f / focal_length_px, .ground_distance = ground_distance, - .distance_age = get_time_delta_us(get_distance_measure_time()) + .distance_age = get_time_delta_us(get_distance_measure_time()), + .flow_cap = flow_rslt_cap, }; /* update I2C transmit buffer */ diff --git a/src/modules/flow/result_accumulator.c b/src/modules/flow/result_accumulator.c index 66950d1..65cd1b9 100644 --- a/src/modules/flow/result_accumulator.c +++ b/src/modules/flow/result_accumulator.c @@ -62,6 +62,15 @@ void result_accumulator_feed(result_accumulator_ctx *ctx, const result_accumulat ctx->px_flow_x_accu += frame->pixel_flow_x; ctx->px_flow_y_accu += frame->pixel_flow_y; + /* Convert the flow from pixels to rad: + * This is done with a linear relationship because the involved angles are low. + * + * Calculation for maximum possible error: (for an assumed drone configuration) + * With a 10Hz accumulation period and 5m/s at 1m distance the real speed in rad is 0.463rad per accumulation period. + * Each frame (taken at 400Hz) measures a distance of 4.16px (at 8mm focal length, 4x binning -> 3mm/px). + * With the linear formula this equates to 0.0125rad per frame. + * Accumulated value in rad is thus 40 * 0.0125 = 5rad. Error: 8%. + * At half the speed the error is just 2%. */ float flow_x_rad = frame->pixel_flow_y * frame->rad_per_pixel; float flow_y_rad = - frame->pixel_flow_x * frame->rad_per_pixel; ctx->rad_flow_x_accu += flow_x_rad; @@ -85,6 +94,24 @@ void result_accumulator_feed(result_accumulator_ctx *ctx, const result_accumulat ctx->valid_dist_time += frame->dt; } } + + /* convert the algorithm's capability into a velocity in rad / s: */ + float flow_cap_mvx_rad = frame->flow_cap.max_y_px_frame * frame->rad_per_pixel / frame->dt; + float flow_cap_mvy_rad = frame->flow_cap.max_x_px_frame * frame->rad_per_pixel / frame->dt; + + if (ctx->data_count > 0) { + /* accumulate the minimum value: */ + if (flow_cap_mvx_rad < ctx->flow_cap_mvx_rad) { + ctx->flow_cap_mvx_rad = flow_cap_mvx_rad; + } + if (flow_cap_mvy_rad < ctx->flow_cap_mvy_rad) { + ctx->flow_cap_mvy_rad = flow_cap_mvy_rad; + } + } else { + ctx->flow_cap_mvx_rad = flow_cap_mvx_rad; + ctx->flow_cap_mvy_rad = flow_cap_mvy_rad; + } + ctx->data_count++; ctx->frame_count++; ctx->full_time += frame->dt + frame->dropped_dt; From 26b2bfbb0acdb2fb22391c1fc3db8f8faf185db6 Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Wed, 30 Sep 2015 16:03:43 -0700 Subject: [PATCH 101/120] Make velocity calculation 1-dimensional, and pass it over UAVCAN The UAVCAN message and APM can't deal with per-axis velocity limits. The X and Y values were always the same. --- src/include/flow.h | 11 ++--------- src/include/result_accumulator.h | 9 +++------ src/modules/flow/flow.c | 12 ++++-------- src/modules/flow/main.c | 10 +++++----- src/modules/flow/result_accumulator.c | 14 ++++---------- src/modules/uavcannode/uavcannode_main.cpp | 3 +-- 6 files changed, 19 insertions(+), 40 deletions(-) diff --git a/src/include/flow.h b/src/include/flow.h index d62aa12..dfb8f54 100644 --- a/src/include/flow.h +++ b/src/include/flow.h @@ -52,11 +52,6 @@ typedef struct _flow_klt_image { uint32_t meta; } flow_klt_image; -typedef struct _flow_capability { - float max_x_px_frame; - float max_y_px_frame; -} flow_capability; - /** * @brief Computes pixel flow from image1 to image2 * Searches the corresponding position in the new image (image2) of max. 64 pixels from the old image (image1). @@ -74,9 +69,8 @@ uint16_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_ra /* * Returns the capability of the flow algorithm. - * @param cap Receives the capability. */ -void get_flow_capability(flow_capability *cap); +float get_flow_capability(void); /** * Preprocesses the image for use with compute_klt. @@ -104,9 +98,8 @@ uint16_t compute_klt(flow_klt_image *image1, flow_klt_image *image2, float x_rat /* * Returns the capability of the KLT algorithm. - * @param cap Receives the capability. */ -void get_flow_klt_capability(flow_capability *cap); +float get_flow_klt_capability(void); /** * diff --git a/src/include/result_accumulator.h b/src/include/result_accumulator.h index 2babcaf..001e16b 100644 --- a/src/include/result_accumulator.h +++ b/src/include/result_accumulator.h @@ -37,7 +37,6 @@ #include #include #include "i2c_frame.h" -#include "flow.h" typedef struct _result_accumulator_frame { float dt; ///< The time delta of this sample. @@ -52,7 +51,7 @@ typedef struct _result_accumulator_frame { float pixel_flow_y; ///< The measured y-flow in the current image in pixel. Sensor linear motion along the positive Y axis induces a negative flow. float ground_distance; ///< The measured distance to the ground in meter. uint32_t distance_age; ///< Age of the distance measurement in us. - flow_capability flow_cap; + float max_px_frame; ///< Max velocity, in pixels per frame. } result_accumulator_frame; typedef struct _result_accumulator_ctx { @@ -75,10 +74,8 @@ typedef struct _result_accumulator_ctx { int16_t last_gyro_temp; ///< Temperature * 100 in centi-degrees Celsius float last_ground_distance; ///< The measured distance to the ground in meter. uint32_t last_distance_age; ///< Age of the distance measurement in us. - float flow_cap_mvx_rad; /**< The maximum velocity that could be measured by all datasets together in one accumulation period. - * This is the minimum of all max velocities. In rad / s. */ - float flow_cap_mvy_rad; /**< The maximum velocity that could be measured by all datasets together in one accumulation period. - * This is the minimum of all max velocities. In rad / s. */ + float flow_cap_mv_rad; /**< The maximum velocity that could be measured by all datasets together in one accumulation period. + * This is the minimum of all max velocities. In rad / s. */ } result_accumulator_ctx; typedef struct _result_accumulator_output_flow { diff --git a/src/modules/flow/flow.c b/src/modules/flow/flow.c index 9c9e307..f7c7c57 100644 --- a/src/modules/flow/flow.c +++ b/src/modules/flow/flow.c @@ -390,11 +390,9 @@ static inline uint32_t compute_sad_8x8(uint8_t *image1, uint8_t *image2, uint16_ return acc; } -void get_flow_capability(flow_capability *cap) { +float get_flow_capability() { const uint16_t search_size = SEARCH_SIZE; - - cap->max_x_px_frame = search_size + 0.5; - cap->max_y_px_frame = search_size + 0.5; + return search_size + 0.5; } uint16_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate, @@ -546,11 +544,9 @@ void klt_preprocess_image(uint8_t *image, flow_klt_image *klt_image) { } } -void get_flow_klt_capability(flow_capability *cap) { +float get_flow_klt_capability() { uint16_t topPyrStep = 1 << (PYR_LVLS - 1); - - cap->max_x_px_frame = topPyrStep * 2; - cap->max_y_px_frame = topPyrStep * 2; + return topPyrStep * 2; } uint16_t compute_klt(flow_klt_image *image1, flow_klt_image *image2, float x_rate, float y_rate, float z_rate, diff --git a/src/modules/flow/main.c b/src/modules/flow/main.c index 230a65e..94cab16 100644 --- a/src/modules/flow/main.c +++ b/src/modules/flow/main.c @@ -476,12 +476,12 @@ int main(void) /* no result for this frame. */ flow_rslt_count = 0; } - /* determine capability: */ - flow_capability flow_rslt_cap; + /* determine velocity capability: */ + float flow_mv_cap; if (!use_klt) { - get_flow_capability(&flow_rslt_cap); + flow_mv_cap = get_flow_capability(); } else { - get_flow_klt_capability(&flow_rslt_cap); + flow_mv_cap = get_flow_klt_capability(); } /* calculate flow value from the raw results */ @@ -534,7 +534,7 @@ int main(void) .rad_per_pixel = 1.0f / focal_length_px, .ground_distance = ground_distance, .distance_age = get_time_delta_us(get_distance_measure_time()), - .flow_cap = flow_rslt_cap, + .max_px_frame = flow_mv_cap, }; /* update I2C transmit buffer */ diff --git a/src/modules/flow/result_accumulator.c b/src/modules/flow/result_accumulator.c index 65cd1b9..3e8049f 100644 --- a/src/modules/flow/result_accumulator.c +++ b/src/modules/flow/result_accumulator.c @@ -96,20 +96,14 @@ void result_accumulator_feed(result_accumulator_ctx *ctx, const result_accumulat } /* convert the algorithm's capability into a velocity in rad / s: */ - float flow_cap_mvx_rad = frame->flow_cap.max_y_px_frame * frame->rad_per_pixel / frame->dt; - float flow_cap_mvy_rad = frame->flow_cap.max_x_px_frame * frame->rad_per_pixel / frame->dt; + float flow_cap_mv_rad = frame->max_px_frame * frame->rad_per_pixel / frame->dt; if (ctx->data_count > 0) { - /* accumulate the minimum value: */ - if (flow_cap_mvx_rad < ctx->flow_cap_mvx_rad) { - ctx->flow_cap_mvx_rad = flow_cap_mvx_rad; - } - if (flow_cap_mvy_rad < ctx->flow_cap_mvy_rad) { - ctx->flow_cap_mvy_rad = flow_cap_mvy_rad; + if (flow_cap_mv_rad < ctx->flow_cap_mv_rad) { + ctx->flow_cap_mv_rad = flow_cap_mv_rad; } } else { - ctx->flow_cap_mvx_rad = flow_cap_mvx_rad; - ctx->flow_cap_mvy_rad = flow_cap_mvy_rad; + ctx->flow_cap_mv_rad = flow_cap_mv_rad; } ctx->data_count++; diff --git a/src/modules/uavcannode/uavcannode_main.cpp b/src/modules/uavcannode/uavcannode_main.cpp index 826188a..a65dac5 100644 --- a/src/modules/uavcannode/uavcannode_main.cpp +++ b/src/modules/uavcannode/uavcannode_main.cpp @@ -404,10 +404,9 @@ __EXPORT void fmu_comm_update(const result_accumulator_frame* frame_data) { r.gyro_rate_integral_xyz_radians[1] = flow_rad.integrated_ygyro; r.gyro_rate_integral_xyz_radians[2] = flow_rad.integrated_zgyro; r.integration_time_usec = flow_rad.integration_time; - //FIXME: r.max_axis_velocity_radians_sec + r.max_axis_velocity_radians_sec = accumulator.flow_cap_mv_rad; r.samples_matched_pct = flow_rad.quality; r.gyro_temperature_celsius = flow_rad.temperature / 100.0f; - //FIXME: r.gyro_rate_integral_xyz_covariance inst->publish(r); ::uavcan::equipment::range_sensor::Measurement m; From 3b32e83ae73c7037d0df307e5b8340bcd58e4789 Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Thu, 1 Oct 2015 14:45:35 -0700 Subject: [PATCH 102/120] Don't send invalid ground distance over USB --- src/modules/flow/main.c | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/flow/main.c b/src/modules/flow/main.c index 94cab16..063d1a8 100644 --- a/src/modules/flow/main.c +++ b/src/modules/flow/main.c @@ -109,7 +109,7 @@ static camera_img_param img_stream_param; typedef struct __attribute__((packed)) { uint64_t timestamp; uint32_t exposure; - int32_t ground_distance_mm; + uint32_t ground_distance_mm; uint8_t snapshot_buffer_mem[USB_IMAGE_PIXELS * USB_IMAGE_PIXELS]; } usb_packet_format; @@ -229,7 +229,11 @@ static void start_send_image(float ground_distance_m) { usb_packet.timestamp = 0; #endif usb_packet.exposure = snapshot_buffer.param.exposure; - usb_packet.ground_distance_mm = ground_distance_m * 1000.0f; + if (ground_distance_m > 0) { + usb_packet.ground_distance_mm = ground_distance_m * 1000.0f; + } else { + usb_packet.ground_distance_mm = 0; + } DCD_EP_Flush(&USB_OTG_dev, IMAGE_IN_EP); send_image_step(); } From 90640a8d4366d4ef0c548fe42da3e52e2c5c9d01 Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Thu, 8 Oct 2015 15:54:44 -0700 Subject: [PATCH 103/120] i2c_slave: Test individual interrupt-pending bits. Otherwise when multiple interrupts are pending, it fails to clear any, and gets stuck in the ISR. --- src/modules/i2c_slave/i2c_slave.c | 47 +++++++++---------------------- 1 file changed, 13 insertions(+), 34 deletions(-) diff --git a/src/modules/i2c_slave/i2c_slave.c b/src/modules/i2c_slave/i2c_slave.c index d6fa4b5..bbaa93e 100644 --- a/src/modules/i2c_slave/i2c_slave.c +++ b/src/modules/i2c_slave/i2c_slave.c @@ -149,20 +149,14 @@ void I2C1_EV_IRQHandler(void) { static uint8_t txDataIndex1 = 0x00; static uint8_t txDataIndex2 = 0x00; static uint8_t rxDataIndex = 0x00; - switch (I2C_GetLastEvent(I2C1 )) { - - case I2C_EVENT_SLAVE_RECEIVER_ADDRESS_MATCHED : { - I2C1 ->SR1; - I2C1 ->SR2; - rxDataIndex = 0; - break; - } - case I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED : { - I2C1 ->SR1; - I2C1 ->SR2; - break; - } - case I2C_EVENT_SLAVE_BYTE_RECEIVED : { + + if (I2C_GetITStatus(I2C1, I2C_IT_ADDR) == SET) { + uint8_t sr2 = I2C1->SR2; + if (sr2 & I2C_SR2_TRA) { + // Write + rxDataIndex = 0; + } + } else if (I2C_GetITStatus(I2C1, I2C_IT_RXNE)) { //receive address offset dataRX = I2C_ReceiveData(I2C1 ); rxDataIndex++; @@ -177,11 +171,7 @@ void I2C1_EV_IRQHandler(void) { //indicate sending readout_done_frame1 = 0; readout_done_frame2 = 0; - break; - } - case I2C_EVENT_SLAVE_BYTE_TRANSMITTING : - case I2C_EVENT_SLAVE_BYTE_TRANSMITTED : { - + } else if (I2C_GetITStatus(I2C1, I2C_IT_TXE)) { if (txDataIndex1 < (I2C_FRAME_SIZE)) { I2C_SendData(I2C1, txDataFrame1[publishedIndexFrame1][txDataIndex1]); @@ -192,7 +182,6 @@ void I2C1_EV_IRQHandler(void) { if (txDataIndex2 < I2C_INTEGRAL_FRAME_SIZE) { txDataIndex2++; } - } //check whether last byte is read frame1 @@ -205,20 +194,10 @@ void I2C1_EV_IRQHandler(void) { readout_done_frame2 = 1; stop_accumulation = 1; } - - break; - } - - case I2C_EVENT_SLAVE_ACK_FAILURE : { - I2C1 ->SR1 &= 0x00FF; - break; - } - - case I2C_EVENT_SLAVE_STOP_DETECTED : { - I2C1 ->SR1; - I2C1 ->CR1 |= 0x1; - break; - } + } else if (I2C_GetITStatus(I2C1, I2C_IT_AF)) { + I2C_ClearITPendingBit(I2C1, I2C_IT_AF); + } else if (I2C_GetITStatus(I2C1, I2C_IT_STOPF)) { + I2C_Cmd(I2C1, ENABLE); } } From 11776e874cef8be5bd690b66cf06c757b2a40958 Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Fri, 9 Oct 2015 14:59:14 -0700 Subject: [PATCH 104/120] Rename lidar -> lidar-lite --- makefiles/baremetal/config_px4flow-v2_default.mk | 2 +- src/modules/{lidar => lidar-lite}/lidar.c | 0 src/modules/{lidar => lidar-lite}/module.mk | 0 3 files changed, 1 insertion(+), 1 deletion(-) rename src/modules/{lidar => lidar-lite}/lidar.c (100%) rename src/modules/{lidar => lidar-lite}/module.mk (100%) diff --git a/makefiles/baremetal/config_px4flow-v2_default.mk b/makefiles/baremetal/config_px4flow-v2_default.mk index 6229b98..7e280b4 100644 --- a/makefiles/baremetal/config_px4flow-v2_default.mk +++ b/makefiles/baremetal/config_px4flow-v2_default.mk @@ -41,7 +41,7 @@ MODULES += modules/libc # Flow MODULES += modules/flow -MODULES += modules/lidar +MODULES += modules/lidar-lite # Generate parameter XML file #GEN_PARAM_XML = 1 diff --git a/src/modules/lidar/lidar.c b/src/modules/lidar-lite/lidar.c similarity index 100% rename from src/modules/lidar/lidar.c rename to src/modules/lidar-lite/lidar.c diff --git a/src/modules/lidar/module.mk b/src/modules/lidar-lite/module.mk similarity index 100% rename from src/modules/lidar/module.mk rename to src/modules/lidar-lite/module.mk From e478c63b5ea0ca62981c4dc23ffe0f05b9137c2a Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Fri, 9 Oct 2015 17:12:42 -0700 Subject: [PATCH 105/120] Add Lightware SF10 Lidar driver --- .../baremetal/config_px4flow-v2_default.mk | 2 +- src/modules/lidar-sf10/lidar.c | 188 ++++++++++++++++++ src/modules/lidar-sf10/module.mk | 5 + 3 files changed, 194 insertions(+), 1 deletion(-) create mode 100644 src/modules/lidar-sf10/lidar.c create mode 100644 src/modules/lidar-sf10/module.mk diff --git a/makefiles/baremetal/config_px4flow-v2_default.mk b/makefiles/baremetal/config_px4flow-v2_default.mk index 7e280b4..0aec58d 100644 --- a/makefiles/baremetal/config_px4flow-v2_default.mk +++ b/makefiles/baremetal/config_px4flow-v2_default.mk @@ -41,7 +41,7 @@ MODULES += modules/libc # Flow MODULES += modules/flow -MODULES += modules/lidar-lite +MODULES += modules/lidar-sf10 # Generate parameter XML file #GEN_PARAM_XML = 1 diff --git a/src/modules/lidar-sf10/lidar.c b/src/modules/lidar-sf10/lidar.c new file mode 100644 index 0000000..c4d7c0c --- /dev/null +++ b/src/modules/lidar-sf10/lidar.c @@ -0,0 +1,188 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include "distance.h" +#include "usbd_cdc_vcp.h" +#include "main.h" +#include "distance_mode_filter.h" + +#include "stm32f4xx.h" +#include "stm32f4xx_i2c.h" +#include "stm32f4xx_rcc.h" +#include "stm32f4xx_gpio.h" +#include "misc.h" + +//Stated range of sensor +#define MINIMUM_DISTANCE 0.0f +#define MAXIMUM_DISTANCE 60.0f + +#define LIDAR_ADDRESS 0x55 + +bool i2c_started = false; + +static void i2c_init_master(void) +{ + //Reset I2C + I2C_DeInit(I2C1); + I2C_SoftwareResetCmd(I2C1, ENABLE); + I2C_SoftwareResetCmd(I2C1, DISABLE); + + //Turn on clocks + RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE); + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); + + //Route I2C out to B8 (SCL) and B9 (SDA) + GPIO_InitTypeDef gpio_init; + gpio_init.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_9; + gpio_init.GPIO_Mode = GPIO_Mode_AF; + gpio_init.GPIO_Speed = GPIO_Speed_50MHz; + gpio_init.GPIO_PuPd = GPIO_PuPd_UP; + gpio_init.GPIO_OType = GPIO_OType_OD; + GPIO_Init(GPIOB, &gpio_init); + GPIO_PinAFConfig(GPIOB, GPIO_PinSource8, GPIO_AF_I2C1); //SCL + GPIO_PinAFConfig(GPIOB, GPIO_PinSource9, GPIO_AF_I2C1); //SDA + + //NVIC + NVIC_InitTypeDef nvic_init; + nvic_init.NVIC_IRQChannel = I2C1_EV_IRQn; + nvic_init.NVIC_IRQChannelPreemptionPriority = 0; + nvic_init.NVIC_IRQChannelSubPriority = 0; + nvic_init.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&nvic_init); + nvic_init.NVIC_IRQChannel = I2C1_ER_IRQn; + NVIC_Init(&nvic_init); + + //I2C block setup + I2C_InitTypeDef i2c_init; + i2c_init.I2C_ClockSpeed = 100000; + i2c_init.I2C_Mode = I2C_Mode_I2C; + i2c_init.I2C_DutyCycle = I2C_DutyCycle_2; + i2c_init.I2C_OwnAddress1 = 0x0; + i2c_init.I2C_Ack = I2C_Ack_Disable; + i2c_init.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; + I2C_Init(I2C1, &i2c_init); + I2C_Cmd(I2C1, ENABLE); + + i2c_started = true; +} + +/* Data from the LIDAR */ + +float sample, sample_filter; +uint32_t measure_time = 0; +bool sample_valid = false; + +__EXPORT void distance_init(void) +{ + i2c_init_master(); + + /* Run interrupt-based drivers after config */ + I2C_ITConfig(I2C1, I2C_IT_EVT, ENABLE); + I2C_ITConfig(I2C1, I2C_IT_ERR, ENABLE); +} + + +__EXPORT void distance_trigger(void) +{ + +} + +__EXPORT void distance_readback(void) +{ + if(!i2c_started) + return; + I2C_GenerateSTART(I2C1, ENABLE); +} + +static void lidar_process(uint16_t value); +void I2C1_EV_IRQHandler(void); +void I2C1_ER_IRQHandler(void); + +__EXPORT void I2C1_EV_IRQHandler(void) +{ + if (I2C_GetITStatus(I2C1, I2C_IT_SB)) { + I2C_Send7bitAddress(I2C1, LIDAR_ADDRESS << 1, I2C_Direction_Receiver); + } else if (I2C_GetITStatus(I2C1, I2C_IT_ADDR)) { + I2C_AcknowledgeConfig(I2C1, ENABLE); + I2C_GenerateSTOP(I2C1, DISABLE); + (void) I2C1->SR2; + } else if (I2C_GetITStatus(I2C1, I2C_IT_BTF)) { + I2C_AcknowledgeConfig(I2C1, DISABLE); + I2C_GenerateSTOP(I2C1, ENABLE); + + uint16_t value = I2C_ReceiveData(I2C1); + value |= I2C_ReceiveData(I2C1) << 8; + lidar_process(value); + } +} + +__EXPORT void I2C1_ER_IRQHandler(void) { + if((I2C_ReadRegister(I2C1, I2C_Register_SR1) & 0xFF00) != 0x00) { + I2C_GenerateSTOP(I2C1, ENABLE); + I2C1->SR1 &= 0x00FF; + } + sample_valid = false; +} + +static void lidar_process(uint16_t sample16) +{ + if(sample16 <= 0) { // 0 or less means invalid data + sample_valid = false; + return; + } + sample16 = ((sample16 >> 8) & 0xFF) | ((sample16 << 8) & 0xFF00); + sample = (sample16 / 100.0f); /* convert cm->m */ + if(sample < MINIMUM_DISTANCE || sample > MAXIMUM_DISTANCE) { + sample_valid = false; + return; + } + sample_filter = insert_distance_value_and_get_mode_value(sample); + measure_time = get_boot_time_us(); + sample_valid = true; +} + +__EXPORT bool distance_read(float* distance_filtered, float* distance_raw) +{ + if(sample_valid) { + *distance_raw = sample; + *distance_filtered = sample_filter; + return true; + } + return false; +} + +__EXPORT uint32_t get_distance_measure_time(void) +{ + return measure_time; +} diff --git a/src/modules/lidar-sf10/module.mk b/src/modules/lidar-sf10/module.mk new file mode 100644 index 0000000..3984c4b --- /dev/null +++ b/src/modules/lidar-sf10/module.mk @@ -0,0 +1,5 @@ +DEFAULT_VISIBILITY=default + +SRCS += lidar.c + +$(info SRCS=$(SRCS)) From 8d4fe40b99ce679b4cc89ddd1bc3d4e6892e892c Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Mon, 21 Sep 2015 11:46:18 -0700 Subject: [PATCH 106/120] mt9v034: write reserved registers with recommended values from Rev G datasheet This eliminates the noise seen on every 4th row --- src/modules/flow/mt9v034.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/modules/flow/mt9v034.c b/src/modules/flow/mt9v034.c index f37d4d6..0c8d777 100644 --- a/src/modules/flow/mt9v034.c +++ b/src/modules/flow/mt9v034.c @@ -429,6 +429,13 @@ static void mt9v034_configure_general(mt9v034_sensor_ctx *ctx, bool initial) { */ ctx->pixel_frame_line_ctrl_reg = 0x0000; mt9v034_WriteReg(MTV_PIXEL_FRAME_LINE_CTRL_REG, ctx->pixel_frame_line_ctrl_reg); + + // Write reserved registers per Rev G datasheet table 8 recommendations + mt9v034_WriteReg(0x13, 0x2D2E); + mt9v034_WriteReg(0x20, 0x03C7); + mt9v034_WriteReg(0x24, 0x001B); + mt9v034_WriteReg(0x2B, 0x0003); + mt9v034_WriteReg(0x2F, 0x0003); } /* settings that are the same for both contexts: */ From 366f80e8a530341683298c0a07846e3d4f13b0c3 Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Thu, 8 Oct 2015 15:53:29 -0700 Subject: [PATCH 107/120] Increase minimum horizontal blanking to reduce corruption. Datasheet Rev A says 690. Rev G says 704. Testing says higher. --- src/modules/flow/mt9v034.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/flow/mt9v034.c b/src/modules/flow/mt9v034.c index 0c8d777..7e52c69 100644 --- a/src/modules/flow/mt9v034.c +++ b/src/modules/flow/mt9v034.c @@ -149,8 +149,8 @@ static uint16_t mt9v034_hor_blanking(uint16_t raw_width, int binning) { case 4: hor_blanking = 91; break; } /* increase if window is smaller: */ - if (raw_width < 690 - hor_blanking) { - hor_blanking = 690 - raw_width; + if (raw_width < 800 - hor_blanking) { + hor_blanking = 800 - raw_width; } return hor_blanking; } From 5bb7a8b14c6e703e89ae10c1160ad5562c6181bd Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Mon, 12 Oct 2015 11:21:54 -0700 Subject: [PATCH 108/120] Clean remaining sign-compare warnings --- src/include/camera.h | 6 +++--- src/modules/flow/camera.c | 19 ++++++++----------- src/modules/libc/stdlib/lib_qsort.c | 6 +++--- 3 files changed, 14 insertions(+), 17 deletions(-) diff --git a/src/include/camera.h b/src/include/camera.h index 44f548e..45ef1db 100644 --- a/src/include/camera.h +++ b/src/include/camera.h @@ -72,7 +72,7 @@ * If there is a certain amount of change in the image brightness the tolerance band will decide after * how much brightness delta the exposure algorithm will start to re-adjust the exposure time. */ -#define CONFIG_CAMERA_DESIRED_EXPOSURE_TOL_8B (24u) +#define CONFIG_CAMERA_DESIRED_EXPOSURE_TOL_8B (24) /** * The smoothing factor of the exposure time. This is the constant of an exponential filter. * @@ -84,7 +84,7 @@ /** * The interval between updating the exposure value in number of frames. Snapshot and invalid frames are skipped. */ -#define CONFIG_CAMERA_EXPOSURE_UPDATE_INTERVAL (4u) +#define CONFIG_CAMERA_EXPOSURE_UPDATE_INTERVAL (4) struct _camera_sensor_interface; typedef struct _camera_sensor_interface camera_sensor_interface; @@ -393,7 +393,7 @@ struct _camera_ctx { camera_img_param_ex img_stream_param; ///< The parameters of the image streaming mode. camera_img_param img_stream_param_pend; ///< The pending image streaming mode parameters. camera_image_buffer buffers[CONFIG_CAMERA_MAX_BUFFER_COUNT];///< The image buffers for image stream mode. - int buffer_count; ///< Total number of buffers. + size_t buffer_count; ///< Total number of buffers. volatile uint8_t buf_avail[CONFIG_CAMERA_MAX_BUFFER_COUNT];///< Indexes to the buffers that are available. Ordered in the MRU order. volatile uint8_t buf_avail_count; ///< Number of buffer indexes in the avail_bufs array. volatile uint8_t buf_put_back_pos; ///< Position where to put back the reserved buffers. diff --git a/src/modules/flow/camera.c b/src/modules/flow/camera.c index f4b79c0..3bcbd1f 100644 --- a/src/modules/flow/camera.c +++ b/src/modules/flow/camera.c @@ -83,8 +83,7 @@ bool camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const c ctx->img_stream_param.analog_gain = ctx->analog_gain; ctx->img_stream_param.exposure = ctx->exposure; ctx->snapshot_param = ctx->img_stream_param; - int i; - for (i = 0; i < buffer_count; ++i) { + for (unsigned i = 0; i < buffer_count; ++i) { // check the buffer: if (buffers[i].buffer_size < img_size || buffers[i].buffer == NULL) { return false; @@ -128,8 +127,7 @@ bool camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const c } static void uint32_t_memcpy(uint32_t *dst, const uint32_t *src, size_t count) { - int i; - for (i = 0; i < count; ++i) { + for (size_t i = 0; i < count; ++i) { dst[i] = src[i]; } } @@ -180,7 +178,7 @@ static void camera_buffer_fifo_push_at(camera_ctx *ctx, size_t pos, const int *i static void camera_update_exposure_hist(camera_ctx *ctx, const uint8_t *buffer, size_t size) { if (ctx->exposure_sampling_stride > 0) { - int i; + size_t i; int c = 0; for (i = ctx->exposure_sampling_stride / 2; i < size; i += ctx->exposure_sampling_stride) { int idx = (buffer[i] >> (8u - CONFIG_CAMERA_EXPOSURE_BIN_BITS)); @@ -451,7 +449,7 @@ bool camera_img_stream_schedule_param_change(camera_ctx *ctx, const camera_img_p // invalid size parameter! return false; } - int i; + size_t i; for (i = 0; i < ctx->buffer_count; ++i) { // check image size against each buffer: if (img_size > ctx->buffers[i].buffer_size) { @@ -504,7 +502,7 @@ void camera_snapshot_acknowledge(camera_ctx *ctx) { } static bool camera_img_stream_get_buffers_idx(camera_ctx *ctx, int bidx[], size_t count, bool reset_new_frm) { - int i; + size_t i; __disable_irq(); if (!camera_buffer_fifo_remove_front(ctx, bidx, count)) { if (reset_new_frm) { @@ -542,7 +540,7 @@ int camera_img_stream_get_buffers(camera_ctx *ctx, camera_image_buffer *buffers[ if (count > ctx->buffer_count - 1 || count <= 0) return -1; /* buffer management needs to be performed atomically: */ int bidx[count]; - int i; + size_t i; while (1) { if (wait_for_new) { /* wait until a new frame is available: */ @@ -570,10 +568,9 @@ void camera_img_stream_return_buffers(camera_ctx *ctx, camera_image_buffer *buff if (!ctx->buffers_are_reserved) return; /* get the buffer indexes: */ int bidx[count]; - int i; - for (i = 0; i < count; ++i) { + for (size_t i = 0; i < count; ++i) { int idx = buffers[i] - &ctx->buffers[0]; - if (idx < 0 || idx >= ctx->buffer_count) return; + if (idx < 0 || idx >= (int) ctx->buffer_count) return; bidx[i] = idx; } /* buffer management needs to be performed atomically: */ diff --git a/src/modules/libc/stdlib/lib_qsort.c b/src/modules/libc/stdlib/lib_qsort.c index 1a13773..5b229e4 100644 --- a/src/modules/libc/stdlib/lib_qsort.c +++ b/src/modules/libc/stdlib/lib_qsort.c @@ -220,15 +220,15 @@ void qsort(void *base, size_t nmemb, size_t size, pn = (char *) base + nmemb * size; r = min(pa - (char *)base, pb - pa); vecswap(base, pb - r, r); - r = min(pd - pc, pn - pd - size); + r = min(pd - pc, pn - pd - (int)size); vecswap(pb, pn - r, r); - if ((r = pb - pa) > size) + if ((r = pb - pa) > (int)size) { qsort(base, r / size, size, compar); } - if ((r = pd - pc) > size) + if ((r = pd - pc) > (int)size) { /* Iterate rather than recurse to save stack space */ From cef5809340af4084aa5a939d5f6a08cf8b469d05 Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Mon, 12 Oct 2015 11:34:03 -0700 Subject: [PATCH 109/120] Clean all other warnings --- src/modules/flow/communication.c | 2 +- src/modules/flow/distance_mode_filter.c | 14 +++--- src/modules/flow/result_accumulator.c | 1 + src/modules/i2c_slave/i2c_slave.c | 1 + src/modules/lidar-lite/lidar.c | 61 ++----------------------- src/modules/lidar-sf10/lidar.c | 3 ++ src/modules/sonar/sonar.c | 4 +- 7 files changed, 18 insertions(+), 68 deletions(-) diff --git a/src/modules/flow/communication.c b/src/modules/flow/communication.c index 06b34e9..2bec201 100644 --- a/src/modules/flow/communication.c +++ b/src/modules/flow/communication.c @@ -50,7 +50,7 @@ extern void systemreset(bool to_bootloader); -extern void notify_changed_camera_parameters(); +extern void notify_changed_camera_parameters(void); mavlink_system_t mavlink_system; diff --git a/src/modules/flow/distance_mode_filter.c b/src/modules/flow/distance_mode_filter.c index a2b7cf9..c335be4 100644 --- a/src/modules/flow/distance_mode_filter.c +++ b/src/modules/flow/distance_mode_filter.c @@ -37,24 +37,22 @@ /** * insert-only ring buffer of distance data, needs to be of uneven size * the initialization to zero will make the filter respond zero for the - * first N inserted readinds, which is a decent startup-logic. + * first N inserted readings, which is a decent startup-logic. */ static float distance_values[5] = { 0.0f }; static unsigned insert_index = 0; -static void distance_bubble_sort(float distance_values[], unsigned n); - -void distance_bubble_sort(float distance_values[], unsigned n) +static void distance_bubble_sort(float distances[], unsigned n) { float t; for (unsigned i = 0; i < (n - 1); i++) { for (unsigned j = 0; j < (n - i - 1); j++) { - if (distance_values[j] > distance_values[j+1]) { + if (distances[j] > distances[j+1]) { /* swap two values */ - t = distance_values[j]; - distance_values[j] = distance_values[j + 1]; - distance_values[j + 1] = t; + t = distances[j]; + distances[j] = distances[j + 1]; + distances[j + 1] = t; } } } diff --git a/src/modules/flow/result_accumulator.c b/src/modules/flow/result_accumulator.c index 3e8049f..9ea0906 100644 --- a/src/modules/flow/result_accumulator.c +++ b/src/modules/flow/result_accumulator.c @@ -41,6 +41,7 @@ #include "result_accumulator.h" #include "i2c_frame.h" #include "settings.h" +#include "gyro.h" void result_accumulator_init(result_accumulator_ctx *ctx) { diff --git a/src/modules/i2c_slave/i2c_slave.c b/src/modules/i2c_slave/i2c_slave.c index bbaa93e..b85ed0b 100644 --- a/src/modules/i2c_slave/i2c_slave.c +++ b/src/modules/i2c_slave/i2c_slave.c @@ -32,6 +32,7 @@ ****************************************************************************/ #include +#include #include "fmu_comm.h" #include "px4_config.h" diff --git a/src/modules/lidar-lite/lidar.c b/src/modules/lidar-lite/lidar.c index c50d8d3..b792ca7 100644 --- a/src/modules/lidar-lite/lidar.c +++ b/src/modules/lidar-lite/lidar.c @@ -36,6 +36,7 @@ #include "usbd_cdc_vcp.h" #include "main.h" #include "distance_mode_filter.h" +#include "timer.h" #include "stm32f4xx.h" #include "stm32f4xx_i2c.h" @@ -114,64 +115,6 @@ static void i2c_init_master(void) i2c_started = true; } -/* Returns false on error condition... also responsible for error recovery */ -static bool i2c_wait_for_event(uint32_t evt) -{ - while(!I2C_CheckEvent(I2C1, evt)) { - if(I2C_GetFlagStatus(I2C1, I2C_FLAG_AF) == SET) { - I2C_ClearFlag(I2C1, I2C_FLAG_AF); - I2C_GenerateSTOP(I2C1, ENABLE); - return false; - } - } - return true; -} - -/* Returns false on NAK */ -static bool i2c_write(uint8_t address, uint8_t *data, uint8_t length) -{ - if(!i2c_started) - return false; - while(I2C_GetFlagStatus(I2C1, I2C_FLAG_BUSY)); - I2C_GenerateSTART(I2C1, ENABLE); - if(!i2c_wait_for_event(I2C_EVENT_MASTER_MODE_SELECT)) return false; - I2C_Send7bitAddress(I2C1, address << 1, I2C_Direction_Transmitter); - if(!i2c_wait_for_event(I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)) return false; - while(length > 0) { - I2C_SendData(I2C1, *data); - if(!i2c_wait_for_event(I2C_EVENT_MASTER_BYTE_TRANSMITTED)) return false; - data++; - length--; - } - I2C_GenerateSTOP(I2C1, ENABLE); - return true; -} - -/* Returns false on NAK */ -static bool i2c_read(uint8_t address, uint8_t *data, uint8_t length) -{ - if(!i2c_started) - return false; - while(I2C_GetFlagStatus(I2C1, I2C_FLAG_BUSY)); - I2C_GenerateSTART(I2C1, ENABLE); - if(!i2c_wait_for_event(I2C_EVENT_MASTER_MODE_SELECT)) return false; - I2C_Send7bitAddress(I2C1, address << 1, I2C_Direction_Receiver); - if(!i2c_wait_for_event(I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED)) return false; - while(length > 0) { - if(length == 1) { - I2C_AcknowledgeConfig(I2C1, DISABLE); - I2C_GenerateSTOP(I2C1, ENABLE); - } else { - I2C_AcknowledgeConfig(I2C1, ENABLE); - } - if(!i2c_wait_for_event(I2C_EVENT_MASTER_BYTE_RECEIVED)) return false; - *data = I2C_ReceiveData(I2C1); - data++; - length--; - } - return true; -} - /* Data from the LIDAR */ float sample, sample_filter; @@ -246,6 +189,7 @@ __EXPORT void distance_readback(void) static void lidar_process(void); +void I2C1_EV_IRQHandler(void); __EXPORT void I2C1_EV_IRQHandler(void) { if(ld_nextev != 0 && I2C_CheckEvent(I2C1, ld_nextev)) { @@ -325,6 +269,7 @@ __EXPORT void I2C1_EV_IRQHandler(void) } } +void I2C1_ER_IRQHandler(void); __EXPORT void I2C1_ER_IRQHandler(void) { if((I2C_ReadRegister(I2C1, I2C_Register_SR1) & 0xFF00) != 0x00) { I2C_GenerateSTOP(I2C1, ENABLE); diff --git a/src/modules/lidar-sf10/lidar.c b/src/modules/lidar-sf10/lidar.c index c4d7c0c..0a532f8 100644 --- a/src/modules/lidar-sf10/lidar.c +++ b/src/modules/lidar-sf10/lidar.c @@ -36,6 +36,7 @@ #include "usbd_cdc_vcp.h" #include "main.h" #include "distance_mode_filter.h" +#include "timer.h" #include "stm32f4xx.h" #include "stm32f4xx_i2c.h" @@ -129,6 +130,7 @@ static void lidar_process(uint16_t value); void I2C1_EV_IRQHandler(void); void I2C1_ER_IRQHandler(void); +void I2C1_EV_IRQHandler(void); __EXPORT void I2C1_EV_IRQHandler(void) { if (I2C_GetITStatus(I2C1, I2C_IT_SB)) { @@ -147,6 +149,7 @@ __EXPORT void I2C1_EV_IRQHandler(void) } } +void I2C1_ER_IRQHandler(void); __EXPORT void I2C1_ER_IRQHandler(void) { if((I2C_ReadRegister(I2C1, I2C_Register_SR1) & 0xFF00) != 0x00) { I2C_GenerateSTOP(I2C1, ENABLE); diff --git a/src/modules/sonar/sonar.c b/src/modules/sonar/sonar.c index 20881b8..4d6a80e 100644 --- a/src/modules/sonar/sonar.c +++ b/src/modules/sonar/sonar.c @@ -38,6 +38,7 @@ #include #include #include +#include #include "stm32f4xx.h" #include "stm32f4xx_gpio.h" #include "stm32f4xx_adc.h" @@ -96,7 +97,8 @@ __EXPORT void distance_readback() {} /** * @brief Sonar interrupt handler */ -void UART4_IRQHandler(void) +void UART4_IRQHandler(void); +__EXPORT void UART4_IRQHandler(void) { if (USART_GetITStatus(UART4, USART_IT_RXNE) != RESET) { From 5518ac3021dc0ffdd73fadcc00766a4ed1698fce Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Mon, 12 Oct 2015 13:42:31 -0700 Subject: [PATCH 110/120] Refactor main loop poll for frame camera.c shouldn't be calling into UAVCAN; the main loop should poll for images instead of blocking. --- src/include/camera.h | 10 ++--- src/modules/flow/camera.c | 35 ++++++++-------- src/modules/flow/main.c | 86 ++++++++++++++++++--------------------- 3 files changed, 60 insertions(+), 71 deletions(-) diff --git a/src/include/camera.h b/src/include/camera.h index 45ef1db..926d9a1 100644 --- a/src/include/camera.h +++ b/src/include/camera.h @@ -189,14 +189,14 @@ bool camera_img_stream_schedule_param_change(camera_ctx *ctx, const camera_img_p * It is guaranteed to retrieve consecutive images. * @param count Number of most recent images to retrieve. Must be lower than buffer_count - 1 which was * passed to camera_init. - * @param wait_for_new When true the function will wait until the the requested images are available. - * @return 0 when a set of count consecutive most recent images have been retrieved. - * 1 it is not possible to return count consecutive frames. you need to wait until the frames are captured. - * -1 on error. + * @param check_for_new When true the function will return true only if a new frame is pending. It will then clear the + pending flag. + * @return true when a set of count consecutive most recent images have been retrieved. + false when two consecutive images are not ready * @note When this function is successful (return value 0) the buffers need to be returned to the camera driver before * requesting new buffers. (use camera_img_stream_return_buffers) */ -int camera_img_stream_get_buffers(camera_ctx *ctx, camera_image_buffer *buffers[], size_t count, bool wait_for_new); +bool camera_img_stream_get_buffers(camera_ctx *ctx, camera_image_buffer *buffers[], size_t count, bool wait_for_new); /** * Returns the buffers that have been retrieved by camera_img_stream_get_buffers back to the camera driver. diff --git a/src/modules/flow/camera.c b/src/modules/flow/camera.c index 3bcbd1f..9ae44ee 100644 --- a/src/modules/flow/camera.c +++ b/src/modules/flow/camera.c @@ -535,33 +535,30 @@ static bool camera_img_stream_get_buffers_idx(camera_ctx *ctx, int bidx[], size_ return consecutive; } -int camera_img_stream_get_buffers(camera_ctx *ctx, camera_image_buffer *buffers[], size_t count, bool wait_for_new) { +bool camera_img_stream_get_buffers(camera_ctx *ctx, camera_image_buffer *buffers[], size_t count, bool wait_for_new) { if (ctx->buffers_are_reserved) return -1; if (count > ctx->buffer_count - 1 || count <= 0) return -1; /* buffer management needs to be performed atomically: */ int bidx[count]; size_t i; - while (1) { - if (wait_for_new) { - /* wait until a new frame is available: */ - while(!ctx->new_frame_arrived) { - //TODO: camera_img_stream_get_buffers is calling fmu_comm_run() - fmu_comm_run(); - } + + if (wait_for_new) { + /* wait until a new frame is available: */ + if (!ctx->new_frame_arrived) { + return false; } - if (camera_img_stream_get_buffers_idx(ctx, bidx, count, wait_for_new)) { - /* update the pointers: */ - for (i = 0; i < count; ++i) { - buffers[i] = &ctx->buffers[bidx[i]]; - } - return 0; - } else { - /* not possible! check if we want to wait for the new frame: */ - if (!wait_for_new) { - return 1; - } + } + if (camera_img_stream_get_buffers_idx(ctx, bidx, count, wait_for_new)) { + /* update the pointers: */ + for (i = 0; i < count; ++i) { + buffers[i] = &ctx->buffers[bidx[i]]; } + return true; + } else { + /* not possible! */ + return false; } + } void camera_img_stream_return_buffers(camera_ctx *ctx, camera_image_buffer *buffers[], size_t count) { diff --git a/src/modules/flow/main.c b/src/modules/flow/main.c index 063d1a8..7e560e0 100644 --- a/src/modules/flow/main.c +++ b/src/modules/flow/main.c @@ -97,6 +97,9 @@ static volatile bool snap_capture_success = false; static bool snap_ready = true; volatile bool usb_image_transfer_active = false; +static void check_for_frame(void); +static float latest_ground_distance(void); + /* timer constants */ #define DISTANCE_POLL_MS 100 /* steps in milliseconds ticks */ #define SYSTEM_STATE_MS 1000/* steps in milliseconds ticks */ @@ -220,7 +223,8 @@ static void send_image_step(void) { } } -static void start_send_image(float ground_distance_m) { +static void start_send_image(void) { + float ground_distance_m = latest_ground_distance(); usb_image_transfer_active = true; usb_image_pos = 0; #if defined(CONFIG_ARCH_BOARD_PX4FLOW_V2) @@ -260,6 +264,17 @@ static uint8_t image_buffer_8bit_5[FLOW_IMAGE_SIZE * FLOW_IMAGE_SIZE] __attribut static flow_klt_image flow_klt_images[2] __attribute__((section(".ccm"))); +/* variables */ +uint32_t counter = 0; + +result_accumulator_ctx mavlink_accumulator; +uint32_t fps_timing_start; +uint16_t fps_counter = 0; +uint16_t fps_skipped_counter = 0; + +uint32_t last_frame_index = 0; +uint32_t last_processed_frame_timestamp; + /** * @brief Main function. */ @@ -313,12 +328,7 @@ int main(void) /* usart config*/ usart_init(); - fmu_comm_init(); - - float distance_filtered = 0.0f; // distance in meter - float distance_raw = 0.0f; // distance in meter - bool distance_valid = false; distance_init(); /* reset/start timers */ @@ -330,46 +340,16 @@ int main(void) timer_register(take_snapshot_fn, 500); //timer_register(switch_params_fn, 2000); - /* variables */ - uint32_t counter = 0; - - result_accumulator_ctx mavlink_accumulator; - result_accumulator_init(&mavlink_accumulator); - - uint32_t fps_timing_start = get_boot_time_us(); - uint16_t fps_counter = 0; - uint16_t fps_skipped_counter = 0; - - uint32_t last_frame_index = 0; - - uint32_t last_processed_frame_timestamp = get_boot_time_us(); + fps_timing_start = last_processed_frame_timestamp = get_boot_time_us(); /* main loop */ while (1) { /* check timers */ timer_check(); - fmu_comm_run(); - - distance_valid = distance_read(&distance_filtered, &distance_raw); - /* reset to zero for invalid distances */ - if (!distance_valid) { - distance_filtered = -1.0f; - distance_raw = -1.0f; - } - - /* decide which distance to use */ - float ground_distance; - if(FLOAT_AS_BOOL(global_data.param[PARAM_SONAR_FILTERED])) - { - ground_distance = distance_filtered; - } - else - { - ground_distance = distance_raw; - } + check_for_frame(); if (snap_capture_done) { snap_capture_done = false; @@ -377,10 +357,28 @@ int main(void) snap_ready = true; if (snap_capture_success) { /* send the snapshot! */ - start_send_image(ground_distance); + start_send_image(); } } + } +} + +static float latest_ground_distance(void) { + float distance_filtered, distance_raw; + bool distance_valid = distance_read(&distance_filtered, &distance_raw); + if (!distance_valid) { + return -1; + } else if(FLOAT_AS_BOOL(global_data.param[PARAM_SONAR_FILTERED])) { + return distance_filtered; + } else { + return distance_raw; + } +} +static void check_for_frame(void) { + /* get recent images */ + camera_image_buffer *frames[2]; + if (camera_img_stream_get_buffers(&cam_ctx, frames, 2, true)) { /* new gyroscope data */ float x_rate_sensor, y_rate_sensor, z_rate_sensor; int16_t gyro_temp; @@ -392,14 +390,8 @@ int main(void) float z_rate = z_rate_sensor; // z is correct bool use_klt = !FLOAT_EQ_INT(global_data.param[PARAM_ALGORITHM_CHOICE], 0); - - uint32_t start_computations = 0; - - /* get recent images */ - camera_image_buffer *frames[2]; - camera_img_stream_get_buffers(&cam_ctx, frames, 2, true); - start_computations = get_boot_time_us(); + uint32_t start_computations = get_boot_time_us(); int frame_delta = ((int32_t)frames[0]->frame_number - (int32_t)last_frame_index); last_frame_index = frames[0]->frame_number; @@ -536,7 +528,7 @@ int main(void) .pixel_flow_x = pixel_flow_x, .pixel_flow_y = pixel_flow_y, .rad_per_pixel = 1.0f / focal_length_px, - .ground_distance = ground_distance, + .ground_distance = latest_ground_distance(), .distance_age = get_time_delta_us(get_distance_measure_time()), .max_px_frame = flow_mv_cap, }; From 187822622172ddaf96d3299cdd958b43df616d70 Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Wed, 14 Oct 2015 10:28:48 -0700 Subject: [PATCH 111/120] Fix typo --- src/modules/flow/main.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/flow/main.c b/src/modules/flow/main.c index 7e560e0..42e3e42 100644 --- a/src/modules/flow/main.c +++ b/src/modules/flow/main.c @@ -537,7 +537,7 @@ static void check_for_frame(void) { fmu_comm_update(&frame_data); result_accumulator_feed(&mavlink_accumulator, &frame_data); - uint32_t computaiton_time_us = get_time_delta_us(start_computations); + uint32_t computation_time_us = get_time_delta_us(start_computations); counter++; fps_counter++; @@ -555,7 +555,7 @@ static void check_for_frame(void) { fps_counter = 0; fps_skipped_counter = 0; - mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "TIMING", get_boot_time_us(), computaiton_time_us, fps, fps_skip); + mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "TIMING", get_boot_time_us(), computation_time_us, fps, fps_skip); } mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "EXPOSURE", get_boot_time_us(), frames[0]->param.exposure, frames[0]->param.analog_gain, cam_ctx.last_brightness); From 9d2a7939c9f1c7b5f28988da9644951a8296d75d Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Wed, 28 Oct 2015 10:20:34 -0700 Subject: [PATCH 112/120] Remove unfinished interface to transfer snapshot images asynchronously over USB The actual image transfer is solid, but the format for metadata is not ready to be considered a stable API. --- read_image.py | 37 ------------ src/include/main.h | 1 - src/include/usb_conf.h | 8 +-- src/include/usbd_conf.h | 6 +- src/modules/flow/main.c | 96 -------------------------------- src/modules/flow/usb_composite.c | 30 +--------- 6 files changed, 7 insertions(+), 171 deletions(-) delete mode 100644 read_image.py diff --git a/read_image.py b/read_image.py deleted file mode 100644 index 68e1a46..0000000 --- a/read_image.py +++ /dev/null @@ -1,37 +0,0 @@ -import usb.core, usb.util -import numpy as np -import matplotlib.pyplot as plt -import struct - -dev = usb.core.find(idVendor=0x26ac, idProduct=0x0015) -endpoint = dev[0][(2,0)][0] - -SIZE = 352 -image = np.zeros((SIZE, SIZE), dtype='uint8') -imshow = plt.imshow(image, cmap=plt.cm.gray) -imshow.norm.vmin = 0 -imshow.norm.vmax = 255 -plt.ion() -plt.show() - -HEADER_SIZE = 16 -READ_SIZE = HEADER_SIZE + SIZE*SIZE - -while True: - - data = endpoint.read(64 * (1 + (READ_SIZE) / 64), timeout=20000) # Round up to include the zero-length packet - - if len(data) != READ_SIZE: - # We read a partial image from killing a previous instance of this - # program in the middle of a transfer. We should be using an interface - # alternate setting to enable and reset the endpoint. - print "Ignoring partial buffer of", len(data) - continue - - (timestamp, exposure, distance) = struct.unpack(' MAX_TRANSFER) size = MAX_TRANSFER; - - DCD_EP_Tx(&USB_OTG_dev, IMAGE_IN_EP, &((uint8_t*)&usb_packet)[usb_image_pos], size); - if (size == 0 || size % 64 != 0) { - // We sent a short packet at the end of the transfer. When it completes, we're done. - usb_image_pos = (unsigned) -1; - } else { - usb_image_pos += size; - } -} - -static void start_send_image(void) { - float ground_distance_m = latest_ground_distance(); - usb_image_transfer_active = true; - usb_image_pos = 0; -#if defined(CONFIG_ARCH_BOARD_PX4FLOW_V2) - usb_packet.timestamp = hrt_absolute_time(); -#else - usb_packet.timestamp = 0; -#endif - usb_packet.exposure = snapshot_buffer.param.exposure; - if (ground_distance_m > 0) { - usb_packet.ground_distance_mm = ground_distance_m * 1000.0f; - } else { - usb_packet.ground_distance_mm = 0; - } - DCD_EP_Flush(&USB_OTG_dev, IMAGE_IN_EP); - send_image_step(); -} - -void send_image_completed(void) { - if (usb_image_pos != (unsigned) -1) { - send_image_step(); - } else { - usb_image_transfer_active = false; - } -} - void notify_changed_camera_parameters(void) { camera_reconfigure_general(&cam_ctx); } @@ -281,7 +197,6 @@ uint32_t last_processed_frame_timestamp; int main(void) { __enable_irq(); - snapshot_buffer = BuildCameraImageBuffer(usb_packet.snapshot_buffer_mem); /* load settings and parameters */ global_data_reset_param_defaults(); @@ -337,7 +252,6 @@ int main(void) timer_register(system_receive_fn, SYSTEM_STATE_MS / 2); timer_register(send_params_fn, PARAMS_MS); timer_register(send_video_fn, global_data.param[PARAM_VIDEO_RATE]); - timer_register(take_snapshot_fn, 500); //timer_register(switch_params_fn, 2000); result_accumulator_init(&mavlink_accumulator); @@ -350,16 +264,6 @@ int main(void) timer_check(); fmu_comm_run(); check_for_frame(); - - if (snap_capture_done) { - snap_capture_done = false; - camera_snapshot_acknowledge(&cam_ctx); - snap_ready = true; - if (snap_capture_success) { - /* send the snapshot! */ - start_send_image(); - } - } } } diff --git a/src/modules/flow/usb_composite.c b/src/modules/flow/usb_composite.c index 2a26815..d393b03 100644 --- a/src/modules/flow/usb_composite.c +++ b/src/modules/flow/usb_composite.c @@ -5,7 +5,6 @@ #define INTERFACE_CDC_CONTROL 0 #define INTERFACE_CDC_DATA 1 -#define INTERFACE_IMAGE 2 typedef struct ConfigDesc { USB_ConfigurationDescriptor Config; @@ -19,9 +18,6 @@ typedef struct ConfigDesc { USB_InterfaceDescriptor CDC_data_interface; USB_EndpointDescriptor CDC_out_endpoint; USB_EndpointDescriptor CDC_in_endpoint; - - USB_InterfaceDescriptor ImageInterface; - USB_EndpointDescriptor ImageInEndpoint; } __attribute__((packed)) ConfigDesc; const ConfigDesc configuration_descriptor = { @@ -29,7 +25,7 @@ const ConfigDesc configuration_descriptor = { .bLength = sizeof(USB_ConfigurationDescriptor), .bDescriptorType = USB_DTYPE_Configuration, .wTotalLength = sizeof(ConfigDesc), - .bNumInterfaces = 3, + .bNumInterfaces = 2, .bConfigurationValue = 1, .iConfiguration = 0, .bmAttributes = USB_CONFIG_ATTR_BUSPOWERED, @@ -100,26 +96,6 @@ const ConfigDesc configuration_descriptor = { .wMaxPacketSize = CDC_DATA_MAX_PACKET_SIZE, .bInterval = 0x0 }, - - .ImageInterface = { - .bLength = sizeof(USB_InterfaceDescriptor), - .bDescriptorType = USB_DTYPE_Interface, - .bInterfaceNumber = INTERFACE_IMAGE, - .bAlternateSetting = 0, - .bNumEndpoints = 1, - .bInterfaceClass = USB_CSCP_VendorSpecificClass, - .bInterfaceSubClass = 0x00, - .bInterfaceProtocol = 0x00, - .iInterface = 0, - }, - .ImageInEndpoint = { - .bLength = sizeof(USB_EndpointDescriptor), - .bDescriptorType = USB_DTYPE_Endpoint, - .bEndpointAddress = IMAGE_IN_EP, - .bmAttributes = (USB_EP_TYPE_BULK | ENDPOINT_ATTR_NO_SYNC | ENDPOINT_USAGE_DATA), - .wMaxPacketSize = IMAGE_IN_PACKET_SIZE, - .bInterval = 0x00 - }, }; static uint8_t usb_Init (void *pdev, uint8_t cfgidx); @@ -165,13 +141,9 @@ static uint8_t usb_EP0_RxReady (void *pdev) { return USBD_CDC_cb.EP0_RxReady(pdev); } -void send_image_completed(void); - static uint8_t usb_DataIn (void *pdev, uint8_t epnum) { if (epnum == (CDC_IN_EP & 0x7f) || epnum == (CDC_CMD_EP & 0x7f)) { return USBD_CDC_cb.DataIn(pdev, epnum); - } else if (epnum == (IMAGE_IN_EP & 0x7f)) { - send_image_completed(); } return USBD_OK; } From b6ec5be3283aacbf5b21a19784128117791b30e0 Mon Sep 17 00:00:00 2001 From: Kevin Mehall Date: Wed, 28 Oct 2015 10:27:45 -0700 Subject: [PATCH 113/120] Revert refactor of USB descriptors. This was a preliminary step to adding the asynchronous image transfer endpoint. While it was a cleanup in its own right, might as well use the standard ST code for easier upgrade if it does not need modification. This reverts commits 6493c8f and 430c01d --- src/include/cdc_standard.h | 45 ---- src/include/usb_standard.h | 201 ------------------ .../Class/cdc/src/usbd_cdc_core.c | 6 + src/modules/flow/main.c | 3 +- src/modules/flow/module.mk | 3 +- src/modules/flow/usb_composite.c | 162 -------------- src/modules/flow/usbd_desc.c | 58 +++-- 7 files changed, 45 insertions(+), 433 deletions(-) delete mode 100644 src/include/cdc_standard.h delete mode 100644 src/include/usb_standard.h delete mode 100644 src/modules/flow/usb_composite.c diff --git a/src/include/cdc_standard.h b/src/include/cdc_standard.h deleted file mode 100644 index ed19353..0000000 --- a/src/include/cdc_standard.h +++ /dev/null @@ -1,45 +0,0 @@ -#pragma once - -#define CDC_INTERFACE_CLASS 0x02 -#define CDC_INTERFACE_SUBCLASS_ACM 0x02 -#define CDC_INTERFACE_CLASS_DATA 0x0A - -#define CDC_SUBTYPE_HEADER 0x00 -#define CDC_SUBTYPE_ACM 0x02 -#define CDC_SUBTYPE_UNION 0x06 - -#define CDC_SEND_ENCAPSULATED_COMMAND 0x0 -#define CDC_GET_ENCAPSULATED_RESPONSE 0x1 -#define CDC_SET_LINE_ENCODING 0x20 -#define CDC_GET_LINE_ENCODING 0x21 -#define CDC_SET_CONTROL_LINE_STATE 0x22 -#define CDC_SEND_BREAK 0x23 - -typedef struct { - uint8_t bLength; - uint8_t bDescriptorType; - uint8_t bDescriptorSubtype; - uint16_t bcdCDC; -} __attribute__((packed)) CDC_FunctionalHeaderDescriptor; - -typedef struct { - uint8_t bLength; - uint8_t bDescriptorType; - uint8_t bDescriptorSubtype; - uint8_t bmCapabilities; -} __attribute__((packed)) CDC_FunctionalACMDescriptor; - -typedef struct { - uint8_t bLength; - uint8_t bDescriptorType; - uint8_t bDescriptorSubtype; - uint8_t bMasterInterface; - uint8_t bSlaveInterface; -} __attribute__((packed)) CDC_FunctionalUnionDescriptor; - -typedef struct { - uint32_t baud_rate; - uint8_t char_format; - uint8_t parity_type; - uint8_t data_bits; -} __attribute__((packed)) CDC_LineEncoding; diff --git a/src/include/usb_standard.h b/src/include/usb_standard.h deleted file mode 100644 index a830b02..0000000 --- a/src/include/usb_standard.h +++ /dev/null @@ -1,201 +0,0 @@ -#pragma once - -#include - -// Device -> host -#define USB_IN 0x80 -// Host -> device -#define USB_OUT 0 - -#define USB_REQTYPE_DIRECTION_MASK 0x80 -#define USB_REQTYPE_TYPE_MASK 0x60 -#define USB_REQTYPE_RECIPIENT_MASK 0x1F - -#define USB_REQTYPE_STANDARD (0 << 5) -#define USB_REQTYPE_CLASS (1 << 5) -#define USB_REQTYPE_VENDOR (2 << 5) - -#define USB_RECIPIENT_DEVICE (0 << 0) -#define USB_RECIPIENT_INTERFACE (1 << 0) -#define USB_RECIPIENT_ENDPOINT (2 << 0) -#define USB_RECIPIENT_OTHER (3 << 0) - -typedef struct { - uint8_t bmRequestType; - uint8_t bRequest; - uint16_t wValue; - uint16_t wIndex; - uint16_t wLength; -} __attribute__ ((packed)) USB_SetupPacket; - -// Standard requests -enum { - USB_REQ_GetStatus = 0, - USB_REQ_ClearFeature = 1, - USB_REQ_SetFeature = 3, - USB_REQ_SetAddress = 5, - USB_REQ_GetDescriptor = 6, - USB_REQ_SetDescriptor = 7, - USB_REQ_GetConfiguration = 8, - USB_REQ_SetConfiguration = 9, - USB_REQ_GetInterface = 10, - USB_REQ_SetInterface = 11, - USB_REQ_SynchFrame = 12, -}; - -enum { - USB_FEATURE_EndpointHalt = 0x00, - USB_FEATURE_DeviceRemoteWakeup = 0x01, - USB_FEATURE_TestMode = 0x02, -}; - -enum { - USB_DTYPE_Device = 0x01, - USB_DTYPE_Configuration = 0x02, - USB_DTYPE_String = 0x03, - USB_DTYPE_Interface = 0x04, - USB_DTYPE_Endpoint = 0x05, - USB_DTYPE_DeviceQualifier = 0x06, - USB_DTYPE_Other = 0x07, - USB_DTYPE_InterfacePower = 0x08, - USB_DTYPE_InterfaceAssociation = 0x0B, - USB_DTYPE_CSInterface = 0x24, - USB_DTYPE_CSEndpoint = 0x25, -}; - -typedef enum { - USB_CSCP_NoDeviceClass = 0x00, - USB_CSCP_NoDeviceSubclass = 0x00, - USB_CSCP_NoDeviceProtocol = 0x00, - USB_CSCP_VendorSpecificClass = 0xFF, - USB_CSCP_VendorSpecificSubclass = 0xFF, - USB_CSCP_VendorSpecificProtocol = 0xFF, - USB_CSCP_IADDeviceClass = 0xEF, - USB_CSCP_IADDeviceSubclass = 0x02, - USB_CSCP_IADDeviceProtocol = 0x01, -} USB_cscp; - -#define USB_CONFIG_POWER_MA(mA) ((mA)/2) -#define USB_STRING_LEN(c) (sizeof(USB_DescriptorHeader) + ((c) * 2)) - -#define USB_LANGUAGE_EN_US 0x0409 - -#define ENDPOINT_DESCRIPTOR_DIR_IN 0x80 -#define ENDPOINT_DESCRIPTOR_DIR_OUT 0x00 - -#define USB_CONFIG_ATTR_BUSPOWERED 0x80 -#define USB_CONFIG_ATTR_SELFPOWERED 0x40 -#define USB_CONFIG_ATTR_REMOTEWAKEUP 0x20 - -#define ENDPOINT_ATTR_NO_SYNC (0 << 2) -#define ENDPOINT_ATTR_ASYNC (1 << 2) -#define ENDPOINT_ATTR_ADAPTIVE (2 << 2) -#define ENDPOINT_ATTR_SYNC (3 << 2) - -#define ENDPOINT_USAGE_DATA (0 << 4) -#define ENDPOINT_USAGE_FEEDBACK (1 << 4) -#define ENDPOINT_USAGE_IMPLICIT_FEEDBACK (2 << 4) - -#define USB_EP_TYPE_CONTROL 0x00 -#define USB_EP_TYPE_ISOCHRONOUS 0x01 -#define USB_EP_TYPE_BULK 0x02 -#define USB_EP_TYPE_INTERRUPT 0x03 - -typedef struct { - uint8_t bLength; - uint8_t bDescriptorType; -} __attribute__ ((packed)) USB_DescriptorHeader; - -typedef struct { - uint8_t bLength; - uint8_t bDescriptorType; - uint16_t bcdUSB; - uint8_t bDeviceClass; - uint8_t bDeviceSubClass; - uint8_t bDeviceProtocol; - uint8_t bMaxPacketSize0; - uint16_t idVendor; - uint16_t idProduct; - uint16_t bcdDevice; - uint8_t iManufacturer; - uint8_t iProduct; - uint8_t iSerialNumber; - uint8_t bNumConfigurations; -} __attribute__ ((packed)) USB_DeviceDescriptor; - -typedef struct { - uint8_t bLength; - uint8_t bDescriptorType; - uint16_t bcdUSB; - uint8_t bDeviceSubClass; - uint8_t bMaxPacketSize0; - uint8_t bNumConfigurations; - uint8_t bReserved; -} __attribute__ ((packed)) USB_DeviceQualifierDescriptor; - -typedef struct { - uint8_t bLength; - uint8_t bDescriptorType; - uint16_t wTotalLength; - uint8_t bNumInterfaces; - uint8_t bConfigurationValue; - uint8_t iConfiguration; - uint8_t bmAttributes; - uint8_t bMaxPower; -} __attribute__ ((packed)) USB_ConfigurationDescriptor; - -typedef struct { - uint8_t bLength; - uint8_t bDescriptorType; - uint8_t bInterfaceNumber; - uint8_t bAlternateSetting; - uint8_t bNumEndpoints; - uint8_t bInterfaceClass; - uint8_t bInterfaceSubClass; - uint8_t bInterfaceProtocol; - uint8_t iInterface; -} __attribute__ ((packed)) USB_InterfaceDescriptor; - -typedef struct { - uint8_t bLength; - uint8_t bDescriptorType; - uint8_t bFirstInterface; - uint8_t bInterfaceCount; - uint8_t bFunctionClass; - uint8_t bFunctionSubClass; - uint8_t bFunctionProtocol; - uint8_t iFunction; -} __attribute__ ((packed)) USB_InterfaceAssociationDescriptor; - -typedef struct { - uint8_t bLength; - uint8_t bDescriptorType; - uint8_t bEndpointAddress; - uint8_t bmAttributes; - uint16_t wMaxPacketSize; - uint8_t bInterval; -} __attribute__ ((packed)) USB_EndpointDescriptor; - -typedef struct { - uint8_t bLength; - uint8_t bDescriptorType; - __CHAR16_TYPE__ bString[]; -} __attribute__ ((packed)) USB_StringDescriptor; - -/// Microsoft WCID descriptor -typedef struct { - uint8_t bFirstInterfaceNumber; - uint8_t reserved1; - uint8_t compatibleID[8]; - uint8_t subCompatibleID[8]; - uint8_t reserved2[6]; -} __attribute__((packed)) USB_MicrosoftCompatibleDescriptor_Interface; - -typedef struct { - uint32_t dwLength; - uint16_t bcdVersion; - uint16_t wIndex; - uint8_t bCount; - uint8_t reserved[7]; - USB_MicrosoftCompatibleDescriptor_Interface interfaces[]; -} __attribute__((packed)) USB_MicrosoftCompatibleDescriptor; diff --git a/src/lib/stm32/st/v1.0.2/STM32_USB_Device_Library/Class/cdc/src/usbd_cdc_core.c b/src/lib/stm32/st/v1.0.2/STM32_USB_Device_Library/Class/cdc/src/usbd_cdc_core.c index 930c2ec..a91772b 100644 --- a/src/lib/stm32/st/v1.0.2/STM32_USB_Device_Library/Class/cdc/src/usbd_cdc_core.c +++ b/src/lib/stm32/st/v1.0.2/STM32_USB_Device_Library/Class/cdc/src/usbd_cdc_core.c @@ -429,6 +429,8 @@ __ALIGN_BEGIN uint8_t usbd_cdc_OtherCfgDesc[USB_CDC_CONFIG_DESC_SIZ] __ALIGN_EN static uint8_t usbd_cdc_Init (void *pdev, uint8_t cfgidx) { + uint8_t *pbuf; + /* Open EP IN */ DCD_EP_Open(pdev, CDC_IN_EP, @@ -447,6 +449,10 @@ static uint8_t usbd_cdc_Init (void *pdev, CDC_CMD_PACKET_SZE, USB_OTG_EP_INT); + pbuf = (uint8_t *)USBD_DeviceDesc; + pbuf[4] = DEVICE_CLASS_CDC; + pbuf[5] = DEVICE_SUBCLASS_CDC; + /* Initialize the Interface physical components */ APP_FOPS.pIf_Init(); diff --git a/src/modules/flow/main.c b/src/modules/flow/main.c index 04d1d37..c2f1cd6 100644 --- a/src/modules/flow/main.c +++ b/src/modules/flow/main.c @@ -80,7 +80,6 @@ __ALIGN_BEGIN USB_OTG_CORE_HANDLE USB_OTG_dev __ALIGN_END; -extern USBD_Class_cb_TypeDef custom_composite_cb; #define FLOW_IMAGE_SIZE (64) @@ -215,7 +214,7 @@ int main(void) USBD_Init( &USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, - &custom_composite_cb, + &USBD_CDC_cb, &USR_cb); /* init mavlink */ diff --git a/src/modules/flow/module.mk b/src/modules/flow/module.mk index 1b70646..21ec0a4 100644 --- a/src/modules/flow/module.mk +++ b/src/modules/flow/module.mk @@ -37,8 +37,7 @@ SRCS += main.c \ distance_mode_filter.c \ camera.c \ result_accumulator.c \ - timer.c \ - usb_composite.c \ + timer.c SRCS += $(ST_LIB)STM32F4xx_StdPeriph_Driver/src/misc.c \ diff --git a/src/modules/flow/usb_composite.c b/src/modules/flow/usb_composite.c deleted file mode 100644 index d393b03..0000000 --- a/src/modules/flow/usb_composite.c +++ /dev/null @@ -1,162 +0,0 @@ -#include "usb_standard.h" -#include "cdc_standard.h" -#include "usbd_conf.h" -#include "usbd_cdc_core.h" - -#define INTERFACE_CDC_CONTROL 0 -#define INTERFACE_CDC_DATA 1 - -typedef struct ConfigDesc { - USB_ConfigurationDescriptor Config; - - USB_InterfaceDescriptor CDC_control_interface; - CDC_FunctionalHeaderDescriptor CDC_functional_header; - CDC_FunctionalACMDescriptor CDC_functional_ACM; - CDC_FunctionalUnionDescriptor CDC_functional_union; - USB_EndpointDescriptor CDC_notification_endpoint; - - USB_InterfaceDescriptor CDC_data_interface; - USB_EndpointDescriptor CDC_out_endpoint; - USB_EndpointDescriptor CDC_in_endpoint; -} __attribute__((packed)) ConfigDesc; - -const ConfigDesc configuration_descriptor = { - .Config = { - .bLength = sizeof(USB_ConfigurationDescriptor), - .bDescriptorType = USB_DTYPE_Configuration, - .wTotalLength = sizeof(ConfigDesc), - .bNumInterfaces = 2, - .bConfigurationValue = 1, - .iConfiguration = 0, - .bmAttributes = USB_CONFIG_ATTR_BUSPOWERED, - .bMaxPower = USB_CONFIG_POWER_MA(500) - }, - .CDC_control_interface = { - .bLength = sizeof(USB_InterfaceDescriptor), - .bDescriptorType = USB_DTYPE_Interface, - .bInterfaceNumber = INTERFACE_CDC_CONTROL, - .bAlternateSetting = 0, - .bNumEndpoints = 1, - .bInterfaceClass = CDC_INTERFACE_CLASS, - .bInterfaceSubClass = CDC_INTERFACE_SUBCLASS_ACM, - .bInterfaceProtocol = 0, - .iInterface = 0, - }, - .CDC_functional_header = { - .bLength = sizeof(CDC_FunctionalHeaderDescriptor), - .bDescriptorType = USB_DTYPE_CSInterface, - .bDescriptorSubtype = CDC_SUBTYPE_HEADER, - .bcdCDC = 0x0110, - }, - .CDC_functional_ACM = { - .bLength = sizeof(CDC_FunctionalACMDescriptor), - .bDescriptorType = USB_DTYPE_CSInterface, - .bDescriptorSubtype = CDC_SUBTYPE_ACM, - .bmCapabilities = 0x00, - }, - .CDC_functional_union = { - .bLength = sizeof(CDC_FunctionalUnionDescriptor), - .bDescriptorType = USB_DTYPE_CSInterface, - .bDescriptorSubtype = CDC_SUBTYPE_UNION, - .bMasterInterface = INTERFACE_CDC_CONTROL, - .bSlaveInterface = INTERFACE_CDC_DATA, - }, - .CDC_notification_endpoint = { - .bLength = sizeof(USB_EndpointDescriptor), - .bDescriptorType = USB_DTYPE_Endpoint, - .bEndpointAddress = CDC_CMD_EP, - .bmAttributes = (USB_EP_TYPE_INTERRUPT | ENDPOINT_ATTR_NO_SYNC | ENDPOINT_USAGE_DATA), - .wMaxPacketSize = CDC_CMD_PACKET_SZE, - .bInterval = 0xFF - }, - .CDC_data_interface = { - .bLength = sizeof(USB_InterfaceDescriptor), - .bDescriptorType = USB_DTYPE_Interface, - .bInterfaceNumber = INTERFACE_CDC_DATA, - .bAlternateSetting = 0, - .bNumEndpoints = 2, - .bInterfaceClass = CDC_INTERFACE_CLASS_DATA, - .bInterfaceSubClass = 0, - .bInterfaceProtocol = 0, - .iInterface = 0, - }, - .CDC_out_endpoint = { - .bLength = sizeof(USB_EndpointDescriptor), - .bDescriptorType = USB_DTYPE_Endpoint, - .bEndpointAddress = CDC_OUT_EP, - .bmAttributes = (USB_EP_TYPE_BULK | ENDPOINT_ATTR_NO_SYNC | ENDPOINT_USAGE_DATA), - .wMaxPacketSize = CDC_DATA_MAX_PACKET_SIZE, - .bInterval = 0x0 - }, - .CDC_in_endpoint = { - .bLength = sizeof(USB_EndpointDescriptor), - .bDescriptorType = USB_DTYPE_Endpoint, - .bEndpointAddress = CDC_IN_EP, - .bmAttributes = (USB_EP_TYPE_BULK | ENDPOINT_ATTR_NO_SYNC | ENDPOINT_USAGE_DATA), - .wMaxPacketSize = CDC_DATA_MAX_PACKET_SIZE, - .bInterval = 0x0 - }, -}; - -static uint8_t usb_Init (void *pdev, uint8_t cfgidx); -static uint8_t usb_DeInit (void *pdev, uint8_t cfgidx); -static uint8_t usb_Setup (void *pdev, USB_SETUP_REQ *req); -static uint8_t usb_EP0_RxReady (void *pdev); -static uint8_t usb_DataIn (void *pdev, uint8_t epnum); -static uint8_t usb_DataOut (void *pdev, uint8_t epnum); -static uint8_t usb_SOF (void *pdev); -static uint8_t* usb_GetCfgDesc (uint8_t speed, uint16_t *length); - -USBD_Class_cb_TypeDef custom_composite_cb = { - .Init = usb_Init, - .DeInit = usb_DeInit, - .Setup = usb_Setup, - .EP0_TxSent = NULL, - .EP0_RxReady = usb_EP0_RxReady, - .DataIn = usb_DataIn, - .DataOut = usb_DataOut, - .SOF = usb_SOF, - .IsoINIncomplete = NULL, - .IsoOUTIncomplete = NULL, - .GetConfigDescriptor = usb_GetCfgDesc, -}; - -static uint8_t usb_Init (void *pdev, uint8_t cfgidx) { - USBD_CDC_cb.Init(pdev, cfgidx); - DCD_EP_Open(pdev, IMAGE_IN_EP, IMAGE_IN_PACKET_SIZE, USB_OTG_EP_BULK); - return USBD_OK; -} - -static uint8_t usb_DeInit (void *pdev, uint8_t cfgidx) { - USBD_CDC_cb.DeInit(pdev, cfgidx); - DCD_EP_Close(pdev, IMAGE_IN_EP); - return USBD_OK; -} - -static uint8_t usb_Setup (void *pdev, USB_SETUP_REQ *req) { - return USBD_CDC_cb.Setup(pdev, req); -} - -static uint8_t usb_EP0_RxReady (void *pdev) { - return USBD_CDC_cb.EP0_RxReady(pdev); -} - -static uint8_t usb_DataIn (void *pdev, uint8_t epnum) { - if (epnum == (CDC_IN_EP & 0x7f) || epnum == (CDC_CMD_EP & 0x7f)) { - return USBD_CDC_cb.DataIn(pdev, epnum); - } - return USBD_OK; -} - -static uint8_t usb_DataOut (void *pdev, uint8_t epnum) { - return USBD_CDC_cb.DataOut(pdev, epnum); -} - -static uint8_t usb_SOF (void *pdev) { - return USBD_CDC_cb.SOF(pdev); -} - -static uint8_t* usb_GetCfgDesc (uint8_t speed, uint16_t *length) { - *length = sizeof (configuration_descriptor); - return (uint8_t*) &configuration_descriptor; -} diff --git a/src/modules/flow/usbd_desc.c b/src/modules/flow/usbd_desc.c index b1942e3..20e7624 100644 --- a/src/modules/flow/usbd_desc.c +++ b/src/modules/flow/usbd_desc.c @@ -30,7 +30,6 @@ #include "usbd_req.h" #include "usbd_conf.h" #include "usb_regs.h" -#include "usb_standard.h" /* Vendor ID */ #define USBD_VID 0x26AC @@ -64,25 +63,42 @@ USBD_DEVICE USR_desc = USBD_USR_InterfaceStrDescriptor, }; -const USB_DeviceDescriptor device_descriptor = { - .bLength = sizeof(USB_DeviceDescriptor), - .bDescriptorType = USB_DTYPE_Device, - - .bcdUSB = 0x0200, - .bDeviceClass = 0, - .bDeviceSubClass = USB_CSCP_NoDeviceSubclass, - .bDeviceProtocol = USB_CSCP_NoDeviceProtocol, - - .bMaxPacketSize0 = 64, - .idVendor = USBD_VID, - .idProduct = USBD_PID, - .bcdDevice = 0x0200, - - .iManufacturer = USBD_IDX_MFC_STR, - .iProduct = USBD_IDX_PRODUCT_STR, - .iSerialNumber = USBD_IDX_SERIAL_STR, +/* USB Standard Device Descriptor */ +__ALIGN_BEGIN uint8_t USBD_DeviceDesc[USB_SIZ_DEVICE_DESC] __ALIGN_END = +{ + 0x12, /*bLength */ + USB_DEVICE_DESCRIPTOR_TYPE, /*bDescriptorType*/ + 0x00, /*bcdUSB */ + 0x02, + 0x00, /*bDeviceClass*/ + 0x00, /*bDeviceSubClass*/ + 0x00, /*bDeviceProtocol*/ + USB_OTG_MAX_EP0_SIZE, /*bMaxPacketSize*/ + LOBYTE(USBD_VID), /*idVendor*/ + HIBYTE(USBD_VID), /*idVendor*/ + LOBYTE(USBD_PID), /*idVendor*/ + HIBYTE(USBD_PID), /*idVendor*/ + 0x00, /*bcdDevice rel. 2.00*/ + 0x02, + USBD_IDX_MFC_STR, /*Index of manufacturer string*/ + USBD_IDX_PRODUCT_STR, /*Index of product string*/ + USBD_IDX_SERIAL_STR, /*Index of serial number string*/ + USBD_CFG_MAX_NUM /*bNumConfigurations*/ + } ; /* USB_DeviceDescriptor */ - .bNumConfigurations = USBD_CFG_MAX_NUM +/* USB Standard Device Descriptor */ +__ALIGN_BEGIN uint8_t USBD_DeviceQualifierDesc[USB_LEN_DEV_QUALIFIER_DESC] __ALIGN_END = +{ + USB_LEN_DEV_QUALIFIER_DESC, + USB_DESC_TYPE_DEVICE_QUALIFIER, + 0x00, + 0x02, + 0x00, + 0x00, + 0x00, + 0x40, + 0x01, + 0x00, }; /* USB Standard Device Descriptor */ @@ -103,8 +119,8 @@ __ALIGN_BEGIN uint8_t USBD_LangIDDesc[USB_SIZ_STRING_LANGID] __ALIGN_END = */ uint8_t * USBD_USR_DeviceDescriptor( uint8_t speed , uint16_t *length) { - *length = sizeof(device_descriptor); - return (uint8_t*) &device_descriptor; + *length = sizeof(USBD_DeviceDesc); + return USBD_DeviceDesc; } /** From 99f4b2619f60348b62da4066562fb8a207ce9a0c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Dec 2015 09:15:32 +0100 Subject: [PATCH 114/120] Build fix --- src/modules/flow/camera.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/flow/camera.c b/src/modules/flow/camera.c index 9ae44ee..719c4e9 100644 --- a/src/modules/flow/camera.c +++ b/src/modules/flow/camera.c @@ -141,10 +141,13 @@ static bool camera_buffer_fifo_remove_front(camera_ctx *ctx, int *out, size_t co *out++ = ctx->buf_avail[i]; } // close gap: + #pragma GCC diagnostic push + #pragma GCC diagnostic warning "-Warray-bounds" for (i = count; i < bc && i < CONFIG_CAMERA_MAX_BUFFER_COUNT; ++i) { ctx->buf_avail[i - count] = ctx->buf_avail[i]; } ctx->buf_avail_count = bc - count; + #pragma GCC diagnostic pop return true; } From ee73c63a803c09095a6cd1cf4270d974675b3ca6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Dec 2015 09:18:48 +0100 Subject: [PATCH 115/120] Boost max exposure for indoor environments --- src/modules/flow/main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/flow/main.c b/src/modules/flow/main.c index c2f1cd6..5bf816f 100644 --- a/src/modules/flow/main.c +++ b/src/modules/flow/main.c @@ -233,7 +233,7 @@ int main(void) BuildCameraImageBuffer(image_buffer_8bit_5) }; camera_init(&cam_ctx, mt9v034_get_sensor_interface(), dcmi_get_transport_interface(), - mt9v034_get_clks_per_row(64, 4) * 1, mt9v034_get_clks_per_row(64, 4) * 64, 2.0, + mt9v034_get_clks_per_row(64, 4) * 1, mt9v034_get_clks_per_row(64, 4) * 512, 2.0, &img_stream_param, buffers, 5); } From dacb2f18dc564ceafd05ea84df7bfcfc87a2a549 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Dec 2015 09:19:10 +0100 Subject: [PATCH 116/120] Code style fix --- src/modules/flow/main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/flow/main.c b/src/modules/flow/main.c index 5bf816f..bff225c 100644 --- a/src/modules/flow/main.c +++ b/src/modules/flow/main.c @@ -242,7 +242,7 @@ int main(void) /* usart config*/ usart_init(); - fmu_comm_init(); + fmu_comm_init(); distance_init(); /* reset/start timers */ From 2a014d31c9b8a7fbfd8a9e043d99ac1be3bc76ae Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Dec 2015 09:24:32 +0100 Subject: [PATCH 117/120] Remove unused headers --- src/modules/flow/dcmi.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/flow/dcmi.c b/src/modules/flow/dcmi.c index ec1c270..9fc71e3 100644 --- a/src/modules/flow/dcmi.c +++ b/src/modules/flow/dcmi.c @@ -38,8 +38,6 @@ #include #include #include "no_warnings.h" -#include "mavlink_bridge_header.h" -#include #include "dcmi.h" #include "timer.h" #include "stm32f4xx_gpio.h" From 11bdc5b463473d3a8adc54bfd018bd3ef543793b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Dec 2015 09:24:58 +0100 Subject: [PATCH 118/120] Flow sending: Condition on USB_SEND_VIDEO only --- src/modules/flow/main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/flow/main.c b/src/modules/flow/main.c index bff225c..76a213d 100644 --- a/src/modules/flow/main.c +++ b/src/modules/flow/main.c @@ -146,7 +146,7 @@ static void send_video_fn(void) { if (previous_image == NULL) return; /* transmit raw 8-bit image */ - if (FLOAT_AS_BOOL(global_data.param[PARAM_USB_SEND_VIDEO]) && !FLOAT_AS_BOOL(global_data.param[PARAM_VIDEO_ONLY])) + if (FLOAT_AS_BOOL(global_data.param[PARAM_USB_SEND_VIDEO])) { mavlink_send_image(previous_image); } From 991071a5f4b213ffb769284dd4d12ee9dd4f7a6f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Dec 2015 09:25:20 +0100 Subject: [PATCH 119/120] Remove unused config param --- src/modules/flow/settings.c | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/modules/flow/settings.c b/src/modules/flow/settings.c index 4cdeae6..f82d029 100644 --- a/src/modules/flow/settings.c +++ b/src/modules/flow/settings.c @@ -206,11 +206,6 @@ void global_data_reset_param_defaults(void){ strcpy(global_data.param_name[PARAM_USB_SEND_QUAL_0], "USB_SEND_Q_0"); global_data.param_access[PARAM_USB_SEND_QUAL_0] = READ_WRITE; - - global_data.param[PARAM_VIDEO_ONLY] = 1; - strcpy(global_data.param_name[PARAM_VIDEO_ONLY], "VIDEO_ONLY"); - global_data.param_access[PARAM_VIDEO_ONLY] = READ_WRITE; - global_data.param[PARAM_VIDEO_RATE] = 150; strcpy(global_data.param_name[PARAM_VIDEO_RATE], "VIDEO_RATE"); global_data.param_access[PARAM_VIDEO_RATE] = READ_WRITE; From d24c035965d7e4d6158f64ecf236bb0c6a8a4c22 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Dec 2015 09:34:38 +0100 Subject: [PATCH 120/120] Be less excessive on exposure --- src/modules/flow/main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/flow/main.c b/src/modules/flow/main.c index 76a213d..f77104b 100644 --- a/src/modules/flow/main.c +++ b/src/modules/flow/main.c @@ -233,7 +233,7 @@ int main(void) BuildCameraImageBuffer(image_buffer_8bit_5) }; camera_init(&cam_ctx, mt9v034_get_sensor_interface(), dcmi_get_transport_interface(), - mt9v034_get_clks_per_row(64, 4) * 1, mt9v034_get_clks_per_row(64, 4) * 512, 2.0, + mt9v034_get_clks_per_row(64, 4) * 1, mt9v034_get_clks_per_row(64, 4) * 128, 2.0, &img_stream_param, buffers, 5); }