From 62c52289eaeec90f7ed592a2428907dab90c5f3a Mon Sep 17 00:00:00 2001 From: Dennis Gunia Date: Wed, 22 Oct 2025 23:09:39 +0200 Subject: [PATCH] added firmware for new pcb revision --- .gitignore | 5 +- software/firmware_module/module_rev3/Makefile | 66 ++++ .../firmware_module/module_rev3/src/global.h | 64 ++++ .../firmware_module/module_rev3/src/main.c | 198 ++++++++++++ .../firmware_module/module_rev3/src/mctrl.c | 304 ++++++++++++++++++ .../firmware_module/module_rev3/src/mctrl.h | 48 +++ .../firmware_module/module_rev3/src/rcount.c | 67 ++++ .../firmware_module/module_rev3/src/rcount.h | 21 ++ .../firmware_module/module_rev3/src/rs458.c | 244 ++++++++++++++ .../firmware_module/module_rev3/src/rs485.h | 41 +++ 10 files changed, 1057 insertions(+), 1 deletion(-) create mode 100644 software/firmware_module/module_rev3/Makefile create mode 100644 software/firmware_module/module_rev3/src/global.h create mode 100644 software/firmware_module/module_rev3/src/main.c create mode 100644 software/firmware_module/module_rev3/src/mctrl.c create mode 100644 software/firmware_module/module_rev3/src/mctrl.h create mode 100644 software/firmware_module/module_rev3/src/rcount.c create mode 100644 software/firmware_module/module_rev3/src/rcount.h create mode 100644 software/firmware_module/module_rev3/src/rs458.c create mode 100644 software/firmware_module/module_rev3/src/rs485.h diff --git a/.gitignore b/.gitignore index 40e4561..207c14b 100644 --- a/.gitignore +++ b/.gitignore @@ -1,8 +1,11 @@ software/firmware_module/module_rev0/build/* software/firmware_module/module_rev0/build +software/firmware_module/module_rev3/build/* +software/firmware_module/module_rev3/build software/pc_client/build/* software/pc_client/build hardware/module_controller/ModuleController-backups/* hardware/module_controller/ModuleController-backups hardware/module_controller/~*.lck -software/pc_client/*.json \ No newline at end of file +software/pc_client/*.json +software/firmware_module/module_rev3/eeprom.hex \ No newline at end of file diff --git a/software/firmware_module/module_rev3/Makefile b/software/firmware_module/module_rev3/Makefile new file mode 100644 index 0000000..495df94 --- /dev/null +++ b/software/firmware_module/module_rev3/Makefile @@ -0,0 +1,66 @@ +# Source and include +BUILD_DIR=build +SRCS=$(wildcard src/*.c) +OBJS=$(SRCS:%.c=build/%.o) +INC=-I ./inc + +# MCU configuration +# Set MCU type, clock frequency and programmer +MCU=atmega8 +CLOCK_FREQ=16000000 +PROG_STR=usbasp + +# Compiler flags +CFLAGS=-std=c11 -Wall -Wextra -Werror -mmcu=$(MCU) -DF_CPU=$(CLOCK_FREQ) +OPT_FLAGS=-O3 -g -DDEBUG + +# Compiler and utility tools +OBJCOPY=avr-objcopy +CC=avr-gcc + +# Project configuration +PROJ_NAME=sflap_controller_fw +PROJ_BLD=$(BUILD_DIR)/$(PROJ_NAME) + +TEMP_EEPROM_DUMP=eeprom.hex +# Rules + +all: $(PROJ_BLD).elf + +$(PROJ_BLD).elf: $(OBJS) + $(CC) -o $@ $^ $(INC) $(CFLAGS) $(OPT_FLAGS) $(MCU_FLAGS) + $(OBJCOPY) -j .text -j .data -O ihex $@ $(PROJ_BLD).hex + $(OBJCOPY) -j .text -j .data -O binary $@ $(PROJ_BLD).bin + + +build/%.o: %.c + mkdir -p build/src + $(CC) -c -o $@ $(INC) $(CFLAGS) $(OPT_FLAGS) $(MCU_FLAGS) $< + +release: OPT_FLAGS=-O2 -DNDEBUG +release: $(PROJ_BLD).elf + +fuse: + avrdude -c $(PROG_STR) -p $(MCU) -U lfuse:w:0xDF:m -U hfuse:w:0xCA:m -B 125kHz + +flash-clean: + avrdude -c $(PROG_STR) -p $(MCU) -U flash:w:$(PROJ_BLD).hex:i + +flash-update: + avrdude -c $(PROG_STR) -p $(MCU) -U eeprom:r:$(TEMP_EEPROM_DUMP):i + avrdude -c $(PROG_STR) -p $(MCU) -U flash:w:$(PROJ_BLD).hex:i + avrdude -c $(PROG_STR) -p $(MCU) -U eeprom:w:$(TEMP_EEPROM_DUMP):i + +clear-address: + avrdude -c $(PROG_STR) -p $(MCU) -U eeprom:r:$(TEMP_EEPROM_DUMP):i + sed -i 's/:20000000..../:200000000000/' $(TEMP_EEPROM_DUMP) + avrdude -c $(PROG_STR) -p $(MCU) -U eeprom:w:$(TEMP_EEPROM_DUMP):i + +flash-debug: + avrdude -c $(PROG_STR) -p $(MCU) -U flash:w:$(PROJ_BLD).elf:e + +clean: + rm -rf build + rm $(TEMP_EEPROM_DUMP) + +.PHONY = clean, release, flash, flash-debug \ No newline at end of file diff --git a/software/firmware_module/module_rev3/src/global.h b/software/firmware_module/module_rev3/src/global.h new file mode 100644 index 0000000..f1e4160 --- /dev/null +++ b/software/firmware_module/module_rev3/src/global.h @@ -0,0 +1,64 @@ +/* Copyright (C) 2025 Dennis Gunia - All Rights Reserved + * You may use, distribute and modify this code under the + * terms of the AGPL-3.0 license. + * + * https://www.dennisgunia.de + * https://github.com/dennis9819/splitflap_v1 + */ + +#include +#include +#include +#include +#include +#include +#include + + +// I/O Pin definition +#define BUX_RX PD0 // RX Pin (to buffer) +#define BUX_TX PD1 // TX Pin (to buffer) +#define BUX_DIR PD2 // Buffer direction pin +#define SENSOR_HOME PD3 // Home sensor pin + +#define MOTOR_A PC0 // Motor phase A driver +#define MOTOR_B PC1 // Motor phase B driver +#define MOTOR_C PC2 // Motor phase C driver +#define MOTOR_D PC3 // Motor phase D driver + +// EEPROM Addresses +#define CONF_CONST_OKAY (uint8_t)0xAA +#define CONF_ADDR_OKAY 0x0004 +#define CONF_ADDR_ADDR 0x0000 +#define CONF_ADDR_OFFSET 0x0002 + +// Protocol definitions +#define PROTO_MAXPKGLEN 64 // maximum size of package in bytes + +// Command Bytes +#define CMDB_SETVAL (uint8_t)0x10 // Set display value +#define CMDB_SETVALR (uint8_t)0x11 // Set display value and do a full rotation +#define CMDB_EEPROMR (uint8_t)0xF0 // Read EEPROM +#define CMDB_EEPROMW (uint8_t)0xF1 // Write EEPROM +#define CMDB_GSTS (uint8_t)0xF8 // Get status +#define CMDB_PING (uint8_t)0xFE // Ping +#define CMDB_RESET (uint8_t)0x30 // Reset device +#define CMDB_PWRON (uint8_t)0x21 // Power motor on +#define CMDB_RPWROFF (uint8_t)0x20 // Power motor off +#define CMDB_GVER (uint8_t)0xF9 // Get Firmware Version + +// Command Responses +#define CMDR_ERR_INVALID 0xEE // Invalid command +#define CMDR_ACK 0xAA // Acknowledge +#define CMDR_PING 0xFF // Ping response + +// Utility definitions +#define SHIFT_0B 0 +#define SHIFT_1B 8 +#define SHIFT_2B 16 +#define SHIFT_3B 24 + +// Firmware Version +#define FVER_SEMVER_MAJOR 2 +#define FVER_SEMVER_MINOR 0 +#define FVER_SEMVER_PATCH 0 \ No newline at end of file diff --git a/software/firmware_module/module_rev3/src/main.c b/software/firmware_module/module_rev3/src/main.c new file mode 100644 index 0000000..0bf90aa --- /dev/null +++ b/software/firmware_module/module_rev3/src/main.c @@ -0,0 +1,198 @@ +/* Copyright (C) 2025 Dennis Gunia - All Rights Reserved + * You may use, distribute and modify this code under the + * terms of the AGPL-3.0 license. + * + * https://www.dennisgunia.de + * https://github.com/dennis9819/splitflap_v1 + */ + +#include "global.h" +#include "mctrl.h" +#include "rcount.h" +#include "rs485.h" + +uint16_t address = 0x0000; +uint16_t calib_offset = 0x0000; +char *payload; + + +void eeprom_write_c(uint16_t address, uint8_t data) +{ + // disable interrupt + cli(); + while (EECR & (1 << EEWE)) + ; // wait until previous write is done + EEAR = address; + EEDR = data; + EECR |= (1 << EEMWE); // enable Master Write Enable + EECR |= (1 << EEWE); // write one + sei(); +} + +uint8_t eeprom_read_c(uint16_t address) +{ + while (EECR & (1 << EEWE)) + ; // wait until previous write is done + EEAR = address; + EECR |= (1 << EERE); // read one + return EEDR; +} + +void initialSetup() +{ + wdt_disable(); + DDRB &= ~(1 << PB0); // Set PB0/JP1 as input + PORTB |= (1 << PB0); // set pullup enable for PB0/JP1 + if (eeprom_read_c(CONF_ADDR_OKAY) == CONF_CONST_OKAY && (PINB && (1 << PB0)) > 0) + { + uint8_t addrL = eeprom_read_c(CONF_ADDR_ADDR); + uint8_t addrH = eeprom_read_c(CONF_ADDR_ADDR + 1); + address = addrL | (addrH << 8); + uint8_t offsetL = eeprom_read_c(CONF_ADDR_OFFSET); + uint8_t offsetH = eeprom_read_c(CONF_ADDR_OFFSET + 1); + calib_offset = (offsetL | (offsetH << 8)); + } + else + { + eeprom_write_c(CONF_ADDR_ADDR, (uint8_t)0x00); + eeprom_write_c(CONF_ADDR_ADDR + 1, (uint8_t)0x00); + eeprom_write_c(CONF_ADDR_OFFSET, (uint8_t)0x00); + eeprom_write_c(CONF_ADDR_OFFSET + 1, (uint8_t)0x00); + eeprom_write_c(CONF_ADDR_OKAY, CONF_CONST_OKAY); + } +} + +void readCommand() +{ + int payload_len = parse_buffer(address, payload); + if (payload_len > 0) // if positive, package is valid + { + payload += 2; // skip address bytes + // read command byte + uint8_t opcode = *payload; + // parse commands + if (opcode == CMDB_SETVAL) + { + // 0x1O = Set Digit + uint8_t targetDigit = *(payload + 1); + mctrl_set(targetDigit, 0); + } + else if (opcode == CMDB_SETVALR) + { + // 0x11 = Set Digit (full rotation) + uint8_t targetDigit = *(payload + 1); + mctrl_set(targetDigit, 1); + } + else if (opcode == CMDB_EEPROMR) + { + // 0xFO = READ EEPROM + uint8_t bytes = 5; + char *msg = malloc(bytes + 1); + *msg = CMDR_ACK; + for (uint16_t i = 1; i < (uint16_t)bytes + 1; i++) + { + *(msg + i) = (char)eeprom_read_c(i - 1); + } + _delay_ms(2); + sfbus_send_frame_v2(0xFFFF, msg, bytes + 1); + free(msg); + } + else if (opcode == CMDB_EEPROMW) + { + // 0xF1 = WRITE EEPROM + eeprom_write_c(CONF_ADDR_OKAY, (char)0xFF); + for (uint16_t i = 0; i < 4; i++) + { + eeprom_write_c(i, *(payload + 1 + i)); + } + eeprom_write_c(CONF_ADDR_OKAY, CONF_CONST_OKAY); + // respond with readout + uint8_t bytes = 5; + char *msg = malloc(bytes + 1); + *msg = CMDR_ACK; + for (uint16_t i = 1; i < (uint16_t)bytes + 1; i++) + { + *(msg + i) = (char)eeprom_read_c(i - 1); + } + _delay_ms(2); + sfbus_send_frame_v2(0xFFFF, msg, bytes + 1); + free(msg); + // now use new addr + uint8_t addrL = eeprom_read_c(CONF_ADDR_ADDR); + uint8_t addrH = eeprom_read_c(CONF_ADDR_ADDR + 1); + address = addrL | (addrH << SHIFT_1B); + } + else if (opcode == CMDB_GSTS) + { + char *msg = malloc(7); + *msg = (char)getSts(); + uint16_t voltage = getVoltage(); + *(msg + 2) = (char)((voltage >> SHIFT_0B) & 0xFF); + *(msg + 1) = (char)((voltage >> SHIFT_1B) & 0xFF); + uint32_t counter = rc_getCounter(); + *(msg + 6) = (char)((counter >> SHIFT_0B) & 0xFF); + *(msg + 5) = (char)((counter >> SHIFT_1B) & 0xFF); + *(msg + 4) = (char)((counter >> SHIFT_2B) & 0xFF); + *(msg + 3) = (char)((counter >> SHIFT_3B) & 0xFF); + _delay_ms(2); + sfbus_send_frame_v2(0xFFFF, msg, 7); + free(msg); + } + else if (opcode == CMDB_GVER) + { + char *msg = malloc(3); + *(msg + 0) = FVER_SEMVER_MAJOR; + *(msg + 1) = FVER_SEMVER_MINOR; + *(msg + 2) = FVER_SEMVER_PATCH; + _delay_ms(2); + sfbus_send_frame_v2(0xFFFF, msg, 3); + free(msg); + } + else if (opcode == CMDB_PING) + { + char msg = (char)CMDR_PING; + _delay_ms(2); + sfbus_send_frame_v2(0xFFFF, &msg, 1); + } + else if (opcode == CMDB_RPWROFF) + { + mctrl_power(0); + } + else if (opcode == CMDB_PWRON) + { + mctrl_power(1); + } + else if (opcode == CMDB_RESET) + { + do + { + wdt_enable(WDTO_15MS); + for (;;) + { + } + } while (0); + } + else + { + // invalid opcode + char msg = CMDR_ERR_INVALID; + _delay_ms(2); + sfbus_send_frame_v2(0xFFFF, &msg, 1); + } + //memset(payload, 0x00, PROTO_MAXPKGLEN); // clear buffer + } +} + +int main() +{ + payload = malloc(PROTO_MAXPKGLEN); + initialSetup(); + rs485_init(); + setup_async_rx(); + mctrl_init(calib_offset); + + while (1 == 1) + { + readCommand(); + } +} diff --git a/software/firmware_module/module_rev3/src/mctrl.c b/software/firmware_module/module_rev3/src/mctrl.c new file mode 100644 index 0000000..94ebdaf --- /dev/null +++ b/software/firmware_module/module_rev3/src/mctrl.c @@ -0,0 +1,304 @@ +/* Copyright (C) 2025 Dennis Gunia - All Rights Reserved + * You may use, distribute and modify this code under the + * terms of the AGPL-3.0 license. + * + * https://www.dennisgunia.de + * https://github.com/dennis9819/splitflap_v1 + */ + + +#include "mctrl.h" + +// Motor driver steps definition. Reverse for direction change. +uint8_t motor_steps[4] = { + 0b00001000, + 0b00000100, + 0b00000010, + 0b00000001, +}; + +uint8_t step_index = 0; // current index in motor_steps +uint8_t target_flap = 0; // target flap +uint16_t absolute_pos = 0; // absolute position in steps +uint16_t steps_since_home = 0; // steps since last home signal + +// homing util variables +uint8_t homing = 0; // current homing step +uint8_t lastSens = 0; // home sonsor signal from last tick + +// counter for auto powersaving +uint8_t ticksSinceMove = 0; + +// value to goto after the current is reached. 255 = NONE. +uint8_t afterRotation = STEPS_AFTERROT; + +int16_t *delta_err; + +// error and status flags +uint8_t sts_flag_errorTooBig = 0; // last home signal too early or too late +uint8_t sts_flag_noHome = 0; // no home signal detected. Wheel stuck +uint8_t sts_flag_fuse = 0; // blown fuse detcted +uint8_t sts_flag_pwrdwn = 0; // device is powered down by controller +uint8_t sts_flag_failsafe = 0; // device is powered down for safety reasons +uint8_t sts_flag_busy = 0; // device is busy +// voltage monitoring variables +uint16_t currentVoltage = 0; // current ADC reading +uint8_t currentFaultReadings = 0; // ticks with faulty readings (too many will + // trip pwrdwn and sts_flag_fuse) +uint16_t timer_ticks = 0; + +int STEPS_OFFSET = 0; +// initialize motor controller +void mctrl_init(int cal_offset) +{ + if (cal_offset < 800){ + STEPS_OFFSET = STEPS_OFFSET_DEF; + }else{ + STEPS_OFFSET = cal_offset; + } + DDRC = 0x0F; // set all pins as outputs + PORTC = 0x00; // set all to LOW + + DDRD &= ~(1 << PD4); // PD4 is input + PORTD |= (1 << PD4); // PD4 pullup + + // setup adc + ADMUX = 0xC7; // Internal Vref at 2.56V, ADC7 + ADCSRA = (1 << ADEN) | (1 << ADSC) | (1 << ADPS1); // Enable ADC, Start first + // reading No freerunning, 8MHz + while ((ADCSRA & (1 << ADSC)) > 0) + { + }; + // wait until first reading is complete, + // to avoid error flag on first tick! + + // setup timer for ISR + TCCR1A = 0; + TCCR1B = (1 << WGM12) | (1 << CS11) | (1 << CS10); // CTC und Prescaler 64 + OCR1A = MISR_OCR1A; + TIMSK = 1 << OCIE1A; // Timerinterrupts aktivieren + homing = 1; + delta_err = malloc(ERROR_DATASETS * sizeof(uint16_t)); + _delay_ms(MDELAY_STARTUP); + sei(); +} + +// call when critical fail. Powers down motor and sets flags +void failSafe() +{ + sts_flag_failsafe = 1; + PORTC = 0x00; +} + +// read voltage non blocking (called every tick) +void readVoltage() +{ + currentVoltage = ADC; // read last measurement + ADMUX = 0xC7; // select ADC7 + ADCSRA |= (1 << ADSC); // trigger next reading + if (currentVoltage < MVOLTAGE_LSTOP) + { // if voltage is too low, fuse is probably broken + currentFaultReadings++; + if (currentFaultReadings > MVOLTAGE_FAULTRD) + { // too many fault readings trigger failSafe + sts_flag_fuse = 1; + failSafe(); + } + } +} + +// MAIN service routine. Called by timer 1 +ISR(TIMER1_COMPA_vect) +{ + timer_ticks ++; + readVoltage(); // read and check voltage + if (sts_flag_pwrdwn == 1 || sts_flag_failsafe == 1) + { + return; + } // if sts_flag_pwrdwn, STOP! + if (steps_since_home > STEPS_PER_REV * MHOME_TOLERANCE) + { // check if home is missing for too long + // home missing error. Wheel probably stuck or power fail + sts_flag_noHome = 1; + failSafe(); + } + else if (homing == 1) + { // Homing procedure 1. step: move out of home + if ((PIND & (1 << PD4)) > 0) + { + homing = 2; + } + else + { + mctrl_step(); + } + } + else if (homing == 2) + { // Homing procedure 2. step: find magnet + mctrl_step(); + if ((PIND & (1 << PD4)) == 0) + { + homing = 3; + steps_since_home = 0; + absolute_pos = STEPS_OFFSET; + incrementCounter(); + } + } + else if (homing == 3) + { // Homing procedure 3. step: apply offset + if (absolute_pos <= 0) + { + homing = 0; + absolute_pos = STEPS_OFFSET; // set correct position again + } + mctrl_step(); + absolute_pos--; + } + else + { // when no failsafe is triggered and homing is done + // calculate target position + uint16_t target_pos = (target_flap * STEPS_PER_FLAP) + STEPS_OFFSET; + if (target_pos >= STEPS_PER_REV) + { + target_pos -= STEPS_PER_REV; + } + if (absolute_pos != target_pos) + { + // if target position is not reached, move motor + ticksSinceMove = 0; + mctrl_step(); + absolute_pos++; + if (absolute_pos >= STEPS_PER_REV) + { + absolute_pos -= STEPS_PER_REV; + } + // detect home position + if ((PIND & (1 << PD3)) == 0) + { + if (lastSens == 0) + { + // new home transition + int16_t errorDelta = + (int16_t)(absolute_pos > (STEPS_PER_REV / 2) ? absolute_pos - STEPS_PER_REV : absolute_pos); + sts_flag_errorTooBig = (errorDelta > MHOME_ERRDELTA) || (errorDelta < -MHOME_ERRDELTA) ? 1 : 0; + // storeErr(errorDelta); + absolute_pos = 0; + steps_since_home = 0; + // increment rotations counter + incrementCounter(); + } + lastSens = 1; + } + else + { + lastSens = 0; + } + } + else + { // if target position is reached + if (afterRotation < (AMOUNTFLAPS + 5)) + { // if after rotation is set, apply it as new target + target_flap = afterRotation; + afterRotation = STEPS_AFTERROT; + } + else if (ticksSinceMove < 2) + { // if motor has not been moved + sts_flag_busy = 0; + } + else if (ticksSinceMove < MPWRSVG_TICKSTOP) + { // if motor has not been moved + ticksSinceMove++; + } + else + { // power off after 50 ticks + // PORTC = 0x00; // turn off stepper + } + } + } + rc_tick(); // process counter tick, non-blocking +} + +// TODO +void storeErr(int16_t error) +{ + int16_t *delta_err_tmp = malloc(ERROR_DATASETS * sizeof(uint16_t)); + memcpy(delta_err, delta_err_tmp + sizeof(uint16_t), ((ERROR_DATASETS - 2) * sizeof(uint16_t))); + memcpy(&error, delta_err_tmp, sizeof(uint16_t)); + free(delta_err); + delta_err = delta_err_tmp; +} +// TODO +void getErr(int16_t *error) +{ + memcpy(delta_err, error, (ERROR_DATASETS * sizeof(uint16_t))); +} + +// return status flag +uint8_t getSts() +{ + uint8_t status = sts_flag_errorTooBig; // bit 0: delta too big + status |= sts_flag_noHome << 1; // bit 1: no home found + status |= sts_flag_fuse << 2; // bit 2: fuse blown + status |= sts_flag_pwrdwn << 4; // bit 4: device powered down + status |= sts_flag_failsafe << 5; // bit 5: failsafe active + status |= sts_flag_busy << 6; // bit 6: device busy + if ((PIND & (1 << PD4)) == 0) + { + status |= (1 << 3); + } + return status; +} + +// return voltage +uint16_t getVoltage() +{ + return currentVoltage; +} + +// set target flap +void mctrl_set(uint8_t flap, uint8_t fullRotation) +{ + sts_flag_busy = 1; + if (fullRotation == 0) + { + target_flap = flap; + } + else + { + target_flap = (target_flap + (STEPS_PER_FLAP - 1)) % STEPS_PER_FLAP; + afterRotation = flap; + } +} + +// trigger home procedure +void mctrl_home() +{ + homing = 1; +} + +// change motor power state +void mctrl_power(uint8_t state) +{ + if (state == 0) + { + sts_flag_pwrdwn = 1; + PORTC = 0x00; + } + else + { + sts_flag_pwrdwn = 0; + PORTC = motor_steps[step_index]; + } +} + +// do stepper step (I/O) +void mctrl_step() +{ + step_index++; + steps_since_home++; + if (step_index > 3) + { + step_index = 0; + } + PORTC = motor_steps[step_index]; +} diff --git a/software/firmware_module/module_rev3/src/mctrl.h b/software/firmware_module/module_rev3/src/mctrl.h new file mode 100644 index 0000000..1277e41 --- /dev/null +++ b/software/firmware_module/module_rev3/src/mctrl.h @@ -0,0 +1,48 @@ +/* Copyright (C) 2025 Dennis Gunia - All Rights Reserved + * You may use, distribute and modify this code under the + * terms of the AGPL-3.0 license. + * + * https://www.dennisgunia.de + * https://github.com/dennis9819/splitflap_v1 + */ + + +#include "global.h" +#include "rcount.h" + +#pragma once + +#define STEPS_PER_REV 2025 // steps per revolution +#define STEPS_PER_FLAP 45 // steps per flap +#define STEPS_ADJ 0 // added per flap to compensate for motor power down +#define STEPS_OFFSET_DEF 1400 // ansolute offset between home and first flap +#define AMOUNTFLAPS 45 // amount of flaps installed in system +#define STEPS_AFTERROT 255 // value to goto after current target flap is reached +#define ERROR_DATASETS 8 // length of error array + +#define MDELAY_STARTUP 1000 // delay to wait after motor startup +#define MHOME_TOLERANCE 1.5 // tolerance for intial homing procedure +#define MHOME_ERRDELTA 30 // maximum deviation between expected home and actual home +#define MVOLTAGE_FAULTRD 20 // max. amount of fault readings before flag is set +#define MVOLTAGE_LSTOP 128 // lower voltage threshold for fuse detection +#define MPWRSVG_TICKSTOP 50 // inactive ticks before motor shutdown + +#define MISR_OCR1A 580 // tick timer (defines rotation speed) +// 450, 480 also possible ? + +#ifdef __cplusplus +extern "C" { +#endif // __cplusplus +void mctrl_init(int cal_offset); +void mctrl_step(); +void mctrl_set(uint8_t flap, uint8_t fullRotation); + +void getErr(int16_t* error); +uint8_t getSts(); +uint16_t getVoltage(); +void mctrl_power(uint8_t state); + +uint16_t timer_ticks; +#ifdef __cplusplus +} +#endif // __cplusplus diff --git a/software/firmware_module/module_rev3/src/rcount.c b/software/firmware_module/module_rev3/src/rcount.c new file mode 100644 index 0000000..1975456 --- /dev/null +++ b/software/firmware_module/module_rev3/src/rcount.c @@ -0,0 +1,67 @@ +/* Copyright (C) 2025 Dennis Gunia - All Rights Reserved + * You may use, distribute and modify this code under the + * terms of the AGPL-3.0 license. + * + * https://www.dennisgunia.de + * https://github.com/dennis9819/splitflap_v1 + */ + + +#include "rcount.h" + +uint8_t rc_eeprom_write_c(uint16_t address, uint8_t data) +{ + // disable interrupt + if (EECR & (1 << EEWE)) + { + return 1; + } + EEAR = address; + EEDR = data; + EECR |= (1 << EEMWE); // enable Master Write Enable + EECR |= (1 << EEWE); // write one + return 0; +} + +uint8_t rc_eeprom_read_c(uint16_t address) +{ + while (EECR & (1 << EEWE)) + ; // wait until previous write is done + EEAR = address; + EECR |= (1 << EERE); // read one + return EEDR; +} + +uint32_t counter = (uint32_t)0xFFFFFFFF; +uint8_t counter_phase = 5; +void rc_tick() +{ + if (counter == (uint32_t)0xFFFFFFFF) + { + counter = rc_getCounter(); + } + if (counter_phase < 5) + { + cli(); + if (rc_eeprom_write_c(0x100 + counter_phase, ((counter >> (counter_phase * 8)) & 0xFF)) == 0) + { + counter_phase++; + } + sei(); + } +} + +void incrementCounter() +{ + counter++; + counter_phase = 0; +} + +uint32_t rc_getCounter() +{ + uint32_t counter = rc_eeprom_read_c(RC_BASEADDR); + counter |= ((uint32_t)rc_eeprom_read_c(RC_BASEADDR + 1) << SHIFT_1B); + counter |= ((uint32_t)rc_eeprom_read_c(RC_BASEADDR + 2) << SHIFT_2B); + counter |= ((uint32_t)rc_eeprom_read_c(RC_BASEADDR + 3) << SHIFT_3B); + return counter; +} diff --git a/software/firmware_module/module_rev3/src/rcount.h b/software/firmware_module/module_rev3/src/rcount.h new file mode 100644 index 0000000..6b848a0 --- /dev/null +++ b/software/firmware_module/module_rev3/src/rcount.h @@ -0,0 +1,21 @@ +/* Copyright (C) 2025 Dennis Gunia - All Rights Reserved + * You may use, distribute and modify this code under the + * terms of the AGPL-3.0 license. + * + * https://www.dennisgunia.de + * https://github.com/dennis9819/splitflap_v1 + */ + +#include "global.h" + +#define RC_BASEADDR 0x100 + +#ifdef __cplusplus +extern "C" { +#endif // __cplusplus +void incrementCounter(); +uint32_t rc_getCounter(); +void rc_tick(); +#ifdef __cplusplus +} +#endif // __cplusplus diff --git a/software/firmware_module/module_rev3/src/rs458.c b/software/firmware_module/module_rev3/src/rs458.c new file mode 100644 index 0000000..5b8bc18 --- /dev/null +++ b/software/firmware_module/module_rev3/src/rs458.c @@ -0,0 +1,244 @@ +/* Copyright (C) 2025 Dennis Gunia - All Rights Reserved + * You may use, distribute and modify this code under the + * terms of the AGPL-3.0 license. + * + * https://www.dennisgunia.de + * https://github.com/dennis9819/splitflap_v1 + */ + +#include "mctrl.h" +#include "rs485.h" + +void rs485_init() +{ + // init I/O + DDRD &= ~(1 << PD0); // BUS_DIR & TX is OUTPUT + DDRD |= (1 << PD3) | (1 << PD2) | (1 << PD1); // BUS_DIR & TX is OUTPUT + PORTD &= 0x07; // clear PD0-PD4 + // init UART + UBRRH = (BAUDRATE >> 8); + UBRRL = BAUDRATE; // set baud rate + UCSRB |= (1 << TXEN) | (1 << RXEN); // enable receiver and transmitter + UCSRC |= (1 << URSEL) | (1 << UCSZ0) | (1 << UCSZ1); // 8bit data format +} + +// send byte over rs485 +void rs485_send_c(char data) +{ + PORTD |= (1 << PD2) | (1 << PD3); // set transciever to transmitt + while (!(UCSRA & (1 << UDRE))) + ; // wait until buffer is empty + UCSRA = (1 << TXC); // clear transmit Complete bit + UDR = data; + while (!(UCSRA & (1 << TXC))) + { + }; // wait until transmitt complete + PORTD &= ~((1 << PD2) | (1 << PD3)); // set transciever back to receive +} + +// receive without timeout +char rs485_recv_c() +{ + while (!(UCSRA & (1 << RXC))) + ; // wait while data is being received + return UDR; +} + +// receive with timeout +int rs485_recv_c_rxout(uint8_t timeout, char *data) +{ + timer_ticks = 0; + while (!(UCSRA & (1 << RXC))) + { + if (timer_ticks > timeout) + { + return 0; + } + } + *data = UDR; + return 1; +} + +// SFBUS Functions +// receive paket with crc checking and timeout +int sfbus_recv_frame_v2(uint16_t address, char *payload) +{ + const uint8_t RX_TIMEOUT = 4; + while (rs485_recv_c() != SFBUS_SOF_BYTE) + { + } // Wait for start byte + uint8_t frm_version, frm_length; + if (rs485_recv_c_rxout(RX_TIMEOUT, (char *)&frm_version) == 0) // read header: version + return -1; + if (rs485_recv_c_rxout(RX_TIMEOUT, (char *)&frm_length) == 0) // read header: payload length + return -1; + + // ALWAYS!!! receive full packet to avoid sync error + for (int i = 0; i < frm_length; i++) + { + if (rs485_recv_c_rxout(RX_TIMEOUT, payload + i) == 0) + { + return -1; + } + } + // check protocol version + if (frm_version != 0x01) + { + return -1; // abort on incompatible version + } + // if version matches, read address and checksum + uint16_t recv_addr = (*(payload + 0) & 0xFF) | ((*(payload + 1) & 0xFF) << SHIFT_1B); + uint16_t checksum = (*(payload + (frm_length - 2)) & 0xFF) | ((*(payload + (frm_length - 1)) & 0xFF) << SHIFT_1B); + + // calculate checksum of received payload + uint16_t checksum_calc = calc_CRC16(payload + 2, frm_length - 4); + + // return length on valid address and crc, return 0 to ignore + return (checksum == checksum_calc && address == recv_addr) ? frm_length : 0; +} + +// send paket with crc +void sfbus_send_frame_v2(uint16_t address, char *payload, uint8_t length) +{ + uint8_t framelen = length; + // claculate crc value + uint16_t crc = calc_CRC16(payload, framelen); + + rs485_send_c(SFBUS_SOF_BYTE); // send startbyte + rs485_send_c(1); // send protocol version + rs485_send_c((char)(framelen + 4)); // send lentgh of remaining frame + + rs485_send_c((char)(address & 0xFF)); // target address + rs485_send_c((char)((address >> SHIFT_1B) & 0xFF)); + + while (framelen > 0) + { // send payload + rs485_send_c(*payload); + payload++; + framelen--; + } + + rs485_send_c((char)(crc & 0xFF)); // send crc + rs485_send_c((char)((crc >> SHIFT_1B) & 0xFF)); +} + +// calculate crc checksum +uint16_t calc_CRC16(char *buffer, uint8_t len) +{ + uint16_t crc16 = 0xFFFF; + for (uint8_t pos = 0; pos < len; pos++) + { + char byte_value = *(buffer); // read byte after byte from buffer + buffer++; + + crc16 ^= byte_value; // XOR byte into least sig. byte of crc + for (int i = 8; i != 0; i--) + { // Loop over each bit + if ((crc16 & 0x0001) != 0) + { // If the LSB is set + crc16 >>= 1; // Shift right and XOR 0xA001 + crc16 ^= 0xA001; + } + else + { // Else LSB is not set + crc16 >>= 1; // Just shift right + } + } + } + return crc16; +} + +// async receive function +uint8_t rx_buffer_ix_ptr = 0; // receive buffer index pointer +uint8_t rx_buffer_ix_rem = 255; // receive buffer remaining +uint8_t rx_buffer_rx_done = 0; // receive buffer complete flag (must be reset before next pkg can be received) +uint8_t *rx_buffer_temp; // receive buffer + +// setup async receive function +void setup_async_rx() +{ + rx_buffer_temp = malloc(PROTO_MAXPKGLEN); // reserve memory for rx buffer + UCSRB |= (1 << RXCIE); //enable rx interrupt +} + +ISR(USART_RXC_vect) +{ + if (rx_buffer_rx_done == 0) // only receive when last buffer was processed + { + uint8_t rx_byte = UDR; // read rx buffer + if (timer_ticks > 6) // too long since last byte + { + rx_buffer_ix_ptr = 0; // reset pointer and start new transaction + rx_buffer_ix_rem = 255; // reset remaining bytes to default + } + if (rx_buffer_ix_ptr == 0) + { + // wait for startbyte + if (rx_byte == SFBUS_SOF_BYTE) + { + *(rx_buffer_temp) = rx_byte; + rx_buffer_ix_ptr++; + } + } + else if (rx_buffer_ix_ptr > 0 && rx_buffer_ix_ptr < (PROTO_MAXPKGLEN - 1)) // when package is already started + { // and buffer is smaller than max length + *(rx_buffer_temp + rx_buffer_ix_ptr) = rx_byte; // receive single byte + rx_buffer_ix_rem--; + rx_buffer_ix_ptr++; + if (rx_buffer_ix_ptr == 3) // third byte is always frame length + { + rx_buffer_ix_rem = rx_byte; + } + if (rx_buffer_ix_rem == 0) // when last byte is received + { + rx_buffer_ix_ptr--; + rx_buffer_rx_done = 1; + } + } + timer_ticks = 0; // reset timer ticks + } +} + +int parse_buffer(uint16_t address, char *payload) +{ + if (rx_buffer_rx_done > 0) + { + uint8_t frm_length = *(rx_buffer_temp + 2); + // check protocol version + if (*(rx_buffer_temp + 1) != 0x01) + { + clear_buffer(); + return -1; // abort on incompatible version + } + if (frm_length < PROTO_MAXPKGLEN) // prevent buffer overflow + { + memcpy(payload, rx_buffer_temp + 3, frm_length); // copy buffer + // if version matches, read address and checksum + uint16_t recv_addr = (*(rx_buffer_temp + 3) & 0xFF) | ((*(rx_buffer_temp + 4) & 0xFF) << SHIFT_1B); + uint16_t checksum = (*(rx_buffer_temp + (rx_buffer_ix_ptr - 1)) & 0xFF) | + ((*(rx_buffer_temp + (rx_buffer_ix_ptr)) & 0xFF) << SHIFT_1B); + // calculate checksum of received payload + uint16_t checksum_calc = calc_CRC16((char *)rx_buffer_temp + 5, rx_buffer_ix_ptr - 6); + + //mctrl_set(address == recv_addr ? 20 : recv_addr + 1, 0); + clear_buffer(); + // return length on valid address and crc, return 0 to ignore + return (checksum == checksum_calc && address == recv_addr) ? frm_length : 0; + } + else + { + // invalid package + clear_buffer(); + return -1; + } + } + return -2; +} + +void clear_buffer() +{ + rx_buffer_ix_ptr = 0; // reset pointer and start new transaction + rx_buffer_ix_rem = PROTO_MAXPKGLEN; // reset remaining bytes to default + rx_buffer_rx_done = 0; // reset done flag + //memset(rx_buffer_temp, 0x00, PROTO_MAXPKGLEN); // clear buffer +} \ No newline at end of file diff --git a/software/firmware_module/module_rev3/src/rs485.h b/software/firmware_module/module_rev3/src/rs485.h new file mode 100644 index 0000000..c162815 --- /dev/null +++ b/software/firmware_module/module_rev3/src/rs485.h @@ -0,0 +1,41 @@ +/* Copyright (C) 2025 Dennis Gunia - All Rights Reserved + * You may use, distribute and modify this code under the + * terms of the AGPL-3.0 license. + * + * https://www.dennisgunia.de + * https://github.com/dennis9819/splitflap_v1 + */ +#include "global.h" + +#pragma once +//#define F_CPU 16000000UL +//#define UART_BAUD 19200 // RS485 baud rate +#define UART_BAUD 57600 // RS485 baud rate +#define BAUDRATE ((F_CPU) / (UART_BAUD * 16UL) - 1) // set baud rate value for UBRR + +#define SFBUS_SOF_BYTE '+' // Byte marks start of frame +#define SFBUS_EOF_BYTE '$' // Byte marks end of frame + +#ifdef __cplusplus +extern "C" +{ +#endif // __cplusplus + + void rs485_init(void); + void rs485_send_c(char data); + char rs485_recv_c(void); + int rs485_recv_c_rxout(uint8_t timeout, char *data); + + int sfbus_recv_frame_v2(uint16_t address, char *payload); + void sfbus_send_frame_v2(uint16_t address, char *payload, uint8_t length); + uint16_t calc_CRC16(char *buffer, uint8_t len); + + void setup_async_rx(); + int parse_buffer(uint16_t address, char *payload); + void clear_buffer(); + // auxilary var for uart timeout + extern uint16_t timer_ticks; + +#ifdef __cplusplus +} +#endif // __cplusplus