pat9125.c 7.0 KB

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