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@99:c6665262fd3d, 2017-04-20 (annotated)

Committer:
hazheng
Date:
Thu Apr 20 16:23:19 2017 +0000
Revision:
99:c6665262fd3d
Parent:
97:0ed9ede9a995
Parent:
98:fc92bb37ee17
Child:
100:ffbeefc9e218
Merge

Who changed what in which revision?

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