pat9125.cpp 7.9 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311
  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. #include "Configuration_prusa.h"
  8. #if defined(FILAMENT_SENSOR) && (FILAMENT_SENSOR_TYPE == FSENSOR_PAT9125)
  9. //PAT9125 registers
  10. #define PAT9125_PID1 0x00
  11. #define PAT9125_PID2 0x01
  12. #define PAT9125_MOTION 0x02
  13. #define PAT9125_DELTA_XL 0x03
  14. #define PAT9125_DELTA_YL 0x04
  15. #define PAT9125_MODE 0x05
  16. #define PAT9125_CONFIG 0x06
  17. #define PAT9125_WP 0x09
  18. #define PAT9125_SLEEP1 0x0a
  19. #define PAT9125_SLEEP2 0x0b
  20. #define PAT9125_RES_X 0x0d
  21. #define PAT9125_RES_Y 0x0e
  22. #define PAT9125_DELTA_XYH 0x12
  23. #define PAT9125_SHUTTER 0x14
  24. #define PAT9125_FRAME 0x17
  25. #define PAT9125_ORIENTATION 0x19
  26. #define PAT9125_BANK_SELECTION 0x7f
  27. #if defined(PAT9125_SWSPI)
  28. #include "swspi.h"
  29. #elif defined(PAT9125_SWI2C)
  30. #include "swi2c.h"
  31. #elif defined(PAT9125_I2C)
  32. #include "twi.h"
  33. #else
  34. #error unknown PAT9125 communication method
  35. #endif
  36. uint8_t pat9125_PID1 = 0;
  37. uint8_t pat9125_PID2 = 0;
  38. int16_t pat9125_x = 0;
  39. int16_t pat9125_y = 0;
  40. uint8_t pat9125_b = 0;
  41. uint8_t pat9125_s = 0;
  42. // Init sequence, address & value.
  43. const PROGMEM uint8_t pat9125_init_bank0[] = {
  44. // Disable write protect.
  45. PAT9125_WP, 0x5a,
  46. // Set the X resolution to zero to let the sensor know that it could safely ignore movement in the X axis.
  47. PAT9125_RES_X, PAT9125_XRES,
  48. // Set the Y resolution to a maximum (or nearly a maximum).
  49. PAT9125_RES_Y, PAT9125_YRES,
  50. // Set data format and sensor orientation.
  51. PAT9125_ORIENTATION, ((PAT9125_12B_RES?0x04:0) | (PAT9125_INVERT_X?0x08:0) | (PAT9125_INVERT_Y?0x10:0) | (PAT9125_SWAP_XY?0x20:0)),
  52. // Now continues the magic sequence from the PAT912EL Application Note: Firmware Guides for Tracking Optimization.
  53. 0x5e, 0x08,
  54. 0x20, 0x64,
  55. 0x2b, 0x6d,
  56. 0x32, 0x2f,
  57. 0xff //end of sequence
  58. };
  59. // Init sequence, address & value.
  60. const PROGMEM uint8_t pat9125_init_bank1[] = {
  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. 0xff //end of sequence
  88. };
  89. static uint8_t pat9125_rd_reg(uint8_t addr);
  90. static void pat9125_wr_reg(uint8_t addr, uint8_t data);
  91. static uint8_t pat9125_wr_reg_verify(uint8_t addr, uint8_t data);
  92. static uint8_t pat9125_wr_seq(const uint8_t* seq);
  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_check(PAT9125_I2C_ADDR) == 0;
  103. #elif defined(PAT9125_I2C)
  104. twi_init();
  105. return twi_check(PAT9125_I2C_ADDR) == 0;
  106. #endif
  107. }
  108. uint8_t pat9125_init(void)
  109. {
  110. if (!pat9125_probe())
  111. return 0;
  112. // Switch to bank0, not allowed to perform pat9125_wr_reg_verify on this register.
  113. pat9125_wr_reg(PAT9125_BANK_SELECTION, 0);
  114. // Verify that the sensor responds with its correct product ID.
  115. pat9125_PID1 = pat9125_rd_reg(PAT9125_PID1);
  116. pat9125_PID2 = pat9125_rd_reg(PAT9125_PID2);
  117. if ((pat9125_PID1 != 0x31) || (pat9125_PID2 != 0x91))
  118. {
  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. return 0;
  123. }
  124. #if PAT9125_NEW_INIT
  125. // Software reset (i.e. set bit7 to 1). It will reset to 0 automatically.
  126. // pat9125_wr_reg_verify is not allowed because the register contents will change as soon as they are written. No point in verifying those.
  127. pat9125_wr_reg(PAT9125_CONFIG, 0x97);
  128. // Wait until the sensor reboots.
  129. _delay_ms(1);
  130. //Write init sequence in bank0. MUST ALREADY BE IN bank0.
  131. if (!pat9125_wr_seq(pat9125_init_bank0))
  132. return 0;
  133. _delay_ms(10); // not sure why this is here. But I'll allow it.
  134. // Switch to bank1, not allowed to perform pat9125_wr_reg_verify on this register.
  135. pat9125_wr_reg(PAT9125_BANK_SELECTION, 0x01);
  136. //Write init sequence in bank1. MUST ALREADY BE IN bank1.
  137. if (!pat9125_wr_seq(pat9125_init_bank1))
  138. return 0;
  139. // Switch to bank0, not allowed to perform pat9125_wr_reg_verify on this register.
  140. pat9125_wr_reg(PAT9125_BANK_SELECTION, 0x00);
  141. // Enable write protect.
  142. pat9125_wr_reg(PAT9125_WP, 0x00); //prevents writing to registers over 0x09
  143. pat9125_PID1 = pat9125_rd_reg(PAT9125_PID1);
  144. pat9125_PID2 = pat9125_rd_reg(PAT9125_PID2);
  145. #else //PAT9125_NEW_INIT
  146. // Disable write protect.
  147. pat9125_wr_reg(PAT9125_WP, 0x5a); //allows writing to all registers
  148. pat9125_wr_reg(PAT9125_RES_X, PAT9125_XRES);
  149. pat9125_wr_reg(PAT9125_RES_Y, PAT9125_YRES);
  150. printf_P(PSTR("PAT9125_RES_X=%u\n"), pat9125_rd_reg(PAT9125_RES_X));
  151. printf_P(PSTR("PAT9125_RES_Y=%u\n"), pat9125_rd_reg(PAT9125_RES_Y));
  152. pat9125_wr_reg(PAT9125_ORIENTATION, ((PAT9125_12B_RES?0x04:0) | (PAT9125_INVERT_X?0x08:0) | (PAT9125_INVERT_Y?0x10:0) | (PAT9125_SWAP_XY?0x20:0)));
  153. // Enable write protect.
  154. pat9125_wr_reg(PAT9125_WP, 0x00); //prevents writing to registers over 0x09
  155. #endif //PAT9125_NEW_INIT
  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. uint16_t ucXL = pat9125_rd_reg(PAT9125_DELTA_XL);
  169. uint16_t ucYL = pat9125_rd_reg(PAT9125_DELTA_YL);
  170. uint16_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;
  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. uint16_t ucYL = pat9125_rd_reg(PAT9125_DELTA_YL);
  192. uint16_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;
  197. }
  198. return 1;
  199. }
  200. return 0;
  201. }
  202. uint8_t pat9125_update_bs(void)
  203. {
  204. if ((pat9125_PID1 == 0x31) && (pat9125_PID2 == 0x91))
  205. {
  206. pat9125_b = pat9125_rd_reg(PAT9125_FRAME);
  207. pat9125_s = pat9125_rd_reg(PAT9125_SHUTTER);
  208. if (pat9125_PID1 == 0xff) return 0;
  209. return 1;
  210. }
  211. return 0;
  212. }
  213. static uint8_t pat9125_rd_reg(uint8_t addr)
  214. {
  215. uint8_t data = 0;
  216. #if defined(PAT9125_SWSPI)
  217. swspi_start();
  218. swspi_tx(addr & 0x7f);
  219. data = swspi_rx();
  220. swspi_stop();
  221. #elif defined(PAT9125_SWI2C)
  222. if (!swi2c_readByte_A8(PAT9125_I2C_ADDR, addr, &data)) //NO ACK error
  223. goto error;
  224. #elif defined(PAT9125_I2C)
  225. if (twi_r8(PAT9125_I2C_ADDR,addr,&data))
  226. goto error;
  227. #endif
  228. return data;
  229. error:
  230. pat9125_PID1 = 0xff;
  231. pat9125_PID2 = 0xff;
  232. return 0;
  233. }
  234. static void pat9125_wr_reg(uint8_t addr, uint8_t data)
  235. {
  236. #if defined(PAT9125_SWSPI)
  237. swspi_start();
  238. swspi_tx(addr | 0x80);
  239. swspi_tx(data);
  240. swspi_stop();
  241. #elif defined(PAT9125_SWI2C)
  242. if (!swi2c_writeByte_A8(PAT9125_I2C_ADDR, addr, &data)) //NO ACK error
  243. goto error;
  244. #elif defined(PAT9125_I2C)
  245. if (twi_w8(PAT9125_I2C_ADDR,addr,data))
  246. goto error;
  247. #endif
  248. return;
  249. error:
  250. pat9125_PID1 = 0xff;
  251. pat9125_PID2 = 0xff;
  252. return;
  253. }
  254. static 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. }
  259. static uint8_t pat9125_wr_seq(const uint8_t* seq)
  260. {
  261. for (;;) {
  262. const uint8_t addr = pgm_read_byte(seq++);
  263. if (addr == 0xff)
  264. break;
  265. if (!pat9125_wr_reg_verify(addr, pgm_read_byte(seq++)))
  266. // Verification of the register write failed.
  267. return 0;
  268. }
  269. return 1;
  270. }
  271. #endif