pat9125.cpp 6.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257
  1. #include "uni_avr_rpi.h"
  2. #ifdef PAT9125
  3. #include "pat9125.h"
  4. #ifdef PAT9125_SWSPI
  5. #include "swspi.h"
  6. #endif //PAT9125_SWSPI
  7. #ifdef PAT9125_SWI2C
  8. #include "swi2c.h"
  9. #endif //PAT9125_SWI2C
  10. #ifdef PAT9125_HWI2C
  11. #include <Wire.h>
  12. #endif //PAT9125_HWI2C
  13. unsigned char pat9125_PID1 = 0;
  14. unsigned char pat9125_PID2 = 0;
  15. int pat9125_x = 0;
  16. int pat9125_y = 0;
  17. unsigned char pat9125_b = 0;
  18. unsigned char pat9125_s = 0;
  19. // Init sequence, address & value.
  20. const PROGMEM unsigned char pat9125_init_seq1[] = {
  21. // Disable write protect.
  22. PAT9125_WP, 0x5a,
  23. // Set the X resolution to zero to let the sensor know that it could safely ignore movement in the X axis.
  24. PAT9125_RES_X, PAT9125_XRES,
  25. // Set the Y resolution to a maximum (or nearly a maximum).
  26. PAT9125_RES_Y, PAT9125_YRES,
  27. // Set 12-bit X/Y data format.
  28. PAT9125_ORIENTATION, 0x04,
  29. // PAT9125_ORIENTATION, 0x04 | (xinv?0x08:0) | (yinv?0x10:0), //!? direction switching does not work
  30. // Now continues the magic sequence from the PAT912EL Application Note: Firmware Guides for Tracking Optimization.
  31. 0x5e, 0x08,
  32. 0x20, 0x64,
  33. 0x2b, 0x6d,
  34. 0x32, 0x2f,
  35. // stopper
  36. 0x0ff
  37. };
  38. // Init sequence, address & value.
  39. const PROGMEM unsigned char pat9125_init_seq2[] = {
  40. // Magic sequence to enforce full frame rate of the sensor.
  41. 0x06, 0x028,
  42. 0x33, 0x0d0,
  43. 0x36, 0x0c2,
  44. 0x3e, 0x001,
  45. 0x3f, 0x015,
  46. 0x41, 0x032,
  47. 0x42, 0x03b,
  48. 0x43, 0x0f2,
  49. 0x44, 0x03b,
  50. 0x45, 0x0f2,
  51. 0x46, 0x022,
  52. 0x47, 0x03b,
  53. 0x48, 0x0f2,
  54. 0x49, 0x03b,
  55. 0x4a, 0x0f0,
  56. 0x58, 0x098,
  57. 0x59, 0x00c,
  58. 0x5a, 0x008,
  59. 0x5b, 0x00c,
  60. 0x5c, 0x008,
  61. 0x61, 0x010,
  62. 0x67, 0x09b,
  63. 0x6e, 0x022,
  64. 0x71, 0x007,
  65. 0x72, 0x008,
  66. // stopper
  67. 0x0ff
  68. };
  69. int pat9125_init()
  70. {
  71. #ifdef PAT9125_SWSPI
  72. swspi_init();
  73. #endif //PAT9125_SWSPI
  74. #ifdef PAT9125_SWI2C
  75. swi2c_init(PAT9125_SWI2C_SDA, PAT9125_SWI2C_SCL, PAT9125_SWI2C_CFG);
  76. #endif //PAT9125_SWI2C
  77. #ifdef PAT9125_HWI2C
  78. Wire.begin();
  79. #endif //PAT9125_HWI2C
  80. // Verify that the sensor responds with its correct product ID.
  81. pat9125_PID1 = pat9125_rd_reg(PAT9125_PID1);
  82. pat9125_PID2 = pat9125_rd_reg(PAT9125_PID2);
  83. // pat9125_PID1 = 0x31;
  84. // pat9125_PID2 = 0x91;
  85. if ((pat9125_PID1 != 0x31) || (pat9125_PID2 != 0x91))
  86. {
  87. pat9125_PID1 = pat9125_rd_reg(PAT9125_PID1);
  88. pat9125_PID2 = pat9125_rd_reg(PAT9125_PID2);
  89. if ((pat9125_PID1 != 0x31) || (pat9125_PID2 != 0x91))
  90. return 0;
  91. }
  92. // Switch to bank0, not allowed to perform OTS_RegWriteRead.
  93. pat9125_wr_reg(PAT9125_BANK_SELECTION, 0);
  94. // Software reset (i.e. set bit7 to 1). It will reset to 0 automatically.
  95. // After the reset, OTS_RegWriteRead is not allowed.
  96. pat9125_wr_reg(PAT9125_CONFIG, 0x97);
  97. // Wait until the sensor reboots.
  98. // Delay 1ms.
  99. delayMicroseconds(1000);
  100. {
  101. const unsigned char *ptr = pat9125_init_seq1;
  102. for (;;) {
  103. const unsigned char addr = pgm_read_byte_near(ptr ++);
  104. if (addr == 0x0ff)
  105. break;
  106. if (! pat9125_wr_reg_verify(addr, pgm_read_byte_near(ptr ++)))
  107. // Verification of the register write failed.
  108. return 0;
  109. }
  110. }
  111. // Delay 10ms.
  112. delayMicroseconds(10000);
  113. // Switch to bank1, not allowed to perform OTS_RegWrite.
  114. pat9125_wr_reg(PAT9125_BANK_SELECTION, 0x01);
  115. {
  116. const unsigned char *ptr = pat9125_init_seq2;
  117. for (;;) {
  118. const unsigned char addr = pgm_read_byte_near(ptr ++);
  119. if (addr == 0x0ff)
  120. break;
  121. if (! pat9125_wr_reg_verify(addr, pgm_read_byte_near(ptr ++)))
  122. // Verification of the register write failed.
  123. return 0;
  124. }
  125. }
  126. // Switch to bank0, not allowed to perform OTS_RegWriteRead.
  127. pat9125_wr_reg(PAT9125_BANK_SELECTION, 0x00);
  128. // Enable write protect.
  129. pat9125_wr_reg(PAT9125_WP, 0x00);
  130. pat9125_PID1 = pat9125_rd_reg(PAT9125_PID1);
  131. pat9125_PID2 = pat9125_rd_reg(PAT9125_PID2);
  132. return 1;
  133. }
  134. int pat9125_update()
  135. {
  136. if ((pat9125_PID1 == 0x31) && (pat9125_PID2 == 0x91))
  137. {
  138. unsigned char ucMotion = pat9125_rd_reg(PAT9125_MOTION);
  139. pat9125_b = pat9125_rd_reg(PAT9125_FRAME);
  140. pat9125_s = pat9125_rd_reg(PAT9125_SHUTTER);
  141. if (pat9125_PID1 == 0xff) return 0;
  142. if (ucMotion & 0x80)
  143. {
  144. unsigned char ucXL = pat9125_rd_reg(PAT9125_DELTA_XL);
  145. unsigned char ucYL = pat9125_rd_reg(PAT9125_DELTA_YL);
  146. unsigned char ucXYH = pat9125_rd_reg(PAT9125_DELTA_XYH);
  147. if (pat9125_PID1 == 0xff) return 0;
  148. int iDX = ucXL | ((ucXYH << 4) & 0xf00);
  149. int iDY = ucYL | ((ucXYH << 8) & 0xf00);
  150. if (iDX & 0x800) iDX -= 4096;
  151. if (iDY & 0x800) iDY -= 4096;
  152. pat9125_x += iDX;
  153. pat9125_y -= iDY; //negative number, because direction switching does not work
  154. }
  155. return 1;
  156. }
  157. return 0;
  158. }
  159. int pat9125_update_y()
  160. {
  161. if ((pat9125_PID1 == 0x31) && (pat9125_PID2 == 0x91))
  162. {
  163. unsigned char ucMotion = pat9125_rd_reg(PAT9125_MOTION);
  164. if (pat9125_PID1 == 0xff) return 0;
  165. if (ucMotion & 0x80)
  166. {
  167. unsigned char ucYL = pat9125_rd_reg(PAT9125_DELTA_YL);
  168. unsigned char ucXYH = pat9125_rd_reg(PAT9125_DELTA_XYH);
  169. if (pat9125_PID1 == 0xff) return 0;
  170. int iDY = ucYL | ((ucXYH << 8) & 0xf00);
  171. if (iDY & 0x800) iDY -= 4096;
  172. pat9125_y -= iDY; //negative number, because direction switching does not work
  173. }
  174. return 1;
  175. }
  176. return 0;
  177. }
  178. unsigned char pat9125_rd_reg(unsigned char addr)
  179. {
  180. // printf_P(PSTR("pat9125_rd_reg 0x%hhx "), addr);
  181. unsigned char data = 0;
  182. #ifdef PAT9125_SWSPI
  183. swspi_start();
  184. swspi_tx(addr & 0x7f);
  185. data = swspi_rx();
  186. swspi_stop();
  187. #endif //PAT9125_SWSPI
  188. #ifdef PAT9125_SWI2C
  189. int iret = swi2c_readByte_A8(PAT9125_I2C_ADDR, addr, &data);
  190. if (!iret) //NO ACK error
  191. {
  192. pat9125_PID1 = 0xff;
  193. pat9125_PID2 = 0xff;
  194. // printf_P(PSTR("ERR\n"));
  195. return 0;
  196. }
  197. // printf_P(PSTR("0x%hhx OK\n"), data);
  198. #endif //PAT9125_SWI2C
  199. #ifdef PAT9125_HWI2C
  200. Wire.beginTransmission(PAT9125_I2C_ADDR);
  201. Wire.write(addr);
  202. Wire.endTransmission();
  203. if (Wire.requestFrom(PAT9125_I2C_ADDR, 1) == 1)
  204. // if (Wire.available())
  205. data = Wire.read();
  206. #endif //PAT9125_HWI2C
  207. return data;
  208. }
  209. void pat9125_wr_reg(unsigned char addr, unsigned char data)
  210. {
  211. // printf_P(PSTR("pat9125_wr_reg 0x%hhx 0x%hhx "), addr, data);
  212. #ifdef PAT9125_SWSPI
  213. swspi_start();
  214. swspi_tx(addr | 0x80);
  215. swspi_tx(data);
  216. swspi_stop();
  217. #endif //PAT9125_SWSPI
  218. #ifdef PAT9125_SWI2C
  219. int iret = swi2c_writeByte_A8(PAT9125_I2C_ADDR, addr, &data);
  220. if (!iret) //NO ACK error
  221. {
  222. pat9125_PID1 = 0xff;
  223. pat9125_PID2 = 0xff;
  224. // printf_P(PSTR("ERR\n"));
  225. return;
  226. }
  227. // printf_P(PSTR("OK\n"));
  228. #endif //PAT9125_SWI2C
  229. #ifdef PAT9125_HWI2C
  230. Wire.beginTransmission(PAT9125_I2C_ADDR);
  231. Wire.write(addr);
  232. Wire.write(data);
  233. Wire.endTransmission();
  234. #endif //PAT9125_HWI2C
  235. }
  236. bool pat9125_wr_reg_verify(unsigned char addr, unsigned char data)
  237. {
  238. pat9125_wr_reg(addr, data);
  239. return pat9125_rd_reg(addr) == data;
  240. }
  241. #endif //PAT9125