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@80:c85cb93713b3, 2017-04-18 (annotated)

Committer:
hazheng
Date:
Tue Apr 18 19:26:33 2017 +0000
Branch:
Drift
Revision:
80:c85cb93713b3
Parent:
79:bdbac82c979b
Child:
81:32bd7a25a699
Transfer to State Machine.

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 32:5badeff825dc 12 #include <string>
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 80:c85cb93713b3 17 #define CAM_DISP_DEBUG_CENTER
hazheng 80:c85cb93713b3 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 79:bdbac82c979b 36 static volatile uint8_t is_border_find = BOTH_NOT_FOUND;
hazheng 63:d9a81b3d69f5 37 //static Thread * m_imgProcessThread = NULL;
hazheng 63:d9a81b3d69f5 38 //static Ticker m_tickerImgProc;
hazheng 44:15de535c4005 39
hazheng 57:0d8a155d511d 40 //void image_processing();
hazheng 44:15de535c4005 41 void cal_black_calibrate();
hazheng 63:d9a81b3d69f5 42 //void tick_image_proc();
hazheng 34:f79db3bc2f86 43
hazheng 34:f79db3bc2f86 44 inline void ardu_cam_spi_write_8(int address, int value)
hazheng 29:f87d8790f57d 45 {
hazheng 34:f79db3bc2f86 46 // take the SS pin low to select the chip:
hazheng 56:7d3395ae022d 47 #ifdef SW_DEBUG
hazheng 56:7d3395ae022d 48 g_sw_spi_lock.lock();
hazheng 56:7d3395ae022d 49 #endif
hazheng 34:f79db3bc2f86 50 cam_cs = 0;
hazheng 34:f79db3bc2f86 51 // send in the address and value via SPI:
hazheng 34:f79db3bc2f86 52 g_spi_port.write(address | 0x80);
hazheng 34:f79db3bc2f86 53 g_spi_port.write(value);
hazheng 34:f79db3bc2f86 54 // take the SS pin high to de-select the chip:
hazheng 34:f79db3bc2f86 55 cam_cs = 1;
hazheng 56:7d3395ae022d 56 #ifdef SW_DEBUG
hazheng 56:7d3395ae022d 57 g_sw_spi_lock.unlock();
hazheng 56:7d3395ae022d 58 #endif
hazheng 34:f79db3bc2f86 59 }
hazheng 34:f79db3bc2f86 60
hazheng 34:f79db3bc2f86 61 inline uint8_t ardu_cam_spi_read_8(int address)
hazheng 34:f79db3bc2f86 62 {
hazheng 34:f79db3bc2f86 63 // take the SS pin low to select the chip:
hazheng 56:7d3395ae022d 64 #ifdef SW_DEBUG
hazheng 56:7d3395ae022d 65 g_sw_spi_lock.lock();
hazheng 56:7d3395ae022d 66 #endif
hazheng 29:f87d8790f57d 67 cam_cs = 0;
hazheng 34:f79db3bc2f86 68 // send in the address and value via SPI:
hazheng 34:f79db3bc2f86 69 g_spi_port.write(address & 0x7F);
hazheng 34:f79db3bc2f86 70 uint8_t value = static_cast<uint8_t>(g_spi_port.write(0x00));
hazheng 34:f79db3bc2f86 71 // take the SS pin high to de-select the chip:
hazheng 29:f87d8790f57d 72 cam_cs = 1;
hazheng 56:7d3395ae022d 73 #ifdef SW_DEBUG
hazheng 56:7d3395ae022d 74 g_sw_spi_lock.unlock();
hazheng 56:7d3395ae022d 75 #endif
hazheng 34:f79db3bc2f86 76 return value;
hazheng 34:f79db3bc2f86 77 }
hazheng 34:f79db3bc2f86 78 /*
hazheng 34:f79db3bc2f86 79 inline void ardu_cam_spi_set_burst()
hazheng 34:f79db3bc2f86 80 {
hazheng 34:f79db3bc2f86 81 cam_cs = 0;
hazheng 34:f79db3bc2f86 82
hazheng 34:f79db3bc2f86 83 g_spi_port.write(BURST_FIFO_READ & 0x7F);
hazheng 29:f87d8790f57d 84
hazheng 34:f79db3bc2f86 85 cam_cs = 1;
hazheng 34:f79db3bc2f86 86 }
hazheng 34:f79db3bc2f86 87
hazheng 34:f79db3bc2f86 88 inline uint16_t ardu_cam_spi_burst_read_16()
hazheng 34:f79db3bc2f86 89 {
hazheng 34:f79db3bc2f86 90 cam_cs = 0;
hazheng 29:f87d8790f57d 91
hazheng 34:f79db3bc2f86 92 uint16_t value = static_cast<uint16_t>(g_spi_port.write(0x00));
hazheng 34:f79db3bc2f86 93 value = (value << 8) & 0xFF00;
hazheng 34:f79db3bc2f86 94 value = value | static_cast<uint16_t>(g_spi_port.write(0x00));
hazheng 34:f79db3bc2f86 95
hazheng 34:f79db3bc2f86 96 cam_cs = 1;
hazheng 34:f79db3bc2f86 97 return value;
hazheng 29:f87d8790f57d 98 }
hazheng 29:f87d8790f57d 99 */
hazheng 29:f87d8790f57d 100
hazheng 29:f87d8790f57d 101
hazheng 28:271fc8445e89 102 bool ardu_cam_init()
hazheng 28:271fc8445e89 103 {
hazheng 56:7d3395ae022d 104 //char buf[20];
hazheng 32:5badeff825dc 105
hazheng 36:7e747e19f660 106 CamRegBuf * camReg = new CamRegBuf(g_core, CAM_SCCB_WRITE, CAM_SCCB_READ);
hazheng 36:7e747e19f660 107
hazheng 36:7e747e19f660 108 camReg->WriteRegSet(ResetProg);
hazheng 46:a5eb9bd3bb55 109 wait_ms(10);
hazheng 37:7074a6118d03 110 camReg->WriteRegSet(QVGA);
hazheng 44:15de535c4005 111 wait_ms(10);
hazheng 32:5badeff825dc 112
hazheng 36:7e747e19f660 113 #if defined(ARDUCAM_OV2640)
hazheng 32:5badeff825dc 114 camReg->SCCBWrite(0xff, 0x01);
hazheng 36:7e747e19f660 115 #endif
Bobymicjohn 54:f1f5648dfacf 116 //uint8_t camVer = camReg->SCCBRead(CAM_PID_ADDR);
Bobymicjohn 54:f1f5648dfacf 117 //sprintf(buf, "Cam VerH %#x", camVer);
Bobymicjohn 54:f1f5648dfacf 118 //g_core.GetUSBServer().PushReliableMsg('D', buf);
hazheng 32:5badeff825dc 119
Bobymicjohn 54:f1f5648dfacf 120 //camVer = camReg->SCCBRead(CAM_VER_ADDR);
Bobymicjohn 54:f1f5648dfacf 121 //sprintf(buf, "Cam VerL %#x", camVer);
Bobymicjohn 54:f1f5648dfacf 122 //g_core.GetUSBServer().PushReliableMsg('D', buf);
hazheng 36:7e747e19f660 123
hazheng 36:7e747e19f660 124
hazheng 32:5badeff825dc 125 delete camReg;
hazheng 32:5badeff825dc 126 camReg = NULL;
hazheng 32:5badeff825dc 127
hazheng 32:5badeff825dc 128
hazheng 32:5badeff825dc 129
hazheng 32:5badeff825dc 130
hazheng 34:f79db3bc2f86 131 uint8_t VerNum = ardu_cam_spi_read_8(0x40);
hazheng 34:f79db3bc2f86 132 VerNum = ardu_cam_spi_read_8(0x40);
hazheng 29:f87d8790f57d 133
hazheng 32:5badeff825dc 134
Bobymicjohn 54:f1f5648dfacf 135 //sprintf(buf, "Ardu Ver %#x", VerNum);
Bobymicjohn 54:f1f5648dfacf 136 //g_core.GetUSBServer().PushReliableMsg('D', buf);
hazheng 29:f87d8790f57d 137
hazheng 34:f79db3bc2f86 138 ardu_cam_spi_write_8(ARDUCHIP_TEST1, ARDUCHIP_TEST_MSG);
hazheng 34:f79db3bc2f86 139 uint8_t testV = ardu_cam_spi_read_8(ARDUCHIP_TEST1);
hazheng 35:ac4fcca21560 140 if(VerNum != ARDUCHIP_VER_NUM || testV != ARDUCHIP_TEST_MSG)
hazheng 29:f87d8790f57d 141 {
Bobymicjohn 54:f1f5648dfacf 142 //g_core.GetUSBServer().PushReliableMsg('D', "CameraInit Fa");
hazheng 29:f87d8790f57d 143 return false;
hazheng 29:f87d8790f57d 144 }
Bobymicjohn 54:f1f5648dfacf 145 //g_core.GetUSBServer().PushReliableMsg('D', "CameraInit Su");
hazheng 29:f87d8790f57d 146
hazheng 35:ac4fcca21560 147 //ardu_cam_set_mode(MCU2LCD_MODE);
hazheng 43:0d1886f4848a 148 ardu_cam_spi_write_8(ARDUCHIP_CAP_CTRL, 0x00);
hazheng 44:15de535c4005 149 //ardu_cam_start_capture();
hazheng 29:f87d8790f57d 150
hazheng 28:271fc8445e89 151
hazheng 32:5badeff825dc 152 //unsigned char tempV = ardu_cam_read_reg(ARDUCHIP_MODE);
hazheng 32:5badeff825dc 153 //sprintf(buf, "Ardu Stat %#x", tempV);
hazheng 32:5badeff825dc 154 //g_core.GetUSBServer().PushReliableMsg('D', buf);
hazheng 29:f87d8790f57d 155
hazheng 32:5badeff825dc 156 //tempV = ardu_cam_read_reg(ARDUCHIP_CAP_CTRL);
hazheng 32:5badeff825dc 157 //sprintf(buf, "Ardu FS1 %#x", tempV);
hazheng 32:5badeff825dc 158 //g_core.GetUSBServer().PushReliableMsg('D', buf);
hazheng 44:15de535c4005 159 cal_black_calibrate();
hazheng 57:0d8a155d511d 160 //m_imgProcessThread = new Thread(osPriorityRealtime); //osPriorityAboveNormal //osPriorityHigh //osPriorityRealtime
hazheng 57:0d8a155d511d 161 //m_imgProcessThread->set_priority(osPriorityRealtime);
hazheng 57:0d8a155d511d 162 //m_imgProcessThread->start(callback(image_processing));
hazheng 57:0d8a155d511d 163 //m_tickerImgProc.attach(&tick_image_proc, 0.1f);
hazheng 29:f87d8790f57d 164
hazheng 29:f87d8790f57d 165 return true;
hazheng 29:f87d8790f57d 166 }
hazheng 29:f87d8790f57d 167
hazheng 29:f87d8790f57d 168 void ardu_cam_start_capture()
hazheng 29:f87d8790f57d 169 {
hazheng 34:f79db3bc2f86 170 ardu_cam_spi_write_8(ARDUCHIP_FIFO, FIFO_CLEAR_MASK);
hazheng 48:f76b5e252444 171
hazheng 34:f79db3bc2f86 172 ardu_cam_spi_write_8(ARDUCHIP_FIFO, FIFO_START_MASK);
hazheng 29:f87d8790f57d 173 }
hazheng 29:f87d8790f57d 174
hazheng 32:5badeff825dc 175 uint32_t ardu_cam_get_fifo_length()
hazheng 32:5badeff825dc 176 {
hazheng 32:5badeff825dc 177 uint32_t len1,len2,len3,length=0;
hazheng 34:f79db3bc2f86 178 len1 = ardu_cam_spi_read_8(FIFO_SIZE1);
hazheng 34:f79db3bc2f86 179 len2 = ardu_cam_spi_read_8(FIFO_SIZE2);
hazheng 34:f79db3bc2f86 180 len3 = ardu_cam_spi_read_8(FIFO_SIZE3) & 0x07;
hazheng 32:5badeff825dc 181 length = ((len3 << 16) | (len2 << 8) | len1) & 0x07ffff;
hazheng 32:5badeff825dc 182 return length;
hazheng 32:5badeff825dc 183 }
hazheng 32:5badeff825dc 184
hazheng 32:5badeff825dc 185 uint8_t ardu_cam_get_pixel()
hazheng 32:5badeff825dc 186 {
hazheng 34:f79db3bc2f86 187 uint16_t VH = ardu_cam_spi_read_8(SINGLE_FIFO_READ);
hazheng 34:f79db3bc2f86 188 uint16_t VL = ardu_cam_spi_read_8(SINGLE_FIFO_READ);
hazheng 34:f79db3bc2f86 189 //uint16_t VL = ardu_cam_spi_burst_read_16();
hazheng 32:5badeff825dc 190
hazheng 32:5badeff825dc 191 VL = (VL & 0x00FF) | ((VH << 8) & 0xFF00);
hazheng 33:e3fcc4d6bb9b 192 uint8_t ch = ((VL & 0xF800) >> 9);// << 2;
hazheng 33:e3fcc4d6bb9b 193 float pixel = (static_cast<float>(ch) * 0.21);
hazheng 32:5badeff825dc 194
hazheng 33:e3fcc4d6bb9b 195 ch = ((VL & 0x07E0) >> 3);// << 2;
hazheng 33:e3fcc4d6bb9b 196 pixel += (static_cast<float>(ch) * 0.72);
hazheng 32:5badeff825dc 197
hazheng 33:e3fcc4d6bb9b 198 ch = (VL & 0x001F) << 2;
hazheng 33:e3fcc4d6bb9b 199 pixel += (static_cast<float>(ch) * 0.07);
hazheng 32:5badeff825dc 200
hazheng 32:5badeff825dc 201 return static_cast<uint8_t>(pixel);
hazheng 32:5badeff825dc 202 }
hazheng 32:5badeff825dc 203
hazheng 32:5badeff825dc 204 void ardu_cam_print_debug()
hazheng 32:5badeff825dc 205 {
hazheng 32:5badeff825dc 206 uint32_t len = ardu_cam_get_fifo_length();
Bobymicjohn 54:f1f5648dfacf 207 //char buf[20];
Bobymicjohn 54:f1f5648dfacf 208 //sprintf(buf, "Cam L %#x", len);
Bobymicjohn 54:f1f5648dfacf 209 //g_core.GetUSBServer().PushReliableMsg('D', buf);
hazheng 32:5badeff825dc 210
hazheng 33:e3fcc4d6bb9b 211 //if(len < (RESOLUTION_HEIGHT * RESOLUTION_WIDTH * 2))
hazheng 33:e3fcc4d6bb9b 212 // return;
hazheng 33:e3fcc4d6bb9b 213
hazheng 33:e3fcc4d6bb9b 214 //Begin output the picture
hazheng 32:5badeff825dc 215 std::string lineBuf;
hazheng 37:7074a6118d03 216 #if defined(MANUAL_REDUCE_RESULOTION_BY2)
hazheng 37:7074a6118d03 217 lineBuf.resize((RESOLUTION_WIDTH / 2) + 1);
hazheng 37:7074a6118d03 218 #else
hazheng 33:e3fcc4d6bb9b 219 lineBuf.resize(RESOLUTION_WIDTH + 1);
hazheng 37:7074a6118d03 220 #endif
hazheng 32:5badeff825dc 221
hazheng 34:f79db3bc2f86 222 //ardu_cam_spi_set_burst();
hazheng 41:7b21c5e3599e 223
hazheng 42:c4e1606087ff 224 //ardu_cam_get_pixel(); //Get the first dummy pixel
hazheng 41:7b21c5e3599e 225
hazheng 43:0d1886f4848a 226
hazheng 33:e3fcc4d6bb9b 227 for (uint8_t i = 0; i < RESOLUTION_HEIGHT; ++i)
hazheng 32:5badeff825dc 228 {
hazheng 37:7074a6118d03 229 #if defined(MANUAL_REDUCE_RESULOTION_BY2)
hazheng 37:7074a6118d03 230 lineBuf[0] = i / 2;
hazheng 37:7074a6118d03 231
hazheng 37:7074a6118d03 232 for (int j = 0; j < RESOLUTION_WIDTH; ++j)
hazheng 37:7074a6118d03 233 {
hazheng 37:7074a6118d03 234 if(i % 2 == 0 || j % 2 == 0)
hazheng 37:7074a6118d03 235 {
hazheng 37:7074a6118d03 236 ardu_cam_get_pixel();
hazheng 37:7074a6118d03 237 }
hazheng 37:7074a6118d03 238 else
hazheng 37:7074a6118d03 239 {
hazheng 37:7074a6118d03 240 lineBuf[(j / 2) + 1] = ardu_cam_get_pixel();
hazheng 37:7074a6118d03 241 }
hazheng 37:7074a6118d03 242 }
hazheng 37:7074a6118d03 243
hazheng 37:7074a6118d03 244 if(i % 2 == 0)
hazheng 37:7074a6118d03 245 {
hazheng 37:7074a6118d03 246
hazheng 37:7074a6118d03 247 }
hazheng 37:7074a6118d03 248 else
hazheng 37:7074a6118d03 249 {
hazheng 37:7074a6118d03 250 g_core.GetUSBServer().PushReliableMsg('P', lineBuf);
hazheng 37:7074a6118d03 251 wait(0.35);
hazheng 37:7074a6118d03 252 }
hazheng 37:7074a6118d03 253
hazheng 37:7074a6118d03 254 #else
hazheng 37:7074a6118d03 255
hazheng 32:5badeff825dc 256 lineBuf[0] = i;
hazheng 32:5badeff825dc 257
hazheng 33:e3fcc4d6bb9b 258 for (int j = 0; j < RESOLUTION_WIDTH; ++j)
hazheng 32:5badeff825dc 259 {
hazheng 33:e3fcc4d6bb9b 260 //uint8_t p = ardu_cam_get_pixel();
hazheng 33:e3fcc4d6bb9b 261 lineBuf[j + 1] = ardu_cam_get_pixel();
hazheng 32:5badeff825dc 262 }
hazheng 32:5badeff825dc 263
Bobymicjohn 54:f1f5648dfacf 264 //g_core.GetUSBServer().PushReliableMsg('P', lineBuf);
hazheng 35:ac4fcca21560 265 wait(0.35);
hazheng 37:7074a6118d03 266
hazheng 37:7074a6118d03 267 #endif
hazheng 37:7074a6118d03 268
hazheng 32:5badeff825dc 269 }
hazheng 43:0d1886f4848a 270
hazheng 43:0d1886f4848a 271 }
hazheng 43:0d1886f4848a 272
hazheng 44:15de535c4005 273 uint8_t ardu_cam_is_capture_finished()
hazheng 44:15de535c4005 274 {
hazheng 44:15de535c4005 275 return (ardu_cam_spi_read_8(ARDUCHIP_TRIG) & CAP_DONE_MASK);
hazheng 44:15de535c4005 276 }
hazheng 44:15de535c4005 277
hazheng 44:15de535c4005 278 void cal_black_calibrate()
hazheng 44:15de535c4005 279 {
hazheng 44:15de535c4005 280 ardu_cam_start_capture();
hazheng 44:15de535c4005 281
hazheng 44:15de535c4005 282 while (!(ardu_cam_spi_read_8(ARDUCHIP_TRIG) & CAP_DONE_MASK));
hazheng 44:15de535c4005 283
hazheng 44:15de535c4005 284 float temp = 0.0f;
hazheng 44:15de535c4005 285 static uint16_t pixel = 0x00;
hazheng 44:15de535c4005 286 for (uint8_t j = 0; j < RESOLUTION_WIDTH; ++j)
hazheng 44:15de535c4005 287 {
hazheng 44:15de535c4005 288 pixel = static_cast<uint16_t>(ardu_cam_spi_read_8(SINGLE_FIFO_READ)) << 8;
hazheng 44:15de535c4005 289 pixel = pixel | ardu_cam_spi_read_8(SINGLE_FIFO_READ);
hazheng 46:a5eb9bd3bb55 290
hazheng 44:15de535c4005 291 if(CAM_BLK_CAL_LEFT < j && j <= CAM_BLK_CAL_RIGHT)
hazheng 44:15de535c4005 292 {
hazheng 44:15de535c4005 293 temp += static_cast<uint8_t>((pixel >> 3) & 0x00FF);
hazheng 44:15de535c4005 294 }
hazheng 44:15de535c4005 295 }
hazheng 44:15de535c4005 296 temp = temp / (CAM_BLK_CAL_RIGHT - CAM_BLK_CAL_LEFT);
hazheng 44:15de535c4005 297 black_calibrate = temp * 0.7f;
hazheng 44:15de535c4005 298 }
hazheng 44:15de535c4005 299
hazheng 65:295c222fdf88 300 inline void get_img_row_info(const uint8_t display, const uint8_t rowI, uint8_t * left, uint8_t * right, uint8_t* isBorderFound)
hazheng 44:15de535c4005 301 {
hazheng 44:15de535c4005 302 *left = 0;
hazheng 44:15de535c4005 303 *right = RESOLUTION_WIDTH;
hazheng 65:295c222fdf88 304 *isBorderFound = BOTH_NOT_FOUND;
hazheng 44:15de535c4005 305 uint8_t isRightFound = 0;
hazheng 44:15de535c4005 306 static uint16_t pixel = 0x0000;
hazheng 44:15de535c4005 307 static uint8_t pGreen = 0x00;
hazheng 44:15de535c4005 308
hazheng 44:15de535c4005 309 for (uint8_t j = 0; j < RESOLUTION_WIDTH; ++j)
hazheng 44:15de535c4005 310 {
hazheng 44:15de535c4005 311 pixel = static_cast<uint16_t>(ardu_cam_spi_read_8(SINGLE_FIFO_READ)) << 8;
hazheng 44:15de535c4005 312 pixel = pixel | ardu_cam_spi_read_8(SINGLE_FIFO_READ);
hazheng 46:a5eb9bd3bb55 313
hazheng 57:0d8a155d511d 314 pGreen = static_cast<uint8_t>((pixel & 0x07E0) >> 3);
hazheng 44:15de535c4005 315 if((pGreen < black_calibrate))
hazheng 44:15de535c4005 316 {
hazheng 44:15de535c4005 317 if(j < temp_mid_pos)
hazheng 44:15de535c4005 318 {
hazheng 65:295c222fdf88 319 *isBorderFound = LEFT_FOUND;
hazheng 44:15de535c4005 320 *left = j;
hazheng 46:a5eb9bd3bb55 321 #ifdef CAM_DISP_DEBUG
hazheng 46:a5eb9bd3bb55 322 ardu_utft_write_DATA(0xF8, 0x00);
hazheng 46:a5eb9bd3bb55 323 #endif
hazheng 44:15de535c4005 324 }
hazheng 44:15de535c4005 325 else if(!isRightFound)
hazheng 44:15de535c4005 326 {
hazheng 44:15de535c4005 327 *right = j;
hazheng 44:15de535c4005 328 isRightFound = 1;
hazheng 46:a5eb9bd3bb55 329 #ifdef CAM_DISP_DEBUG
hazheng 46:a5eb9bd3bb55 330 ardu_utft_write_DATA(0xF8, 0x00);
hazheng 46:a5eb9bd3bb55 331 #endif
hazheng 44:15de535c4005 332 }
hazheng 73:1dcf56e9f1d4 333 #ifdef CAM_DISP_DEBUG
hazheng 73:1dcf56e9f1d4 334 else
hazheng 73:1dcf56e9f1d4 335 {
hazheng 73:1dcf56e9f1d4 336 ardu_utft_write_DATA(static_cast<uint8_t>(pixel >> 8), static_cast<uint8_t>(pixel));
hazheng 73:1dcf56e9f1d4 337 }
hazheng 73:1dcf56e9f1d4 338 #endif
hazheng 44:15de535c4005 339 }
hazheng 46:a5eb9bd3bb55 340 #ifdef CAM_DISP_DEBUG
hazheng 46:a5eb9bd3bb55 341 else
hazheng 46:a5eb9bd3bb55 342 {
hazheng 46:a5eb9bd3bb55 343 ardu_utft_write_DATA(static_cast<uint8_t>(pixel >> 8), static_cast<uint8_t>(pixel));
hazheng 46:a5eb9bd3bb55 344 }
hazheng 50:c387c88141fb 345
hazheng 46:a5eb9bd3bb55 346 #elif defined(CAM_DISP_IMG)
hazheng 46:a5eb9bd3bb55 347 ardu_utft_write_DATA(static_cast<uint8_t>(pixel >> 8), static_cast<uint8_t>(pixel));
hazheng 46:a5eb9bd3bb55 348 #endif
hazheng 44:15de535c4005 349 }
hazheng 65:295c222fdf88 350
hazheng 65:295c222fdf88 351 if(*isBorderFound == LEFT_FOUND && isRightFound)
hazheng 65:295c222fdf88 352 {
hazheng 65:295c222fdf88 353 *isBorderFound = BOTH_FOUND;
hazheng 65:295c222fdf88 354 }
hazheng 65:295c222fdf88 355 else if(isRightFound)
hazheng 65:295c222fdf88 356 {
hazheng 65:295c222fdf88 357 *isBorderFound = RIGHT_FOUND;
hazheng 65:295c222fdf88 358 }
hazheng 65:295c222fdf88 359
hazheng 44:15de535c4005 360 }
hazheng 44:15de535c4005 361
hazheng 44:15de535c4005 362 volatile const uint8_t* ardu_cam_get_center_array()
hazheng 44:15de535c4005 363 {
hazheng 44:15de535c4005 364 return centerLine;
hazheng 44:15de535c4005 365 }
hazheng 44:15de535c4005 366
hazheng 44:15de535c4005 367 void image_processing()
hazheng 44:15de535c4005 368 {
hazheng 63:d9a81b3d69f5 369
hazheng 57:0d8a155d511d 370 //while(true)
hazheng 44:15de535c4005 371 {
hazheng 44:15de535c4005 372 ardu_cam_start_capture();
hazheng 44:15de535c4005 373
hazheng 44:15de535c4005 374 while (!(ardu_cam_spi_read_8(ARDUCHIP_TRIG) & CAP_DONE_MASK));
hazheng 44:15de535c4005 375
hazheng 44:15de535c4005 376 temp_mid_pos = RESOLUTION_WIDTH / 2;
hazheng 79:bdbac82c979b 377 is_border_find = BOTH_NOT_FOUND;
hazheng 57:0d8a155d511d 378 #ifdef CAM_BLK_CAL_ACTIVE
hazheng 57:0d8a155d511d 379 static float calTemp = 0.0f;
hazheng 57:0d8a155d511d 380 calTemp = 0.0f;
hazheng 57:0d8a155d511d 381 static uint16_t greenPixel = 0x00;
hazheng 64:43ab429a37e0 382 uint8_t isValid = 1;
hazheng 57:0d8a155d511d 383 for (uint8_t j = 0; j < RESOLUTION_WIDTH; ++j)
hazheng 57:0d8a155d511d 384 {
hazheng 57:0d8a155d511d 385 if(CAM_BLK_CAL_LEFT < j && j <= CAM_BLK_CAL_RIGHT)
hazheng 57:0d8a155d511d 386 {
hazheng 57:0d8a155d511d 387 greenPixel = static_cast<uint16_t>(ardu_cam_spi_read_8(SINGLE_FIFO_READ)) << 8;
hazheng 57:0d8a155d511d 388 greenPixel = greenPixel | ardu_cam_spi_read_8(SINGLE_FIFO_READ);
hazheng 57:0d8a155d511d 389 greenPixel = (greenPixel & 0x07E0) >> 3;
hazheng 64:43ab429a37e0 390 if(static_cast<uint8_t>(greenPixel) < black_calibrate)
hazheng 64:43ab429a37e0 391 {
hazheng 64:43ab429a37e0 392 isValid = 0;
hazheng 64:43ab429a37e0 393 }
hazheng 57:0d8a155d511d 394 calTemp += static_cast<uint8_t>(greenPixel);
hazheng 57:0d8a155d511d 395 }
hazheng 57:0d8a155d511d 396 else
hazheng 57:0d8a155d511d 397 {
hazheng 57:0d8a155d511d 398 ardu_cam_spi_read_8(SINGLE_FIFO_READ);
hazheng 57:0d8a155d511d 399 ardu_cam_spi_read_8(SINGLE_FIFO_READ);
hazheng 57:0d8a155d511d 400 }
hazheng 57:0d8a155d511d 401 }
hazheng 64:43ab429a37e0 402
hazheng 64:43ab429a37e0 403 if(isValid)
hazheng 64:43ab429a37e0 404 {
hazheng 64:43ab429a37e0 405 calTemp = calTemp / static_cast<float>(CAM_BLK_CAL_RIGHT - CAM_BLK_CAL_LEFT);
hazheng 64:43ab429a37e0 406 black_calibrate = static_cast<uint8_t>(calTemp * 0.7f);
hazheng 64:43ab429a37e0 407 }
hazheng 57:0d8a155d511d 408 #endif
hazheng 44:15de535c4005 409 //cal_black_calibrate();
hazheng 44:15de535c4005 410 uint8_t leftPos = 0;
hazheng 44:15de535c4005 411 uint8_t rightPos = 0;
hazheng 65:295c222fdf88 412 uint8_t isBorderFound = BOTH_NOT_FOUND;
hazheng 65:295c222fdf88 413 //is_encounter_terminate = 0;
hazheng 44:15de535c4005 414 for (uint8_t i = 0; i < CAM_ROI_UPPER_LIMIT; ++i)
hazheng 57:0d8a155d511d 415 /*{
hazheng 57:0d8a155d511d 416 for (uint8_t j = 0; j < RESOLUTION_WIDTH; ++j)
hazheng 57:0d8a155d511d 417 {
hazheng 57:0d8a155d511d 418 ardu_cam_spi_read_8(SINGLE_FIFO_READ);
hazheng 57:0d8a155d511d 419 ardu_cam_spi_read_8(SINGLE_FIFO_READ);
hazheng 57:0d8a155d511d 420 }
hazheng 57:0d8a155d511d 421 }*/
hazheng 57:0d8a155d511d 422 //for(uint8_t i = CAM_ROI_UPPER_LIMIT; i < (2 * CAM_ROI_UPPER_LIMIT); ++i)
hazheng 44:15de535c4005 423 {
hazheng 46:a5eb9bd3bb55 424 #if defined(CAM_DISP_IMG) or defined(CAM_DISP_DEBUG)
hazheng 46:a5eb9bd3bb55 425 ardu_utft_set_camimg_row(CAM_ROI_UPPER_LIMIT - i);
hazheng 46:a5eb9bd3bb55 426 #endif
hazheng 65:295c222fdf88 427 get_img_row_info(0, i, &leftPos, &rightPos, &isBorderFound);
hazheng 44:15de535c4005 428 temp_mid_pos = (leftPos + rightPos) / 2;
hazheng 64:43ab429a37e0 429 uint8_t dis_btw_left_right = rightPos - leftPos - 1;
hazheng 64:43ab429a37e0 430 if(dis_btw_left_right < TERMINATE_WIDTH)
hazheng 64:43ab429a37e0 431 {
hazheng 64:43ab429a37e0 432 is_encounter_terminate = 1;
hazheng 64:43ab429a37e0 433 }
hazheng 46:a5eb9bd3bb55 434
hazheng 46:a5eb9bd3bb55 435 #ifdef CAM_DISP_DEBUG_CENTER
hazheng 44:15de535c4005 436 ardu_utft_set_camimg_rowcol(CAM_ROI_UPPER_LIMIT - i, temp_mid_pos);
hazheng 44:15de535c4005 437 ardu_utft_write_DATA(0xF8, 0x00);
hazheng 44:15de535c4005 438 #endif
hazheng 46:a5eb9bd3bb55 439
hazheng 44:15de535c4005 440 centerLine[CAM_ROI_UPPER_LIMIT - i - 1] = temp_mid_pos;
hazheng 65:295c222fdf88 441 centerLine[(2 * CAM_ROI_UPPER_LIMIT) - i - 1] = isBorderFound;
hazheng 79:bdbac82c979b 442 switch(is_border_find)
hazheng 79:bdbac82c979b 443 {
hazheng 79:bdbac82c979b 444 case BOTH_NOT_FOUND:
hazheng 79:bdbac82c979b 445 switch(isBorderFound)
hazheng 79:bdbac82c979b 446 {
hazheng 79:bdbac82c979b 447 case LEFT_FOUND:
hazheng 79:bdbac82c979b 448 is_border_find = LEFT_FOUND;
hazheng 79:bdbac82c979b 449 break;
hazheng 79:bdbac82c979b 450 case RIGHT_FOUND:
hazheng 79:bdbac82c979b 451 is_border_find = RIGHT_FOUND;
hazheng 79:bdbac82c979b 452 break;
hazheng 79:bdbac82c979b 453 case BOTH_FOUND:
hazheng 79:bdbac82c979b 454 is_border_find = BOTH_FOUND;
hazheng 79:bdbac82c979b 455 break;
hazheng 79:bdbac82c979b 456 default:
hazheng 79:bdbac82c979b 457 break;
hazheng 79:bdbac82c979b 458 }
hazheng 79:bdbac82c979b 459 break;
hazheng 79:bdbac82c979b 460 case LEFT_FOUND:
hazheng 79:bdbac82c979b 461 switch(isBorderFound)
hazheng 79:bdbac82c979b 462 {
hazheng 79:bdbac82c979b 463 case RIGHT_FOUND:
hazheng 79:bdbac82c979b 464 case BOTH_FOUND:
hazheng 79:bdbac82c979b 465 is_border_find = BOTH_FOUND;
hazheng 79:bdbac82c979b 466 break;
hazheng 79:bdbac82c979b 467 default:
hazheng 79:bdbac82c979b 468 break;
hazheng 79:bdbac82c979b 469 }
hazheng 79:bdbac82c979b 470 break;
hazheng 79:bdbac82c979b 471 case RIGHT_FOUND:
hazheng 79:bdbac82c979b 472 switch(isBorderFound)
hazheng 79:bdbac82c979b 473 {
hazheng 79:bdbac82c979b 474 case LEFT_FOUND:
hazheng 79:bdbac82c979b 475 case BOTH_FOUND:
hazheng 79:bdbac82c979b 476 is_border_find = BOTH_FOUND;
hazheng 79:bdbac82c979b 477 break;
hazheng 79:bdbac82c979b 478 default:
hazheng 79:bdbac82c979b 479 break;
hazheng 79:bdbac82c979b 480 }
hazheng 79:bdbac82c979b 481 break;
hazheng 79:bdbac82c979b 482 case BOTH_FOUND:
hazheng 79:bdbac82c979b 483 default:
hazheng 79:bdbac82c979b 484 break;
hazheng 79:bdbac82c979b 485 }
hazheng 44:15de535c4005 486 }
hazheng 56:7d3395ae022d 487
hazheng 57:0d8a155d511d 488
hazheng 63:d9a81b3d69f5 489 //counter.Update();
hazheng 57:0d8a155d511d 490 //Thread::signal_wait(IMG_PROC_SIGNAL, osWaitForever);
hazheng 44:15de535c4005 491 }
hazheng 44:15de535c4005 492 }
hazheng 46:a5eb9bd3bb55 493
hazheng 46:a5eb9bd3bb55 494
hazheng 43:0d1886f4848a 495 void ardu_cam_display_img_utft()
hazheng 43:0d1886f4848a 496 {
hazheng 43:0d1886f4848a 497 ardu_cam_start_capture();
hazheng 44:15de535c4005 498
hazheng 44:15de535c4005 499 while (!(ardu_cam_spi_read_8(ARDUCHIP_TRIG) & CAP_DONE_MASK));
hazheng 44:15de535c4005 500
hazheng 44:15de535c4005 501 temp_mid_pos = RESOLUTION_WIDTH / 2;
hazheng 46:a5eb9bd3bb55 502
hazheng 44:15de535c4005 503 for (uint8_t i = 0; i < CAM_ROI_UPPER_LIMIT; ++i)
hazheng 43:0d1886f4848a 504 {
hazheng 46:a5eb9bd3bb55 505 ardu_utft_set_camimg_row(CAM_ROI_UPPER_LIMIT - i);
hazheng 46:a5eb9bd3bb55 506 for (uint8_t j = 0; j < RESOLUTION_WIDTH; ++j)
hazheng 46:a5eb9bd3bb55 507 {
hazheng 46:a5eb9bd3bb55 508 uint8_t VH = ardu_cam_spi_read_8(SINGLE_FIFO_READ);
hazheng 46:a5eb9bd3bb55 509 uint8_t VL = ardu_cam_spi_read_8(SINGLE_FIFO_READ);
hazheng 46:a5eb9bd3bb55 510
hazheng 46:a5eb9bd3bb55 511 ardu_utft_write_DATA(VH, VL);
hazheng 46:a5eb9bd3bb55 512 }
hazheng 44:15de535c4005 513
hazheng 43:0d1886f4848a 514 }
hazheng 44:15de535c4005 515 }
hazheng 64:43ab429a37e0 516
hazheng 64:43ab429a37e0 517 uint8_t ardu_cam_get_is_encounter_terminate()
hazheng 64:43ab429a37e0 518 {
hazheng 64:43ab429a37e0 519 return is_encounter_terminate;
hazheng 64:43ab429a37e0 520 }
hazheng 79:bdbac82c979b 521
hazheng 79:bdbac82c979b 522 uint8_t ardu_cam_get_is_border_found()
hazheng 79:bdbac82c979b 523 {
hazheng 79:bdbac82c979b 524 return is_border_find;
hazheng 79:bdbac82c979b 525 }
hazheng 63:d9a81b3d69f5 526 /*
hazheng 57:0d8a155d511d 527 void tick_image_proc()
hazheng 57:0d8a155d511d 528 {
hazheng 57:0d8a155d511d 529 m_imgProcessThread->signal_set(IMG_PROC_SIGNAL);
hazheng 57:0d8a155d511d 530 }
hazheng 63:d9a81b3d69f5 531 */
hazheng 46:a5eb9bd3bb55 532 #ifdef __cplusplus
hazheng 46:a5eb9bd3bb55 533 }
hazheng 46:a5eb9bd3bb55 534 #endif
hazheng 46:a5eb9bd3bb55 535
hazheng 46:a5eb9bd3bb55 536
hazheng 46:a5eb9bd3bb55 537