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@64:43ab429a37e0, 2017-04-10 (annotated)

Committer:
hazheng
Date:
Mon Apr 10 16:44:31 2017 +0000
Revision:
64:43ab429a37e0
Parent:
63:d9a81b3d69f5
Child:
65:295c222fdf88
Auto terminate when hit the end of the loop. Commented out all the IMU code, because of the memory space problem.

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