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@57:0d8a155d511d, 2017-04-08 (annotated)

Committer:
hazheng
Date:
Sat Apr 08 15:58:52 2017 +0000
Revision:
57:0d8a155d511d
Parent:
56:7d3395ae022d
Child:
63:d9a81b3d69f5
move the image processing to the main thread. Created the debug counter.

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