pat9125.cpp 7.5 KB

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