diff options
| -rw-r--r-- | .gitignore | 9 | ||||
| -rw-r--r-- | Makefile | 46 | ||||
| -rw-r--r-- | README.txt | 17 | ||||
| -rw-r--r-- | fpm.c | 319 | ||||
| -rw-r--r-- | fpm.h | 46 | ||||
| -rw-r--r-- | main.c | 99 |
6 files changed, 536 insertions, 0 deletions
diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..596a14b --- /dev/null +++ b/.gitignore @@ -0,0 +1,9 @@ +# C +**/*.o +**/*.elf +**/*.hex +**/*.out + +# Vim +*.swp +*.core diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..883a3cf --- /dev/null +++ b/Makefile @@ -0,0 +1,46 @@ +CC = avr-gcc +MCU = atmega328p +PORT = /dev/cuaU0 +TARGET = app + +SRC = main.c fpm.c +OBJ = $(SRC:.c=.o) + +CFLAGS = -std=gnu99 +CFLAGS += -Os +CFLAGS += -Wall +CFLAGS += -mmcu=$(MCU) +CFLAGS += -DBAUD=57600 +CFLAGS += -DF_CPU=16000000UL +CFLAGS += -DFPM_PWD=$(FPM_PWD) +CFLAGS += -ffunction-sections -fdata-sections + +LDFLAGS = -mmcu=$(MCU) +LDFLAGS += -Wl,--gc-sections + +HEX_FLAGS = -O ihex +HEX_FLAGS += -j .text -j .data + +AVRDUDE_FLAGS = -p $(MCU) +AVRDUDE_FLAGS += -c arduino +AVRDUDE_FLAGS += -P $(PORT) +AVRDUDE_FLAGS += -D -U + +%.o: %.c + $(CC) $(CFLAGS) -c -o $@ $< + +elf: $(OBJ) + $(CC) $(LDFLAGS) $(OBJ) -o $(TARGET).elf + +hex: elf + avr-objcopy $(HEX_FLAGS) $(TARGET).elf $(TARGET).hex + +upload: hex + avrdude $(AVRDUDE_FLAGS) flash:w:$(TARGET).hex:i + +.PHONY: clean + +clean: + rm -f *.o *.elf *.hex + + diff --git a/README.txt b/README.txt new file mode 100644 index 0000000..be66f81 --- /dev/null +++ b/README.txt @@ -0,0 +1,17 @@ +CURRENT MEASUREMENTS + +R503 FPM: +ATmega328P: +MOSFET: + + +FS5106B high-torque servo: + +One of them draws 6.1mA when connected and stabilizes at 4.6mA. The other drew +variable amounts initially (4.7mA, 5mA+, but mostly 4.7). Both draw 4.6mA of +quiescent current with a 5.07V supply. + +REMARKS + + 1. Can't use the 3.3V FPM modules with Arduino Uno. The RX and TX lines have + resistors. @@ -0,0 +1,319 @@ +#include <avr/io.h> +#include <util/delay.h> +#include <util/setbaud.h> + +#include "fpm.h" + +#define MAXPDLEN 64 +#define RST_DELAY_MS 500 + +#define HEADER_HO 0xEF +#define HEADER_LO 0x01 +#define ADDR 0xFFFFFFFF + +#define OK 0x00 +#define PACKID 0x01 + +static inline uint8_t read(void) +{ + while (!(UCSR0A & (1 << RXC0))) + ; + return UDR0; +} + +static inline void write(uint8_t c) +{ + while (!(UCSR0A & (1 << UDRE0))) + ; + UDR0 = c; +} + +static inline void send(uint8_t *data, uint8_t n) +{ + int i; + uint16_t pktlen, sum; + + write(HEADER_HO); + write(HEADER_LO); + + write((uint8_t)(ADDR >> 24)); + write((uint8_t)(ADDR >> 16)); + write((uint8_t)(ADDR >> 8)); + write((uint8_t)(ADDR & 0xFF)); + + write(PACKID); + + pktlen = n + 2; + write((uint8_t)(pktlen >> 8)); + write((uint8_t)pktlen); + + sum = (pktlen >> 8) + (pktlen & 0xFF) + PACKID; + for (i = 0; i < n; i++) { + write(data[i]); + sum += data[i]; + } + + write((uint8_t)(sum >> 8)); + write((uint8_t)sum); +} + +static inline uint16_t recv(uint8_t buf[MAXPDLEN]) +{ + int i; + uint16_t len; + uint8_t byte; + + i = 0, len = 0; + + for (;;) { + byte = read(); + switch (i) { + case 0: + if (byte != HEADER_HO) + continue; + break; + case 1: + if (byte != HEADER_LO) + return 0; + case 2: + case 3: + case 4: + case 5: + // address + break; + case 6: + // packet id + break; + case 7: + len = (uint16_t)byte << 8; + break; + case 8: + len |= byte; + break; + default: + if ((i - 9) < MAXPDLEN) { + buf[i - 9] = byte; + if ((i - 8) == len) + return len; + } else + return 0; + break; + } + i++; + } + return 0; +} + +void fpm_led(LED_CTRL ctrl, LED_COLOR color, uint8_t count) +{ + uint8_t buf[MAXPDLEN]; + + buf[0] = 0x35; + buf[1] = ctrl; + buf[2] = 0x60; + buf[3] = color; + buf[4] = count; + + send(buf, 5); + recv(buf); +} + +static inline uint8_t check_pwd(void) +{ + uint8_t buf[MAXPDLEN]; + + buf[0] = 0x13; + buf[1] = (uint8_t)((uint32_t)FPM_PWD >> 24); + buf[2] = (uint8_t)((uint32_t)FPM_PWD >> 16); + buf[3] = (uint8_t)((uint32_t)FPM_PWD >> 8); + buf[4] = (uint8_t)((uint32_t)FPM_PWD & 0xFF); + + send(buf, 5); + recv(buf); + return buf[0] == OK; +} + +static inline uint8_t scan(void) +{ + uint16_t retries; + uint8_t buf[MAXPDLEN]; + + retries = 0; + fpm_led(BREATHE, PURPLE, 0); + + do { + buf[0] = 0x28; + send(buf, 1); + recv(buf); + if (buf[0] != OK) { + retries++; + _delay_ms(100); + } + } while(buf[0] != OK && retries < 100); + + fpm_led(GRAD_OFF, PURPLE, 0); + return buf[0] == OK; +} + +static inline uint8_t img2tz(uint8_t bufid) +{ + uint8_t buf[MAXPDLEN]; + + buf[0] = 0x02; + buf[1] = bufid; + send(buf, 2); + recv(buf); + return buf[0] == OK; +} + +uint8_t fpm_init(void) +{ + uint8_t rc; + + UBRR0H = UBRRH_VALUE; + UBRR0L = UBRRL_VALUE; +#if USE_2X + UCSR0A |= (1 << U2X0); +#else + UCSR0A &= ~(1 << U2X0); +#endif + UCSR0B = (1 << TXEN0) | (1 << RXEN0); + UCSR0C = (1 << UCSZ01) | (1 << UCSZ00); + + _delay_ms(RST_DELAY_MS); + + if ((rc = check_pwd())) + fpm_led(OFF, NIL, 0); + return rc; +} + +uint8_t fpm_get_cfg(struct fpm_cfg *cfg) +{ + uint16_t n; + uint8_t buf[MAXPDLEN]; + + buf[0] = 0x0F; + send(buf, 1); + n = recv(buf); + + if (buf[0] == OK && n >= 17) { + cfg->status = ((uint16_t)buf[1] << 8) | buf[2]; + cfg->sysid = ((uint16_t)buf[3] << 8) | buf[4]; + cfg->cap = ((uint16_t)buf[5] << 8) | buf[6]; + cfg->sec_level = ((uint16_t)buf[7] << 8) | buf[8]; + cfg->addr[0] = buf[9]; + cfg->addr[1] = buf[10]; + cfg->addr[2] = buf[11]; + cfg->addr[3] = buf[12]; + cfg->pkt_size = ((uint16_t)buf[13] << 8) | buf[14]; + cfg->pkt_size = 1 << (cfg->pkt_size + 5); + cfg->baud = (((uint16_t)buf[15] << 8) | buf[16]); + + return 1; + } + return 0; +} + +uint8_t fpm_set_pwd(uint32_t pwd) +{ + uint8_t buf[MAXPDLEN]; + + buf[0] = 0x12; + buf[1] = (uint8_t)(pwd >> 24); + buf[2] = (uint8_t)(pwd >> 16); + buf[3] = (uint8_t)(pwd >> 8); + buf[4] = (uint8_t)(pwd & 0xFF); + + send(buf, 5); + recv(buf); + return buf[0] == OK; +} + +uint16_t fpm_get_count(void) +{ + uint16_t n, count; + uint8_t buf[MAXPDLEN]; + + buf[0] = 0x1D; + send(buf, 1); + n = recv(buf); + + count = 0; + if (buf[0] == OK && n >= 2) { + count = buf[1]; + count <<= 8; + count |= buf[2]; + } + return count; +} + +uint8_t fpm_enroll(void) +{ + struct fpm_cfg cfg; + uint16_t n; + uint8_t buf[MAXPDLEN]; + + fpm_get_cfg(&cfg); + n = fpm_get_count() + 1; + if (n >= cfg.cap) + return 0; + + if (!scan()) + return 0; + + if (!img2tz(1)) + return 0; + + _delay_ms(2000); + + if (!scan()) + return 0; + + if (!img2tz(2)) + return 0; + + buf[0] = 0x05; + send(buf, 1); + recv(buf); + if (buf[0] != OK) + return 0; + + buf[0] = 0x06; + buf[1] = 1; + buf[2] = (uint8_t)(n >> 8); + buf[3] = (uint8_t)(n & 0xFF); + send(buf, 4); + recv(buf); + + return buf[0] == OK; +} + +uint16_t fpm_match(void) +{ + struct fpm_cfg cfg; + uint8_t buf[MAXPDLEN]; + + if (!fpm_get_cfg(&cfg)) + return 0; + + if (!scan()) + return 0; + + if (!img2tz(1)) + return 0; + + buf[0] = 0x04; + buf[1] = 1; + buf[2] = 0x00; + buf[3] = 0x00; + buf[4] = (uint8_t)(cfg.cap >> 8); + buf[5] = (uint8_t)(cfg.cap & 0xFF); + + send(buf, 6); + recv(buf); + + if (buf[0] != OK) + return 0; + + return ((uint16_t)buf[1] << 8) | buf[2]; +} @@ -0,0 +1,46 @@ +#ifndef FPM_R503_H +#define FPM_R503_H + +#include <stdint.h> + +struct fpm_cfg { + uint16_t status; + uint16_t sysid; + uint16_t cap; + uint16_t sec_level; + uint8_t addr[4]; + uint16_t pkt_size; + uint16_t baud; +}; + +typedef enum { + NIL = 0x00, + RED = 0x01, + BLUE = 0x02, + PURPLE = 0x03 +} LED_COLOR; + +typedef enum { + BREATHE = 0x01, + FLASH = 0x02, + ON = 0x03, + OFF = 0x04, + GRAD_ON = 0x05, + GRAD_OFF = 0x06, +} LED_CTRL; + +uint8_t fpm_init(void); + +uint8_t fpm_get_cfg(struct fpm_cfg *cfg); + +uint8_t fpm_set_pwd(uint32_t pwd); + +uint16_t fpm_get_count(void); + +uint8_t fpm_enroll(void); + +uint16_t fpm_match(void); + +void fpm_led(LED_CTRL ctrl, LED_COLOR color, uint8_t count); + +#endif /* FPM_R50_H */ @@ -0,0 +1,99 @@ +#include <stdint.h> +#include <stdlib.h> +#include <avr/wdt.h> +#include <avr/sleep.h> +#include <avr/interrupt.h> +#include <util/delay.h> + +#include "fpm.h" + +#define FRONT_UNLOCK_PIN PD2 +#define FRONT_LOCK_PIN PD3 +#define ENROLL_PIN PD4 +#define BACK_LOCK_PIN PD5 +#define BACK_UNLOCK_PIN PD6 + +#define INPUT_DDR DDRD +#define INPUT_PORT PORTD + +#define FPM_UNLOCK_INT INT0 +#define FPM_INT_VEC INT0_vect + +#define FRONT_LOCK_INT PCINT19 +#define ENROLL_INT PCINT20 +#define BACK_LOCK_INT PCINT21 +#define BACK_UNLOCK_INT PCINT22 +#define BTN_INT_VEC PCINT2_vect + +int main(void) +{ + /* input ports */ + INPUT_DDR &= ~((1 << BACK_LOCK_PIN) | (1 << BACK_UNLOCK_PIN) | + (1 << FRONT_LOCK_PIN) | (1 << FRONT_UNLOCK_PIN) | + (1 << ENROLL_PIN)); + + INPUT_PORT |= ((1 << BACK_LOCK_PIN) | (1 << BACK_UNLOCK_PIN) | + (1 << FRONT_LOCK_PIN) | (1 << FRONT_UNLOCK_PIN) | + (1 << ENROLL_PIN)); + + EICRA = 0b00000000; + EIMSK = (1 << FPM_UNLOCK_INT); + + PCICR |= (1 << PCIE2); + PCMSK2 |= ((1 << FRONT_LOCK_INT) | (1 << ENROLL_INT) | + (1 << BACK_LOCK_INT) | (1 << BACK_UNLOCK_INT)); + + fpm_init(); + sei(); + + for (;;) { + } + + return 0; +} + +static inline int is_pressed(uint8_t btn) +{ + if (!((PIND >> btn) & 0x01)) { + _delay_ms(50); + return !((PIND >> btn) & 0x01); + } + return 0; +} + +ISR(FPM_INT_VEC) +{ + cli(); + + if (fpm_match()) { + fpm_led(BREATHE, BLUE, 1); + } else { + fpm_led(BREATHE, RED, 1); + } + + sei(); +} + +ISR(BTN_INT_VEC) +{ + uint16_t id; + + cli(); + + if (is_pressed(FRONT_LOCK_PIN)) { + fpm_led(FLASH, RED, 1); + } else if (is_pressed(BACK_LOCK_PIN)) { + } else if (is_pressed(BACK_UNLOCK_PIN)) { + } else if (is_pressed(ENROLL_PIN)) { + id = fpm_match(); + if (id == 1 || id == 2) { + fpm_led(BREATHE, BLUE, 1); + _delay_ms(1000); + if (fpm_enroll()) + fpm_led(BREATHE, BLUE, 1); + } else + fpm_led(BREATHE, RED, 1); + } + + sei(); +} |