Jordan Brack / SmartWheelsPublic

SmartWheels self-driving race car. Designed for NXP Cup. Uses FRDM-KL25Z, area-scan camera, and simple image processing to detect and navigate any NXP spec track.

Dependencies:   TSI USBDevice mbed-dev

Fork of SmartWheels by haofan Zheng

Hardwares/ArduCAM.cpp@100:ffbeefc9e218, 2017-04-20 (annotated)

Committer:
hazheng
Date:
Thu Apr 20 21:04:10 2017 +0000
Revision:
100:ffbeefc9e218
Parent:
99:c6665262fd3d 
Better version of Intersection detection.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hazheng 28:271fc8445e89 1 #include "ArduCAM.h"
hazheng 28:271fc8445e89 2
hazheng 100:ffbeefc9e218 3 #define SW_DEBUG
hazheng 29:f87d8790f57d 4 #include "GlobalVariable.h"
hazheng 56:7d3395ae022d 5 #include "SWCommon.h"
hazheng 56:7d3395ae022d 6
hazheng 29:f87d8790f57d 7
hazheng 32:5badeff825dc 8 #include "CamRegBuf.h"
hazheng 32:5badeff825dc 9
hazheng 41:7b21c5e3599e 10 #include "ArduUTFT.h"
hazheng 41:7b21c5e3599e 11
hazheng 98:fc92bb37ee17 12 #include <math.h>
hazheng 32:5badeff825dc 13
hazheng 57:0d8a155d511d 14 #define CAM_BLK_CAL_ACTIVE
hazheng 44:15de535c4005 15
hazheng 46:a5eb9bd3bb55 16 //#define CAM_DISP_DEBUG
hazheng 81:32bd7a25a699 17 //#define CAM_DISP_DEBUG_CENTER
hazheng 81:32bd7a25a699 18 //#define CAM_DISP_IMG
hazheng 46:a5eb9bd3bb55 19
hazheng 57:0d8a155d511d 20 #define IMG_PROC_SIGNAL 0xBB
hazheng 57:0d8a155d511d 21
hazheng 46:a5eb9bd3bb55 22 #ifdef __cplusplus
hazheng 46:a5eb9bd3bb55 23 extern "C" {
hazheng 46:a5eb9bd3bb55 24 #endif
hazheng 44:15de535c4005 25
hazheng 57:0d8a155d511d 26 const static uint8_t CAM_BLK_CAL_LEFT = ((RESOLUTION_WIDTH / 2) - 1);
hazheng 57:0d8a155d511d 27 const static uint8_t CAM_BLK_CAL_RIGHT = ((RESOLUTION_WIDTH / 2) + 1);
hazheng 100:ffbeefc9e218 28 const static uint8_t TERMINATE_WIDTH = static_cast<uint8_t>(RESOLUTION_WIDTH * 0.29f);
hazheng 57:0d8a155d511d 29
hazheng 46:a5eb9bd3bb55 30 static DigitalOut cam_cs(PIN_ACC_CS, 1);
hazheng 29:f87d8790f57d 31
hazheng 46:a5eb9bd3bb55 32 static uint8_t temp_mid_pos = RESOLUTION_WIDTH / 2;
hazheng 46:a5eb9bd3bb55 33 static uint8_t black_calibrate = 70;
hazheng 100:ffbeefc9e218 34 static uint8_t centerLine[CAM_ROI_UPPER_LIMIT * 2];
hazheng 100:ffbeefc9e218 35 static uint8_t leftLine[CAM_ROI_UPPER_LIMIT];
hazheng 100:ffbeefc9e218 36 static uint8_t rightLine[CAM_ROI_UPPER_LIMIT];
hazheng 100:ffbeefc9e218 37 static const uint8_t lineArrayEnd = CAM_ROI_UPPER_LIMIT - 1;
hazheng 100:ffbeefc9e218 38 static uint8_t is_encounter_terminate = 0;
hazheng 100:ffbeefc9e218 39 static uint8_t intersection_info = INTERSECTION_NULL;
hazheng 44:15de535c4005 40
hazheng 44:15de535c4005 41 void cal_black_calibrate();
hazheng 34:f79db3bc2f86 42
hazheng 34:f79db3bc2f86 43 inline void ardu_cam_spi_write_8(int address, int value)
hazheng 29:f87d8790f57d 44 {
hazheng 34:f79db3bc2f86 45 // take the SS pin low to select the chip:
hazheng 34:f79db3bc2f86 46 cam_cs = 0;
hazheng 34:f79db3bc2f86 47 // send in the address and value via SPI:
hazheng 34:f79db3bc2f86 48 g_spi_port.write(address | 0x80);
hazheng 34:f79db3bc2f86 49 g_spi_port.write(value);
hazheng 34:f79db3bc2f86 50 // take the SS pin high to de-select the chip:
hazheng 34:f79db3bc2f86 51 cam_cs = 1;
hazheng 34:f79db3bc2f86 52 }
hazheng 34:f79db3bc2f86 53
hazheng 34:f79db3bc2f86 54 inline uint8_t ardu_cam_spi_read_8(int address)
hazheng 34:f79db3bc2f86 55 {
hazheng 34:f79db3bc2f86 56 // take the SS pin low to select the chip:
hazheng 29:f87d8790f57d 57 cam_cs = 0;
hazheng 34:f79db3bc2f86 58 // send in the address and value via SPI:
hazheng 34:f79db3bc2f86 59 g_spi_port.write(address & 0x7F);
hazheng 34:f79db3bc2f86 60 uint8_t value = static_cast<uint8_t>(g_spi_port.write(0x00));
hazheng 34:f79db3bc2f86 61 // take the SS pin high to de-select the chip:
hazheng 29:f87d8790f57d 62 cam_cs = 1;
Bobymicjohn 97:0ed9ede9a995 63
hazheng 34:f79db3bc2f86 64 return value;
hazheng 34:f79db3bc2f86 65 }
hazheng 29:f87d8790f57d 66
hazheng 28:271fc8445e89 67 bool ardu_cam_init()
hazheng 28:271fc8445e89 68 {
hazheng 87:15fcf7891bf9 69 CamRegBuf * camReg = new CamRegBuf(CAM_SCCB_WRITE, CAM_SCCB_READ);
hazheng 36:7e747e19f660 70
hazheng 36:7e747e19f660 71 camReg->WriteRegSet(ResetProg);
hazheng 46:a5eb9bd3bb55 72 wait_ms(10);
hazheng 37:7074a6118d03 73 camReg->WriteRegSet(QVGA);
hazheng 44:15de535c4005 74 wait_ms(10);
hazheng 32:5badeff825dc 75
hazheng 36:7e747e19f660 76 #if defined(ARDUCAM_OV2640)
hazheng 32:5badeff825dc 77 camReg->SCCBWrite(0xff, 0x01);
hazheng 36:7e747e19f660 78 #endif
hazheng 32:5badeff825dc 79
hazheng 32:5badeff825dc 80 delete camReg;
hazheng 32:5badeff825dc 81 camReg = NULL;
hazheng 32:5badeff825dc 82
Bobymicjohn 97:0ed9ede9a995 83 uint8_t VerNum = ardu_cam_spi_read_8(ARDUCHIP_VER);
Bobymicjohn 97:0ed9ede9a995 84 VerNum = ardu_cam_spi_read_8(ARDUCHIP_VER);
hazheng 29:f87d8790f57d 85
hazheng 34:f79db3bc2f86 86 ardu_cam_spi_write_8(ARDUCHIP_TEST1, ARDUCHIP_TEST_MSG);
hazheng 34:f79db3bc2f86 87 uint8_t testV = ardu_cam_spi_read_8(ARDUCHIP_TEST1);
hazheng 35:ac4fcca21560 88 if(VerNum != ARDUCHIP_VER_NUM || testV != ARDUCHIP_TEST_MSG)
hazheng 29:f87d8790f57d 89 {
hazheng 29:f87d8790f57d 90 return false;
hazheng 29:f87d8790f57d 91 }
hazheng 28:271fc8445e89 92
hazheng 96:ec89c4d1383d 93 ardu_cam_spi_write_8(ARDUCHIP_CAP_CTRL, 0x00);
hazheng 29:f87d8790f57d 94
hazheng 44:15de535c4005 95 cal_black_calibrate();
hazheng 29:f87d8790f57d 96
hazheng 29:f87d8790f57d 97 return true;
hazheng 29:f87d8790f57d 98 }
hazheng 29:f87d8790f57d 99
hazheng 29:f87d8790f57d 100 void ardu_cam_start_capture()
hazheng 29:f87d8790f57d 101 {
hazheng 34:f79db3bc2f86 102 ardu_cam_spi_write_8(ARDUCHIP_FIFO, FIFO_CLEAR_MASK);
hazheng 48:f76b5e252444 103
hazheng 34:f79db3bc2f86 104 ardu_cam_spi_write_8(ARDUCHIP_FIFO, FIFO_START_MASK);
hazheng 29:f87d8790f57d 105 }
hazheng 29:f87d8790f57d 106
hazheng 32:5badeff825dc 107 uint32_t ardu_cam_get_fifo_length()
hazheng 32:5badeff825dc 108 {
hazheng 32:5badeff825dc 109 uint32_t len1,len2,len3,length=0;
hazheng 34:f79db3bc2f86 110 len1 = ardu_cam_spi_read_8(FIFO_SIZE1);
hazheng 34:f79db3bc2f86 111 len2 = ardu_cam_spi_read_8(FIFO_SIZE2);
hazheng 34:f79db3bc2f86 112 len3 = ardu_cam_spi_read_8(FIFO_SIZE3) & 0x07;
hazheng 32:5badeff825dc 113 length = ((len3 << 16) | (len2 << 8) | len1) & 0x07ffff;
hazheng 32:5badeff825dc 114 return length;
hazheng 32:5badeff825dc 115 }
hazheng 32:5badeff825dc 116
hazheng 32:5badeff825dc 117 uint8_t ardu_cam_get_pixel()
hazheng 32:5badeff825dc 118 {
hazheng 34:f79db3bc2f86 119 uint16_t VH = ardu_cam_spi_read_8(SINGLE_FIFO_READ);
hazheng 34:f79db3bc2f86 120 uint16_t VL = ardu_cam_spi_read_8(SINGLE_FIFO_READ);
hazheng 34:f79db3bc2f86 121 //uint16_t VL = ardu_cam_spi_burst_read_16();
hazheng 32:5badeff825dc 122
hazheng 32:5badeff825dc 123 VL = (VL & 0x00FF) | ((VH << 8) & 0xFF00);
hazheng 33:e3fcc4d6bb9b 124 uint8_t ch = ((VL & 0xF800) >> 9);// << 2;
hazheng 33:e3fcc4d6bb9b 125 float pixel = (static_cast<float>(ch) * 0.21);
hazheng 32:5badeff825dc 126
hazheng 33:e3fcc4d6bb9b 127 ch = ((VL & 0x07E0) >> 3);// << 2;
hazheng 33:e3fcc4d6bb9b 128 pixel += (static_cast<float>(ch) * 0.72);
hazheng 32:5badeff825dc 129
hazheng 33:e3fcc4d6bb9b 130 ch = (VL & 0x001F) << 2;
hazheng 33:e3fcc4d6bb9b 131 pixel += (static_cast<float>(ch) * 0.07);
hazheng 32:5badeff825dc 132
hazheng 32:5badeff825dc 133 return static_cast<uint8_t>(pixel);
hazheng 32:5badeff825dc 134 }
hazheng 41:7b21c5e3599e 135
hazheng 44:15de535c4005 136 uint8_t ardu_cam_is_capture_finished()
hazheng 44:15de535c4005 137 {
hazheng 44:15de535c4005 138 return (ardu_cam_spi_read_8(ARDUCHIP_TRIG) & CAP_DONE_MASK);
hazheng 44:15de535c4005 139 }
hazheng 44:15de535c4005 140
hazheng 44:15de535c4005 141 void cal_black_calibrate()
hazheng 44:15de535c4005 142 {
hazheng 44:15de535c4005 143 ardu_cam_start_capture();
hazheng 44:15de535c4005 144
hazheng 44:15de535c4005 145 while (!(ardu_cam_spi_read_8(ARDUCHIP_TRIG) & CAP_DONE_MASK));
hazheng 44:15de535c4005 146
hazheng 44:15de535c4005 147 float temp = 0.0f;
hazheng 44:15de535c4005 148 static uint16_t pixel = 0x00;
hazheng 44:15de535c4005 149 for (uint8_t j = 0; j < RESOLUTION_WIDTH; ++j)
hazheng 44:15de535c4005 150 {
hazheng 44:15de535c4005 151 pixel = static_cast<uint16_t>(ardu_cam_spi_read_8(SINGLE_FIFO_READ)) << 8;
hazheng 44:15de535c4005 152 pixel = pixel | ardu_cam_spi_read_8(SINGLE_FIFO_READ);
hazheng 46:a5eb9bd3bb55 153
hazheng 44:15de535c4005 154 if(CAM_BLK_CAL_LEFT < j && j <= CAM_BLK_CAL_RIGHT)
hazheng 44:15de535c4005 155 {
hazheng 44:15de535c4005 156 temp += static_cast<uint8_t>((pixel >> 3) & 0x00FF);
hazheng 44:15de535c4005 157 }
hazheng 44:15de535c4005 158 }
hazheng 44:15de535c4005 159 temp = temp / (CAM_BLK_CAL_RIGHT - CAM_BLK_CAL_LEFT);
hazheng 44:15de535c4005 160 black_calibrate = temp * 0.7f;
hazheng 44:15de535c4005 161 }
hazheng 44:15de535c4005 162
Bobymicjohn 97:0ed9ede9a995 163 inline void get_img_row_info(const uint8_t rowI, uint8_t * left, uint8_t * right, uint8_t* isBorderFound)
hazheng 44:15de535c4005 164 {
hazheng 44:15de535c4005 165 *left = 0;
hazheng 44:15de535c4005 166 *right = RESOLUTION_WIDTH;
hazheng 65:295c222fdf88 167 *isBorderFound = BOTH_NOT_FOUND;
hazheng 44:15de535c4005 168 uint8_t isRightFound = 0;
hazheng 44:15de535c4005 169 static uint16_t pixel = 0x0000;
hazheng 44:15de535c4005 170 static uint8_t pGreen = 0x00;
hazheng 44:15de535c4005 171
hazheng 44:15de535c4005 172 for (uint8_t j = 0; j < RESOLUTION_WIDTH; ++j)
hazheng 44:15de535c4005 173 {
hazheng 44:15de535c4005 174 pixel = static_cast<uint16_t>(ardu_cam_spi_read_8(SINGLE_FIFO_READ)) << 8;
hazheng 44:15de535c4005 175 pixel = pixel | ardu_cam_spi_read_8(SINGLE_FIFO_READ);
hazheng 46:a5eb9bd3bb55 176
hazheng 57:0d8a155d511d 177 pGreen = static_cast<uint8_t>((pixel & 0x07E0) >> 3);
hazheng 44:15de535c4005 178 if((pGreen < black_calibrate))
hazheng 44:15de535c4005 179 {
hazheng 44:15de535c4005 180 if(j < temp_mid_pos)
hazheng 44:15de535c4005 181 {
hazheng 65:295c222fdf88 182 *isBorderFound = LEFT_FOUND;
hazheng 44:15de535c4005 183 *left = j;
hazheng 46:a5eb9bd3bb55 184 #ifdef CAM_DISP_DEBUG
hazheng 46:a5eb9bd3bb55 185 ardu_utft_write_DATA(0xF8, 0x00);
hazheng 46:a5eb9bd3bb55 186 #endif
hazheng 44:15de535c4005 187 }
hazheng 44:15de535c4005 188 else if(!isRightFound)
hazheng 44:15de535c4005 189 {
hazheng 44:15de535c4005 190 *right = j;
hazheng 44:15de535c4005 191 isRightFound = 1;
hazheng 46:a5eb9bd3bb55 192 #ifdef CAM_DISP_DEBUG
hazheng 46:a5eb9bd3bb55 193 ardu_utft_write_DATA(0xF8, 0x00);
hazheng 46:a5eb9bd3bb55 194 #endif
hazheng 44:15de535c4005 195 }
hazheng 73:1dcf56e9f1d4 196 #ifdef CAM_DISP_DEBUG
hazheng 73:1dcf56e9f1d4 197 else
hazheng 73:1dcf56e9f1d4 198 {
hazheng 73:1dcf56e9f1d4 199 ardu_utft_write_DATA(static_cast<uint8_t>(pixel >> 8), static_cast<uint8_t>(pixel));
hazheng 73:1dcf56e9f1d4 200 }
hazheng 73:1dcf56e9f1d4 201 #endif
hazheng 44:15de535c4005 202 }
hazheng 46:a5eb9bd3bb55 203 #ifdef CAM_DISP_DEBUG
hazheng 46:a5eb9bd3bb55 204 else
hazheng 46:a5eb9bd3bb55 205 {
hazheng 46:a5eb9bd3bb55 206 ardu_utft_write_DATA(static_cast<uint8_t>(pixel >> 8), static_cast<uint8_t>(pixel));
hazheng 46:a5eb9bd3bb55 207 }
hazheng 50:c387c88141fb 208
hazheng 46:a5eb9bd3bb55 209 #elif defined(CAM_DISP_IMG)
hazheng 46:a5eb9bd3bb55 210 ardu_utft_write_DATA(static_cast<uint8_t>(pixel >> 8), static_cast<uint8_t>(pixel));
hazheng 46:a5eb9bd3bb55 211 #endif
hazheng 44:15de535c4005 212 }
hazheng 65:295c222fdf88 213
hazheng 65:295c222fdf88 214 if(*isBorderFound == LEFT_FOUND && isRightFound)
hazheng 65:295c222fdf88 215 {
hazheng 65:295c222fdf88 216 *isBorderFound = BOTH_FOUND;
hazheng 65:295c222fdf88 217 }
hazheng 65:295c222fdf88 218 else if(isRightFound)
hazheng 65:295c222fdf88 219 {
hazheng 65:295c222fdf88 220 *isBorderFound = RIGHT_FOUND;
hazheng 65:295c222fdf88 221 }
hazheng 65:295c222fdf88 222
hazheng 44:15de535c4005 223 }
hazheng 44:15de535c4005 224
hazheng 100:ffbeefc9e218 225 const uint8_t* ardu_cam_get_center_array()
hazheng 44:15de535c4005 226 {
hazheng 44:15de535c4005 227 return centerLine;
hazheng 44:15de535c4005 228 }
hazheng 44:15de535c4005 229
hazheng 44:15de535c4005 230 void image_processing()
hazheng 44:15de535c4005 231 {
hazheng 100:ffbeefc9e218 232 static uint8_t leftPos = 0;
hazheng 100:ffbeefc9e218 233 static uint8_t rightPos = 0;
hazheng 100:ffbeefc9e218 234 static uint8_t isBorderFound = BOTH_NOT_FOUND;
hazheng 63:d9a81b3d69f5 235
hazheng 100:ffbeefc9e218 236 static uint8_t leftNearCenter1 = lineArrayEnd;
hazheng 100:ffbeefc9e218 237 static uint8_t leftNearPos = 0;
hazheng 100:ffbeefc9e218 238 static uint8_t rightNearCenter1 = leftNearCenter1;
hazheng 100:ffbeefc9e218 239 static uint8_t rightNearPos = RESOLUTION_WIDTH;
hazheng 57:0d8a155d511d 240 //while(true)
hazheng 44:15de535c4005 241 {
hazheng 44:15de535c4005 242 ardu_cam_start_capture();
hazheng 44:15de535c4005 243
hazheng 44:15de535c4005 244 while (!(ardu_cam_spi_read_8(ARDUCHIP_TRIG) & CAP_DONE_MASK));
hazheng 44:15de535c4005 245
hazheng 44:15de535c4005 246 temp_mid_pos = RESOLUTION_WIDTH / 2;
hazheng 98:fc92bb37ee17 247 //is_border_find = BOTH_NOT_FOUND;
hazheng 57:0d8a155d511d 248 #ifdef CAM_BLK_CAL_ACTIVE
hazheng 57:0d8a155d511d 249 static float calTemp = 0.0f;
hazheng 57:0d8a155d511d 250 calTemp = 0.0f;
hazheng 57:0d8a155d511d 251 static uint16_t greenPixel = 0x00;
hazheng 100:ffbeefc9e218 252 static uint8_t isValid = 1;
hazheng 100:ffbeefc9e218 253 isValid = 1;
hazheng 57:0d8a155d511d 254 for (uint8_t j = 0; j < RESOLUTION_WIDTH; ++j)
hazheng 57:0d8a155d511d 255 {
hazheng 57:0d8a155d511d 256 if(CAM_BLK_CAL_LEFT < j && j <= CAM_BLK_CAL_RIGHT)
hazheng 57:0d8a155d511d 257 {
hazheng 57:0d8a155d511d 258 greenPixel = static_cast<uint16_t>(ardu_cam_spi_read_8(SINGLE_FIFO_READ)) << 8;
hazheng 57:0d8a155d511d 259 greenPixel = greenPixel | ardu_cam_spi_read_8(SINGLE_FIFO_READ);
hazheng 57:0d8a155d511d 260 greenPixel = (greenPixel & 0x07E0) >> 3;
hazheng 64:43ab429a37e0 261 if(static_cast<uint8_t>(greenPixel) < black_calibrate)
hazheng 64:43ab429a37e0 262 {
hazheng 64:43ab429a37e0 263 isValid = 0;
hazheng 64:43ab429a37e0 264 }
hazheng 57:0d8a155d511d 265 calTemp += static_cast<uint8_t>(greenPixel);
hazheng 57:0d8a155d511d 266 }
hazheng 57:0d8a155d511d 267 else
hazheng 57:0d8a155d511d 268 {
hazheng 57:0d8a155d511d 269 ardu_cam_spi_read_8(SINGLE_FIFO_READ);
hazheng 57:0d8a155d511d 270 ardu_cam_spi_read_8(SINGLE_FIFO_READ);
hazheng 57:0d8a155d511d 271 }
hazheng 57:0d8a155d511d 272 }
hazheng 64:43ab429a37e0 273
hazheng 64:43ab429a37e0 274 if(isValid)
hazheng 64:43ab429a37e0 275 {
hazheng 64:43ab429a37e0 276 calTemp = calTemp / static_cast<float>(CAM_BLK_CAL_RIGHT - CAM_BLK_CAL_LEFT);
hazheng 64:43ab429a37e0 277 black_calibrate = static_cast<uint8_t>(calTemp * 0.7f);
hazheng 64:43ab429a37e0 278 }
hazheng 57:0d8a155d511d 279 #endif
hazheng 44:15de535c4005 280 //cal_black_calibrate();
hazheng 100:ffbeefc9e218 281 intersection_info = INTERSECTION_NULL;
hazheng 98:fc92bb37ee17 282
hazheng 100:ffbeefc9e218 283 leftPos = 0;
hazheng 100:ffbeefc9e218 284 rightPos = 0;
hazheng 100:ffbeefc9e218 285 isBorderFound = BOTH_NOT_FOUND;
hazheng 100:ffbeefc9e218 286
hazheng 100:ffbeefc9e218 287 leftNearCenter1 = lineArrayEnd;
hazheng 100:ffbeefc9e218 288 leftNearPos = 0;
hazheng 100:ffbeefc9e218 289 rightNearCenter1 = leftNearCenter1;
hazheng 100:ffbeefc9e218 290 rightNearPos = RESOLUTION_WIDTH;
hazheng 98:fc92bb37ee17 291
hazheng 82:992ba6f31e24 292 is_encounter_terminate = 0;
hazheng 44:15de535c4005 293 for (uint8_t i = 0; i < CAM_ROI_UPPER_LIMIT; ++i)
hazheng 44:15de535c4005 294 {
hazheng 46:a5eb9bd3bb55 295 #if defined(CAM_DISP_IMG) or defined(CAM_DISP_DEBUG)
hazheng 46:a5eb9bd3bb55 296 ardu_utft_set_camimg_row(CAM_ROI_UPPER_LIMIT - i);
hazheng 46:a5eb9bd3bb55 297 #endif
Bobymicjohn 97:0ed9ede9a995 298 get_img_row_info(i, &leftPos, &rightPos, &isBorderFound);
hazheng 44:15de535c4005 299 temp_mid_pos = (leftPos + rightPos) / 2;
hazheng 100:ffbeefc9e218 300 static uint8_t dis_btw_left_right;
hazheng 100:ffbeefc9e218 301 dis_btw_left_right = rightPos - leftPos - 1;
hazheng 64:43ab429a37e0 302 if(dis_btw_left_right < TERMINATE_WIDTH)
hazheng 64:43ab429a37e0 303 {
hazheng 64:43ab429a37e0 304 is_encounter_terminate = 1;
hazheng 64:43ab429a37e0 305 }
hazheng 46:a5eb9bd3bb55 306
hazheng 46:a5eb9bd3bb55 307 #ifdef CAM_DISP_DEBUG_CENTER
hazheng 44:15de535c4005 308 ardu_utft_set_camimg_rowcol(CAM_ROI_UPPER_LIMIT - i, temp_mid_pos);
hazheng 44:15de535c4005 309 ardu_utft_write_DATA(0xF8, 0x00);
hazheng 44:15de535c4005 310 #endif
hazheng 100:ffbeefc9e218 311 static uint8_t array_pos;
hazheng 100:ffbeefc9e218 312 array_pos = lineArrayEnd - i;
hazheng 100:ffbeefc9e218 313 centerLine[array_pos] = temp_mid_pos;
hazheng 100:ffbeefc9e218 314 leftLine[array_pos] = leftPos;
hazheng 100:ffbeefc9e218 315 rightLine[array_pos] = rightPos;
hazheng 98:fc92bb37ee17 316
hazheng 100:ffbeefc9e218 317 centerLine[(2 * CAM_ROI_UPPER_LIMIT) - 1 - i] = isBorderFound;
hazheng 100:ffbeefc9e218 318
hazheng 100:ffbeefc9e218 319 if(leftPos >= leftNearPos)
hazheng 98:fc92bb37ee17 320 {
hazheng 100:ffbeefc9e218 321 leftNearCenter1 = lineArrayEnd - i;
hazheng 100:ffbeefc9e218 322 leftNearPos = leftPos;
hazheng 98:fc92bb37ee17 323 }
hazheng 100:ffbeefc9e218 324 if(rightPos <= rightNearPos)
hazheng 79:bdbac82c979b 325 {
hazheng 100:ffbeefc9e218 326 rightNearCenter1 = lineArrayEnd - i;
hazheng 100:ffbeefc9e218 327 rightNearPos = rightPos;
hazheng 79:bdbac82c979b 328 }
hazheng 44:15de535c4005 329 }
hazheng 100:ffbeefc9e218 330 if( (0 < rightNearCenter1 && rightNearCenter1 < lineArrayEnd)
hazheng 100:ffbeefc9e218 331 && ((rightLine[0] - rightLine[rightNearCenter1]) >= 4)
hazheng 100:ffbeefc9e218 332 )
hazheng 100:ffbeefc9e218 333 {
hazheng 100:ffbeefc9e218 334 intersection_info = INTERSECTION_IN_R;
hazheng 100:ffbeefc9e218 335 }
hazheng 98:fc92bb37ee17 336
hazheng 100:ffbeefc9e218 337 if( (0 < leftNearCenter1 && leftNearCenter1 < lineArrayEnd)
hazheng 100:ffbeefc9e218 338 && ((leftLine[leftNearCenter1] - leftLine[0]) >= 4)
hazheng 100:ffbeefc9e218 339 )
hazheng 98:fc92bb37ee17 340 {
hazheng 100:ffbeefc9e218 341 switch(intersection_info)
hazheng 100:ffbeefc9e218 342 {
hazheng 100:ffbeefc9e218 343 case INTERSECTION_IN_R:
hazheng 100:ffbeefc9e218 344 intersection_info = INTERSECTION_IN;
hazheng 100:ffbeefc9e218 345 break;
hazheng 100:ffbeefc9e218 346 default:
hazheng 100:ffbeefc9e218 347 intersection_info = INTERSECTION_IN_L;
hazheng 100:ffbeefc9e218 348 break;
hazheng 100:ffbeefc9e218 349 }
hazheng 98:fc92bb37ee17 350 }
hazheng 100:ffbeefc9e218 351 //LOGI("#%d,%d,%d#", rightNearCenter1, abs(rightLine[0] - rightLine[rightNearCenter1]), abs(rightLine[lineArrayEnd] - rightLine[rightNearCenter1]));
hazheng 44:15de535c4005 352 }
hazheng 44:15de535c4005 353 }
hazheng 46:a5eb9bd3bb55 354
hazheng 46:a5eb9bd3bb55 355
Bobymicjohn 97:0ed9ede9a995 356 inline void get_img_row_disp(const uint8_t rowI, uint8_t * left, uint8_t * right)
Bobymicjohn 97:0ed9ede9a995 357 {
Bobymicjohn 97:0ed9ede9a995 358 *left = 0;
Bobymicjohn 97:0ed9ede9a995 359 *right = RESOLUTION_WIDTH;
Bobymicjohn 97:0ed9ede9a995 360 uint8_t isRightFound = 0;
Bobymicjohn 97:0ed9ede9a995 361 static uint16_t pixel = 0x0000;
Bobymicjohn 97:0ed9ede9a995 362 static uint8_t pGreen = 0x00;
Bobymicjohn 97:0ed9ede9a995 363
Bobymicjohn 97:0ed9ede9a995 364 for (uint8_t j = 0; j < RESOLUTION_WIDTH; ++j)
Bobymicjohn 97:0ed9ede9a995 365 {
Bobymicjohn 97:0ed9ede9a995 366 pixel = static_cast<uint16_t>(ardu_cam_spi_read_8(SINGLE_FIFO_READ)) << 8;
Bobymicjohn 97:0ed9ede9a995 367 pixel = pixel | ardu_cam_spi_read_8(SINGLE_FIFO_READ);
Bobymicjohn 97:0ed9ede9a995 368
Bobymicjohn 97:0ed9ede9a995 369 pGreen = static_cast<uint8_t>((pixel & 0x07E0) >> 3);
Bobymicjohn 97:0ed9ede9a995 370 if((pGreen < black_calibrate))
Bobymicjohn 97:0ed9ede9a995 371 {
Bobymicjohn 97:0ed9ede9a995 372 if(j < temp_mid_pos)
Bobymicjohn 97:0ed9ede9a995 373 {
Bobymicjohn 97:0ed9ede9a995 374 *left = j;
Bobymicjohn 97:0ed9ede9a995 375 ardu_utft_write_DATA(0xF8, 0x00);
Bobymicjohn 97:0ed9ede9a995 376 }
Bobymicjohn 97:0ed9ede9a995 377 else if(!isRightFound)
Bobymicjohn 97:0ed9ede9a995 378 {
Bobymicjohn 97:0ed9ede9a995 379 *right = j;
Bobymicjohn 97:0ed9ede9a995 380 isRightFound = 1;
Bobymicjohn 97:0ed9ede9a995 381 ardu_utft_write_DATA(0xF8, 0x00);
Bobymicjohn 97:0ed9ede9a995 382 }
Bobymicjohn 97:0ed9ede9a995 383 else
Bobymicjohn 97:0ed9ede9a995 384 {
Bobymicjohn 97:0ed9ede9a995 385 ardu_utft_write_DATA(static_cast<uint8_t>(pixel >> 8), static_cast<uint8_t>(pixel));
Bobymicjohn 97:0ed9ede9a995 386 }
Bobymicjohn 97:0ed9ede9a995 387 }
Bobymicjohn 97:0ed9ede9a995 388 else
Bobymicjohn 97:0ed9ede9a995 389 {
Bobymicjohn 97:0ed9ede9a995 390 ardu_utft_write_DATA(static_cast<uint8_t>(pixel >> 8), static_cast<uint8_t>(pixel));
Bobymicjohn 97:0ed9ede9a995 391 }
Bobymicjohn 97:0ed9ede9a995 392 }
Bobymicjohn 97:0ed9ede9a995 393
Bobymicjohn 97:0ed9ede9a995 394 }
Bobymicjohn 97:0ed9ede9a995 395
hazheng 43:0d1886f4848a 396 void ardu_cam_display_img_utft()
hazheng 43:0d1886f4848a 397 {
hazheng 43:0d1886f4848a 398 ardu_cam_start_capture();
hazheng 44:15de535c4005 399
hazheng 44:15de535c4005 400 while (!(ardu_cam_spi_read_8(ARDUCHIP_TRIG) & CAP_DONE_MASK));
hazheng 44:15de535c4005 401
hazheng 44:15de535c4005 402 temp_mid_pos = RESOLUTION_WIDTH / 2;
Bobymicjohn 97:0ed9ede9a995 403 #ifdef CAM_BLK_CAL_ACTIVE
Bobymicjohn 97:0ed9ede9a995 404 static float calTemp = 0.0f;
Bobymicjohn 97:0ed9ede9a995 405 calTemp = 0.0f;
Bobymicjohn 97:0ed9ede9a995 406 static uint16_t greenPixel = 0x00;
Bobymicjohn 97:0ed9ede9a995 407 uint8_t isValid = 1;
Bobymicjohn 97:0ed9ede9a995 408 for (uint8_t j = 0; j < RESOLUTION_WIDTH; ++j)
Bobymicjohn 97:0ed9ede9a995 409 {
Bobymicjohn 97:0ed9ede9a995 410 if(CAM_BLK_CAL_LEFT < j && j <= CAM_BLK_CAL_RIGHT)
Bobymicjohn 97:0ed9ede9a995 411 {
Bobymicjohn 97:0ed9ede9a995 412 greenPixel = static_cast<uint16_t>(ardu_cam_spi_read_8(SINGLE_FIFO_READ)) << 8;
Bobymicjohn 97:0ed9ede9a995 413 greenPixel = greenPixel | ardu_cam_spi_read_8(SINGLE_FIFO_READ);
Bobymicjohn 97:0ed9ede9a995 414 greenPixel = (greenPixel & 0x07E0) >> 3;
Bobymicjohn 97:0ed9ede9a995 415 if(static_cast<uint8_t>(greenPixel) < black_calibrate)
Bobymicjohn 97:0ed9ede9a995 416 {
Bobymicjohn 97:0ed9ede9a995 417 isValid = 0;
Bobymicjohn 97:0ed9ede9a995 418 }
Bobymicjohn 97:0ed9ede9a995 419 calTemp += static_cast<uint8_t>(greenPixel);
Bobymicjohn 97:0ed9ede9a995 420 }
Bobymicjohn 97:0ed9ede9a995 421 else
Bobymicjohn 97:0ed9ede9a995 422 {
Bobymicjohn 97:0ed9ede9a995 423 ardu_cam_spi_read_8(SINGLE_FIFO_READ);
Bobymicjohn 97:0ed9ede9a995 424 ardu_cam_spi_read_8(SINGLE_FIFO_READ);
Bobymicjohn 97:0ed9ede9a995 425 }
Bobymicjohn 97:0ed9ede9a995 426 }
hazheng 46:a5eb9bd3bb55 427
Bobymicjohn 97:0ed9ede9a995 428 if(isValid)
Bobymicjohn 97:0ed9ede9a995 429 {
Bobymicjohn 97:0ed9ede9a995 430 calTemp = calTemp / static_cast<float>(CAM_BLK_CAL_RIGHT - CAM_BLK_CAL_LEFT);
Bobymicjohn 97:0ed9ede9a995 431 black_calibrate = static_cast<uint8_t>(calTemp * 0.7f);
Bobymicjohn 97:0ed9ede9a995 432 }
Bobymicjohn 97:0ed9ede9a995 433 #endif
Bobymicjohn 97:0ed9ede9a995 434 //cal_black_calibrate();
Bobymicjohn 97:0ed9ede9a995 435 uint8_t leftPos = 0;
Bobymicjohn 97:0ed9ede9a995 436 uint8_t rightPos = 0;
Bobymicjohn 97:0ed9ede9a995 437 is_encounter_terminate = 0;
hazheng 44:15de535c4005 438 for (uint8_t i = 0; i < CAM_ROI_UPPER_LIMIT; ++i)
hazheng 43:0d1886f4848a 439 {
hazheng 46:a5eb9bd3bb55 440 ardu_utft_set_camimg_row(CAM_ROI_UPPER_LIMIT - i);
hazheng 44:15de535c4005 441
Bobymicjohn 97:0ed9ede9a995 442 get_img_row_disp(i, &leftPos, &rightPos);
Bobymicjohn 97:0ed9ede9a995 443 temp_mid_pos = (leftPos + rightPos) / 2;
Bobymicjohn 97:0ed9ede9a995 444
Bobymicjohn 97:0ed9ede9a995 445 ardu_utft_set_camimg_rowcol(CAM_ROI_UPPER_LIMIT - i, temp_mid_pos);
Bobymicjohn 97:0ed9ede9a995 446 ardu_utft_write_DATA(0xF8, 0x00);
Bobymicjohn 97:0ed9ede9a995 447
hazheng 43:0d1886f4848a 448 }
hazheng 44:15de535c4005 449 }
hazheng 64:43ab429a37e0 450
hazheng 64:43ab429a37e0 451 uint8_t ardu_cam_get_is_encounter_terminate()
hazheng 64:43ab429a37e0 452 {
hazheng 64:43ab429a37e0 453 return is_encounter_terminate;
hazheng 64:43ab429a37e0 454 }
hazheng 98:fc92bb37ee17 455 /*
hazheng 79:bdbac82c979b 456 uint8_t ardu_cam_get_is_border_found()
hazheng 79:bdbac82c979b 457 {
hazheng 79:bdbac82c979b 458 return is_border_find;
hazheng 79:bdbac82c979b 459 }
hazheng 98:fc92bb37ee17 460 */
hazheng 96:ec89c4d1383d 461
Bobymicjohn 97:0ed9ede9a995 462 uint8_t ardu_cam_get_ver_num()
Bobymicjohn 97:0ed9ede9a995 463 {
Bobymicjohn 97:0ed9ede9a995 464 return ardu_cam_spi_read_8(ARDUCHIP_VER);
Bobymicjohn 97:0ed9ede9a995 465 }
Bobymicjohn 97:0ed9ede9a995 466
hazheng 100:ffbeefc9e218 467 uint8_t ardu_cam_get_intersection_info()
hazheng 98:fc92bb37ee17 468 {
hazheng 100:ffbeefc9e218 469 return intersection_info;
hazheng 98:fc92bb37ee17 470 }
hazheng 99:c6665262fd3d 471
hazheng 46:a5eb9bd3bb55 472 #ifdef __cplusplus
hazheng 46:a5eb9bd3bb55 473 }
hazheng 46:a5eb9bd3bb55 474 #endif
hazheng 46:a5eb9bd3bb55 475
hazheng 46:a5eb9bd3bb55 476
hazheng 46:a5eb9bd3bb55 477