3DPrinting/Prusa-Firmware
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

PAT9125 optimalization (rewrited to C, 8bit sampling)

Browse source
This commit is contained in:
Robert Pelnar 2018-07-12 18:11:07 +02:00
parent 410e911364
commit b35a212d15
6 changed files with 116 additions and 107 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

8
Firmware/config.h
View file

@ -16,6 +16,14 @@
#define SWI2C_DEL 20 //2us clock delay
#define SWI2C_TMO 2048 //2048 cycles timeout
//PAT9125 configuration
#define PAT9125_SWI2C
#define PAT9125_I2C_ADDR 0x75 //ID=LO
//#define PAT9125_I2C_ADDR 0x79 //ID=HI
//#define PAT9125_I2C_ADDR 0x73 //ID=NC
#define PAT9125_XRES 0
#define PAT9125_YRES 200
//SM4 configuration
#define SM4_DEFDELAY 500 //default step delay [us]

158
Firmware/pat9125.cpp → Firmware/pat9125.c Executable file → Normal file
View file

@ -1,8 +1,30 @@
#include "uni_avr_rpi.h"
#ifdef PAT9125
//pat9125.c
#include "pat9125.h"
#include <avr/delay.h>
#include <avr/pgmspace.h>
#include "config.h"
#include <stdio.h>
//PAT9125 registers
#define PAT9125_PID1 0x00
#define PAT9125_PID2 0x01
#define PAT9125_MOTION 0x02
#define PAT9125_DELTA_XL 0x03
#define PAT9125_DELTA_YL 0x04
#define PAT9125_MODE 0x05
#define PAT9125_CONFIG 0x06
#define PAT9125_WP 0x09
#define PAT9125_SLEEP1 0x0a
#define PAT9125_SLEEP2 0x0b
#define PAT9125_RES_X 0x0d
#define PAT9125_RES_Y 0x0e
#define PAT9125_DELTA_XYH 0x12
#define PAT9125_SHUTTER 0x14
#define PAT9125_FRAME 0x17
#define PAT9125_ORIENTATION 0x19
#define PAT9125_BANK_SELECTION 0x7f
#ifdef PAT9125_SWSPI
#include "swspi.h"
@ -10,20 +32,18 @@
#ifdef PAT9125_SWI2C
#include "swi2c.h"
#endif //PAT9125_SWI2C
#ifdef PAT9125_HWI2C
#include <Wire.h>
#endif //PAT9125_HWI2C
unsigned char pat9125_PID1 = 0;
unsigned char pat9125_PID2 = 0;
int pat9125_x = 0;
int pat9125_y = 0;
unsigned char pat9125_b = 0;
unsigned char pat9125_s = 0;
uint8_t pat9125_PID1 = 0;
uint8_t pat9125_PID2 = 0;
int16_t pat9125_x = 0;
int16_t pat9125_y = 0;
uint8_t pat9125_b = 0;
uint8_t pat9125_s = 0;
// Init sequence, address & value.
const PROGMEM unsigned char pat9125_init_seq1[] = {
const PROGMEM uint8_t pat9125_init_seq1[] = {
// Disable write protect.
PAT9125_WP, 0x5a,
// Set the X resolution to zero to let the sensor know that it could safely ignore movement in the X axis.
@ -43,7 +63,7 @@ const PROGMEM unsigned char pat9125_init_seq1[] = {
};
// Init sequence, address & value.
const PROGMEM unsigned char pat9125_init_seq2[] = {
const PROGMEM uint8_t pat9125_init_seq2[] = {
// Magic sequence to enforce full frame rate of the sensor.
0x06, 0x028,
0x33, 0x0d0,
@ -74,7 +94,13 @@ const PROGMEM unsigned char pat9125_init_seq2[] = {
0x0ff
};
int pat9125_init()
uint8_t pat9125_rd_reg(uint8_t addr);
void pat9125_wr_reg(uint8_t addr, uint8_t data);
uint8_t pat9125_wr_reg_verify(uint8_t addr, uint8_t data);
uint8_t pat9125_init(void)
{
#ifdef PAT9125_SWSPI
swspi_init();
@ -82,14 +108,9 @@ int pat9125_init()
#ifdef PAT9125_SWI2C
swi2c_init();
#endif //PAT9125_SWI2C
#ifdef PAT9125_HWI2C
Wire.begin();
#endif //PAT9125_HWI2C
// Verify that the sensor responds with its correct product ID.
pat9125_PID1 = pat9125_rd_reg(PAT9125_PID1);
pat9125_PID2 = pat9125_rd_reg(PAT9125_PID2);
// pat9125_PID1 = 0x31;
// pat9125_PID2 = 0x91;
if ((pat9125_PID1 != 0x31) || (pat9125_PID2 != 0x91))
{
pat9125_PID1 = pat9125_rd_reg(PAT9125_PID1);
@ -97,6 +118,8 @@ int pat9125_init()
if ((pat9125_PID1 != 0x31) || (pat9125_PID2 != 0x91))
return 0;
}
#ifdef PAT9125_NEW_INIT
// Switch to bank0, not allowed to perform OTS_RegWriteRead.
pat9125_wr_reg(PAT9125_BANK_SELECTION, 0);
// Software reset (i.e. set bit7 to 1). It will reset to 0 automatically.
@ -104,11 +127,11 @@ int pat9125_init()
pat9125_wr_reg(PAT9125_CONFIG, 0x97);
// Wait until the sensor reboots.
// Delay 1ms.
delayMicroseconds(1000);
_delay_us(1000);
{
const unsigned char *ptr = pat9125_init_seq1;
const uint8_t *ptr = pat9125_init_seq1;
for (;;) {
const unsigned char addr = pgm_read_byte_near(ptr ++);
const uint8_t addr = pgm_read_byte_near(ptr ++);
if (addr == 0x0ff)
break;
if (! pat9125_wr_reg_verify(addr, pgm_read_byte_near(ptr ++)))
@ -117,13 +140,13 @@ int pat9125_init()
}
}
// Delay 10ms.
delayMicroseconds(10000);
_delay_ms(10);
// Switch to bank1, not allowed to perform OTS_RegWrite.
pat9125_wr_reg(PAT9125_BANK_SELECTION, 0x01);
{
const unsigned char *ptr = pat9125_init_seq2;
const uint8_t *ptr = pat9125_init_seq2;
for (;;) {
const unsigned char addr = pgm_read_byte_near(ptr ++);
const uint8_t addr = pgm_read_byte_near(ptr ++);
if (addr == 0x0ff)
break;
if (! pat9125_wr_reg_verify(addr, pgm_read_byte_near(ptr ++)))
@ -138,25 +161,28 @@ int pat9125_init()
pat9125_PID1 = pat9125_rd_reg(PAT9125_PID1);
pat9125_PID2 = pat9125_rd_reg(PAT9125_PID2);
#endif //PAT9125_NEW_INIT
pat9125_wr_reg(PAT9125_RES_X, 0);
pat9125_wr_reg(PAT9125_RES_Y, 200);
return 1;
}
int pat9125_update()
uint8_t pat9125_update(void)
{
if ((pat9125_PID1 == 0x31) && (pat9125_PID2 == 0x91))
{
unsigned char ucMotion = pat9125_rd_reg(PAT9125_MOTION);
uint8_t ucMotion = pat9125_rd_reg(PAT9125_MOTION);
pat9125_b = pat9125_rd_reg(PAT9125_FRAME);
pat9125_s = pat9125_rd_reg(PAT9125_SHUTTER);
if (pat9125_PID1 == 0xff) return 0;
if (ucMotion & 0x80)
{
unsigned char ucXL = pat9125_rd_reg(PAT9125_DELTA_XL);
unsigned char ucYL = pat9125_rd_reg(PAT9125_DELTA_YL);
unsigned char ucXYH = pat9125_rd_reg(PAT9125_DELTA_XYH);
uint8_t ucXL = pat9125_rd_reg(PAT9125_DELTA_XL);
uint8_t ucYL = pat9125_rd_reg(PAT9125_DELTA_YL);
uint8_t ucXYH = pat9125_rd_reg(PAT9125_DELTA_XYH);
if (pat9125_PID1 == 0xff) return 0;
int iDX = ucXL | ((ucXYH << 4) & 0xf00);
int iDY = ucYL | ((ucXYH << 8) & 0xf00);
int16_t iDX = ucXL | ((ucXYH << 4) & 0xf00);
int16_t iDY = ucYL | ((ucXYH << 8) & 0xf00);
if (iDX & 0x800) iDX -= 4096;
if (iDY & 0x800) iDY -= 4096;
pat9125_x += iDX;
@ -167,18 +193,18 @@ int pat9125_update()
return 0;
}
int pat9125_update_y()
uint8_t pat9125_update_y(void)
{
if ((pat9125_PID1 == 0x31) && (pat9125_PID2 == 0x91))
{
unsigned char ucMotion = pat9125_rd_reg(PAT9125_MOTION);
uint8_t ucMotion = pat9125_rd_reg(PAT9125_MOTION);
if (pat9125_PID1 == 0xff) return 0;
if (ucMotion & 0x80)
{
unsigned char ucYL = pat9125_rd_reg(PAT9125_DELTA_YL);
unsigned char ucXYH = pat9125_rd_reg(PAT9125_DELTA_XYH);
uint8_t ucYL = pat9125_rd_reg(PAT9125_DELTA_YL);
uint8_t ucXYH = pat9125_rd_reg(PAT9125_DELTA_XYH);
if (pat9125_PID1 == 0xff) return 0;
int iDY = ucYL | ((ucXYH << 8) & 0xf00);
int16_t iDY = ucYL | ((ucXYH << 8) & 0xf00);
if (iDY & 0x800) iDY -= 4096;
pat9125_y -= iDY; //negative number, because direction switching does not work
}
@ -187,10 +213,26 @@ int pat9125_update_y()
return 0;
}
unsigned char pat9125_rd_reg(unsigned char addr)
uint8_t pat9125_update_y2(void)
{
// printf_P(PSTR("pat9125_rd_reg 0x%hhx "), addr);
unsigned char data = 0;
if ((pat9125_PID1 == 0x31) && (pat9125_PID2 == 0x91))
{
uint8_t ucMotion = pat9125_rd_reg(PAT9125_MOTION);
if (pat9125_PID1 == 0xff) return 0; //NOACK error
if (ucMotion & 0x80)
{
int8_t dy = pat9125_rd_reg(PAT9125_DELTA_YL);
if (pat9125_PID1 == 0xff) return 0; //NOACK error
pat9125_y -= dy; //negative number, because direction switching does not work
}
return 1;
}
return 0;
}
uint8_t pat9125_rd_reg(uint8_t addr)
{
uint8_t data = 0;
#ifdef PAT9125_SWSPI
swspi_start();
swspi_tx(addr & 0x7f);
@ -198,30 +240,18 @@ unsigned char pat9125_rd_reg(unsigned char addr)
swspi_stop();
#endif //PAT9125_SWSPI
#ifdef PAT9125_SWI2C
int iret = swi2c_readByte_A8(PAT9125_I2C_ADDR, addr, &data);
if (!iret) //NO ACK error
if (!swi2c_readByte_A8(PAT9125_I2C_ADDR, addr, &data)) //NO ACK error
{
pat9125_PID1 = 0xff;
pat9125_PID2 = 0xff;
// printf_P(PSTR("ERR\n"));
return 0;
}
// printf_P(PSTR("0x%hhx OK\n"), data);
#endif //PAT9125_SWI2C
#ifdef PAT9125_HWI2C
Wire.beginTransmission(PAT9125_I2C_ADDR);
Wire.write(addr);
Wire.endTransmission();
if (Wire.requestFrom(PAT9125_I2C_ADDR, 1) == 1)
// if (Wire.available())
data = Wire.read();
#endif //PAT9125_HWI2C
return data;
}
void pat9125_wr_reg(unsigned char addr, unsigned char data)
void pat9125_wr_reg(uint8_t addr, uint8_t data)
{
// printf_P(PSTR("pat9125_wr_reg 0x%hhx 0x%hhx "), addr, data);
#ifdef PAT9125_SWSPI
swspi_start();
swspi_tx(addr | 0x80);
@ -229,29 +259,17 @@ void pat9125_wr_reg(unsigned char addr, unsigned char data)
swspi_stop();
#endif //PAT9125_SWSPI
#ifdef PAT9125_SWI2C
int iret = swi2c_writeByte_A8(PAT9125_I2C_ADDR, addr, &data);
if (!iret) //NO ACK error
if (!swi2c_writeByte_A8(PAT9125_I2C_ADDR, addr, &data)) //NO ACK error
{
pat9125_PID1 = 0xff;
pat9125_PID2 = 0xff;
// printf_P(PSTR("ERR\n"));
return;
}
// printf_P(PSTR("OK\n"));
#endif //PAT9125_SWI2C
#ifdef PAT9125_HWI2C
Wire.beginTransmission(PAT9125_I2C_ADDR);
Wire.write(addr);
Wire.write(data);
Wire.endTransmission();
#endif //PAT9125_HWI2C
}
bool pat9125_wr_reg_verify(unsigned char addr, unsigned char data)
uint8_t pat9125_wr_reg_verify(uint8_t addr, uint8_t data)
{
pat9125_wr_reg(addr, data);
return pat9125_rd_reg(addr) == data;
}
#endif //PAT9125

54
Firmware/pat9125.h
View file

@ -1,45 +1,31 @@
//pat9125.h
#ifndef PAT9125_H
#define PAT9125_H
//PAT9125 I2C
#define PAT9125_I2C_ADDR 0x75 //ID=LO
//#define PAT9125_I2C_ADDR 0x79 //ID=HI
//#define PAT9125_I2C_ADDR 0x73 //ID=NC
#include <inttypes.h>
//PAT9125 registers
#define PAT9125_PID1 0x00
#define PAT9125_PID2 0x01
#define PAT9125_MOTION 0x02
#define PAT9125_DELTA_XL 0x03
#define PAT9125_DELTA_YL 0x04
#define PAT9125_MODE 0x05
#define PAT9125_CONFIG 0x06
#define PAT9125_WP 0x09
#define PAT9125_SLEEP1 0x0a
#define PAT9125_SLEEP2 0x0b
#define PAT9125_RES_X 0x0d
#define PAT9125_RES_Y 0x0e
#define PAT9125_DELTA_XYH 0x12
#define PAT9125_SHUTTER 0x14
#define PAT9125_FRAME 0x17
#define PAT9125_ORIENTATION 0x19
#define PAT9125_BANK_SELECTION 0x7f
extern unsigned char pat9125_PID1;
extern unsigned char pat9125_PID2;
#if defined(__cplusplus)
extern "C" {
#endif //defined(__cplusplus)
extern int pat9125_x;
extern int pat9125_y;
extern unsigned char pat9125_b;
extern unsigned char pat9125_s;
extern int pat9125_init();
extern int pat9125_update();
extern int pat9125_update_y();
extern uint8_t pat9125_PID1;
extern uint8_t pat9125_PID2;
extern unsigned char pat9125_rd_reg(unsigned char addr);
extern void pat9125_wr_reg(unsigned char addr, unsigned char data);
extern bool pat9125_wr_reg_verify(unsigned char addr, unsigned char data);
extern int16_t pat9125_x;
extern int16_t pat9125_y;
extern uint8_t pat9125_b;
extern uint8_t pat9125_s;
extern uint8_t pat9125_init(void);
extern uint8_t pat9125_update(void);
extern uint8_t pat9125_update_y(void);
extern uint8_t pat9125_update_y2(void);
#if defined(__cplusplus)
}
#endif //defined(__cplusplus)
#endif //PAT9125_H

1
Firmware/pins_Einsy_1_0.h
View file

@ -21,7 +21,6 @@
#define SWI2C_SDA 20 //SDA on P3
#define SWI2C_SCL 21 //SCL on P3
#define PAT9125_SWI2C
#define X_TMC2130_CS 41

1
Firmware/pins_Rambo_1_0.h
View file

@ -14,7 +14,6 @@
#define SWI2C_SDA 20 //SDA on P3
#define SWI2C_SCL 84 //PH2 on P3, sensor cable must be rewired
#define PAT9125_SWI2C
#define X_STEP_PIN 37

1
Firmware/pins_Rambo_1_3.h
View file

@ -14,7 +14,6 @@
#define SWI2C_SDA 20 //SDA on P3
#define SWI2C_SCL 21 //SCL on P3
#define PAT9125_SWI2C
#define X_STEP_PIN 37