From 2b4a9cbd845432444515dc7d1f19646985ba1c43 Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Tue, 28 Jul 2026 23:17:03 +0300 Subject: [PATCH] simplest servo --- G4:G431/Servo/Makefile | 12 ++ G4:G431/Servo/commproto.cpp | 318 +++++++++++++++++++++++++++++++++++ G4:G431/Servo/commproto.h | 53 ++++++ G4:G431/Servo/hardware.c | 106 ++++++++++++ G4:G431/Servo/hardware.h | 40 +++++ G4:G431/Servo/main.c | 63 +++++++ G4:G431/Servo/openocd.cfg | 4 + G4:G431/Servo/ringbuffer.c | 203 ++++++++++++++++++++++ G4:G431/Servo/ringbuffer.h | 36 ++++ G4:G431/Servo/servo.bin | Bin 0 -> 6108 bytes G4:G431/Servo/servo.c | 70 ++++++++ G4:G431/Servo/servo.cflags | 1 + G4:G431/Servo/servo.config | 3 + G4:G431/Servo/servo.creator | 1 + G4:G431/Servo/servo.cxxflags | 1 + G4:G431/Servo/servo.files | 13 ++ G4:G431/Servo/servo.h | 34 ++++ G4:G431/Servo/servo.includes | 4 + G4:G431/Servo/strfunc.c | 265 +++++++++++++++++++++++++++++ G4:G431/Servo/strfunc.h | 31 ++++ G4:G431/Servo/usart.c | 257 ++++++++++++++++++++++++++++ G4:G431/Servo/usart.h | 49 ++++++ G4:G431/Servo/version.inc | 2 + 23 files changed, 1566 insertions(+) create mode 100644 G4:G431/Servo/Makefile create mode 100644 G4:G431/Servo/commproto.cpp create mode 100644 G4:G431/Servo/commproto.h create mode 100644 G4:G431/Servo/hardware.c create mode 100644 G4:G431/Servo/hardware.h create mode 100644 G4:G431/Servo/main.c create mode 100644 G4:G431/Servo/openocd.cfg create mode 100644 G4:G431/Servo/ringbuffer.c create mode 100644 G4:G431/Servo/ringbuffer.h create mode 100755 G4:G431/Servo/servo.bin create mode 100644 G4:G431/Servo/servo.c create mode 100644 G4:G431/Servo/servo.cflags create mode 100644 G4:G431/Servo/servo.config create mode 100644 G4:G431/Servo/servo.creator create mode 100644 G4:G431/Servo/servo.cxxflags create mode 100644 G4:G431/Servo/servo.files create mode 100644 G4:G431/Servo/servo.h create mode 100644 G4:G431/Servo/servo.includes create mode 100644 G4:G431/Servo/strfunc.c create mode 100644 G4:G431/Servo/strfunc.h create mode 100644 G4:G431/Servo/usart.c create mode 100644 G4:G431/Servo/usart.h create mode 100644 G4:G431/Servo/version.inc diff --git a/G4:G431/Servo/Makefile b/G4:G431/Servo/Makefile new file mode 100644 index 0000000..a1e7886 --- /dev/null +++ b/G4:G431/Servo/Makefile @@ -0,0 +1,12 @@ +BINARY := servo +PREFIX := /usr/bin/arm-none-eabi +CFLAGS = -std=c23 +CPPFLAGS = -std=c++11 +# MCU code +MCU := G431xx +# change this linking script depending on particular MCU model, +LDSCRIPT := stm32g431xb.ld + +include ../makefile.g4 +include ../../makefile.stm32 + diff --git a/G4:G431/Servo/commproto.cpp b/G4:G431/Servo/commproto.cpp new file mode 100644 index 0000000..e08263d --- /dev/null +++ b/G4:G431/Servo/commproto.cpp @@ -0,0 +1,318 @@ +/* + * This file is part of the as3935 project. + * Copyright 2026 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include + +extern "C"{ +#include + +#include "commproto.h" +//#include "flash.h" +#include "hardware.h" +#include "servo.h" +#include "strfunc.h" +} + +// sending function +static int (*SEND)(const char *str) = nullptr; + +extern volatile uint32_t Tms; + +//static uint8_t curbuf[MAXSTRLEN]; + +// COMMAND(USART, "Read USART data or send (USART=hex)") + +#define STR_HELPER(x) #x +#define STR(x) STR_HELPER(x) + +// list of all commands and handlers +#define COMMAND_TABLE \ + COMMAND(help, "show this help") \ + COMMAND(mcureset, "reset MCU") \ + COMMAND(servo, "get/set servoN value, mks (from " STR(SG90_MINPULSE) " to " STR(SG90_MAXPULSE) ")") \ + COMMAND(servopos, "get/set servoN target position with given speed") \ + COMMAND(servospeed, "get/set servoN speed, mks per 20ms (1.." STR(SG90_MAXSPEED) ")") \ + COMMAND(time, "show current time (ms)") \ + +typedef struct { + const char *name; + const char *desc; +} CmdInfo; + +// prototypes +#define COMMAND(name, desc) static errcodes_t cmd_ ## name(const char*, char*); +COMMAND_TABLE +#undef COMMAND + +static const CmdInfo cmdInfo[] = { // command name, description - for `help` +#define COMMAND(name, desc) { #name, desc }, + COMMAND_TABLE +#undef COMMAND +}; + +static const char* errtxt[ERR_AMOUNT] = { + [ERR_OK] = "OK\n", + [ERR_BADCMD] = "BADCMD\n", + [ERR_BADPAR] = "BADPAR\n", + [ERR_BADVAL] = "BADVAL\n", + [ERR_WRONGLEN] = "WRONGLEN\n", + [ERR_CANTRUN] = "CANTRUN\n", + [ERR_BUSY] = "BUSY\n", + [ERR_OVERFLOW] = "OVERFLOW\n", +}; + +const char *EQ = " = "; // equal sign for getters + +// send `command = ` +#define CMDEQ() do{SEND(cmd); SEND(EQ);}while(0) +// send `commandXXX = ` +#define CMDEQP(x) do{SEND(cmd); SEND(u2str((uint32_t)x)); SEND(EQ);}while(0) + + +/** + * @brief splitargs - get command parameter and setter from `args` + * @param args (i) - rest of string after command (like `1 = PU OD OUT`) + * @param parno (o) - parameter number or -1 if none + * @return setter (part after `=` without leading spaces) or NULL if none + */ +static char *splitargs(char *args, int32_t *parno){ + if(!args) return NULL; + uint32_t U32; + char *next = getnum(args, &U32); + int p = -1; + if(next != args && U32 <= MAXPARNO) p = U32; + if(parno) *parno = p; + next = strchr(next, '='); + if(next){ + if(*(++next)) next = omit_spaces(next); + if(*next == 0) next = NULL; + } + return next; +} + +/** + * @brief argsvals - split `args` into `parno` and setter's value + * @param args - rest of string after command + * @param parno (o) - parameter number or -1 if none + * @param parval - integer setter's value + * @return false if no setter or it's not a number, true - got setter's num + */ +static bool argsvals(char *args, int32_t *parno, int32_t *parval){ + char *setter = splitargs(args, parno); + if(!setter) return false; + int32_t I32; + char *next = getint(setter, &I32); + if(next != setter && parval){ + *parval = I32; + return true; + } + return false; +} + +static errcodes_t cmd_servo(const char *cmd, char *args){ + int32_t val, parno; + if(argsvals(args, &parno, &val)){ // setter + if(parno < 0 || parno >= SERVO_AMOUNT) return ERR_BADPAR; + if(!set_servo((uint8_t)parno, (uint16_t)val)) return ERR_BADVAL; + } + uint16_t curval; + if(!get_servo((uint8_t)parno, &curval)) return ERR_BADPAR; + CMDEQP(parno); + SEND(u2str(curval)); + SEND("\n"); + return ERR_AMOUNT; +} + +static errcodes_t cmd_servopos(const char *cmd, char *args){ + int32_t val, parno; + if(argsvals(args, &parno, &val)){ // setter + if(parno < 0 || parno >= SERVO_AMOUNT) return ERR_BADPAR; + if(!servo_set_tagpos((uint8_t)parno, (uint16_t)val)) return ERR_BADVAL; + } + uint16_t curval; + if(!servo_get_tagpos((uint8_t)parno, &curval)) return ERR_BADPAR; + CMDEQP(parno); + SEND(u2str(curval)); + SEND("\n"); + return ERR_AMOUNT; +} + +static errcodes_t cmd_servospeed(const char *cmd, char *args){ + int32_t val, parno; + if(argsvals(args, &parno, &val)){ // setter + if(parno < 0 || parno >= SERVO_AMOUNT) return ERR_BADPAR; + if(!servo_set_speed((uint8_t)parno, (uint16_t)val)) return ERR_BADVAL; + } + uint16_t curval; + if(!servo_get_speed((uint8_t)parno, &curval)) return ERR_BADPAR; + CMDEQP(parno); + SEND(u2str(curval)); + SEND("\n"); + return ERR_AMOUNT; +} + +static errcodes_t cmd_time(const char *cmd, char*){ + CMDEQ(); + SEND(u2str(Tms)); SEND("\n"); + return ERR_AMOUNT; +} + +static errcodes_t cmd_mcureset(const char*, char*){ + NVIC_SystemReset(); + return ERR_CANTRUN; // never reached +} +#if 0 +static errcodes_t cmd_saveconf(const char*, char*){ + if(store_userconf()) return ERR_CANTRUN; + return ERR_OK; +} + +static errcodes_t cmd_eraseflash(const char*, char*){ + if(erase_storage(-1)) return ERR_CANTRUN; + return ERR_OK; +} + +static errcodes_t cmd_readconf(const char*, char* args){ + int32_t CHno = -1; + splitargs(args, &CHno); + if(CHno < 0 || CHno >= SENSORS_AMOUNT) return ERR_BADPAR; + as3935_channel = static_cast(CHno); + uint8_t par; + if(!as3935_get_gain(&par)) return ERR_CANTRUN; + the_conf.spars[CHno].AFE_GB = par; + if(!as3935_get_wdthres(&par)) return ERR_CANTRUN; + the_conf.spars[CHno].WDTH = par; + if(!as3935_get_nflev(&par)) return ERR_CANTRUN; + the_conf.spars[CHno].NF_LEV = par; + if(!as3935_get_srej(&par)) return ERR_CANTRUN; + the_conf.spars[CHno].SREJ = par; + if(!as3935_get_minnumlig(&par)) return ERR_CANTRUN; + the_conf.spars[CHno].MIN_NUM_LIG = par; + if(!as3935_get_maskdist(&par)) return ERR_CANTRUN; + the_conf.spars[CHno].MASK_DIST = par; + if(!as3935_get_lco_fdiv(&par)) return ERR_CANTRUN; + the_conf.spars[CHno].LCO_FDIV = par; + if(!as3935_get_tuncap(&par)) return ERR_CANTRUN; + the_conf.spars[CHno].TUN_CAP = par; + return ERR_OK; +} + +static void showpar(const char *par, uint8_t n, uint8_t v){ + char c[2]; + c[0] = '0' + n; c[1] = 0; + SEND(par); SEND(c); SEND(EQ); + SEND(u2str(v)); +} + +static errcodes_t cmd_dumpconf(const char*, char*){ + SEND("userconf_sz="); SEND(u2str(the_conf.userconf_sz)); + SEND("\ncurr_idx="); SEND(u2str(currentconfidx)); + SEND("\ncapacity="); SEND(u2str(maxCnum-2)); + cmd_setiface("\nsetiface", NULL); + cmd_restonstart("restonstart", NULL); + for(int i = 0; i < SENSORS_AMOUNT; ++i){ + showpar("gain", i, the_conf.spars[i].AFE_GB); + showpar("\nlco_fdiv", i, the_conf.spars[i].LCO_FDIV); + showpar("\nmaskdist", i, the_conf.spars[i].MASK_DIST); + showpar("\nminnumlig", i, the_conf.spars[i].MIN_NUM_LIG); + showpar("\nnflev", i, the_conf.spars[i].NF_LEV); + showpar("\nsrej", i, the_conf.spars[i].SREJ); + showpar("\ntuncap", i, the_conf.spars[i].TUN_CAP); + showpar("\nwdthres", i, the_conf.spars[i].WDTH); + SEND("\n"); + } + return ERR_AMOUNT; +} +#endif + +static errcodes_t cmd_help(const char*, char*){ + SEND(REPOURL); + for(size_t i = 0; i < sizeof(cmdInfo)/sizeof(cmdInfo[0]); i++){ + SEND(cmdInfo[i].name); + SEND(" - "); + SEND(cmdInfo[i].desc); SEND("\n"); + } + return ERR_AMOUNT; +} + +#if 0 +/** + * @brief parse_hex_data - data parsing in case of `hex + text` input format + * @param input - input string + * @param output - output data + * @param max_len - length of `output` + * @return amount of parsed bytes or -1 in case of overflow or error + */ +static int parse_hex_data(char *input, uint8_t *output, int max_len){ + if(!input || !*input || !output || max_len < 1) return 0; + char *p = input; + int out_idx = 0; + while(*p && out_idx < max_len){ + while(*p == ' ' || *p == ',') ++p; // omit spaces and commas as delimeters + if(*p == '\0') break; // EOL + if(*p == '"'){ // TEXT (start/end) + ++p; + while(*p && *p != '"'){ + if(out_idx >= max_len) return -1; + output[out_idx++] = *p++; + } + if(*p == '"'){ + ++p; // go to next symbol after closing quotation mark + }else return -1; // no closing + }else{ // HEX number + char *start = p; + while(*p && *p != ' ' && *p != ',' && *p != '"') ++p; + char saved = *p; + *p = '\0'; // temporarily for `gethex` + uint32_t val; + const char *end = gethex(start, &val); + if(end != p || val > 0xFF){ // not a hex number or have more than 2 symbols + *p = saved; + return -1; + } + *p = saved; + output[out_idx++] = (uint8_t)val; + } + } + return out_idx; +} +#endif + +constexpr uint32_t hash(const char* str, uint32_t h = 0){ + return *str ? hash(str + 1, h + ((h << 7) ^ *str)) : h; +} + +const char *parse_cmd(int (*sendfun)(const char *), char *str){ + SEND = sendfun; + char command[CMD_MAXLEN+1]; + int i = 0; + while(*str > '@' && i < CMD_MAXLEN){ command[i++] = *str++; } + command[i] = 0; + while(*str && *str <= ' ') ++str; + char *restof = (char*) str; + uint32_t h = hash(command); + errcodes_t ecode = ERR_AMOUNT; + switch(h){ +#define COMMAND(name, desc) case hash(#name): ecode = cmd_ ## name(command, restof); break; + COMMAND_TABLE +#undef COMMAND + default: SEND("Unknown command, try 'help'\n"); break; + } + if(ecode < ERR_AMOUNT) return errtxt[ecode]; + return NULL; +} diff --git a/G4:G431/Servo/commproto.h b/G4:G431/Servo/commproto.h new file mode 100644 index 0000000..09eacbe --- /dev/null +++ b/G4:G431/Servo/commproto.h @@ -0,0 +1,53 @@ +/* + * This file is part of the servo project. + * Copyright 2026 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include "version.inc" + +#ifdef EBUG +#define RLSDBG "debug" +#else +#define RLSDBG "release" +#endif + +#define REPOURL "https://github.com/eddyem/stm32samples/tree/master/TODO " RLSDBG " build #" BUILD_NUMBER "@" BUILD_DATE "\n" + + +// error codes for answer message +typedef enum{ + ERR_OK, // all OK + ERR_BADCMD, // wrong command + ERR_BADPAR, // wrong parameter + ERR_BADVAL, // wrong value (for setter) + ERR_WRONGLEN, // wrong message length + ERR_CANTRUN, // can't run given command due to bad parameters or other + ERR_BUSY, // target interface busy, try later + ERR_OVERFLOW, // string was too long -> overflow + ERR_AMOUNT // amount of error codes or "send nothing" +} errcodes_t; + +// maximal length of command (without trailing zero) +#define CMD_MAXLEN 15 +// maximal available parameter number +#define MAXPARNO 255 +// maximal string length includint terminating zero +#define MAXSTRLEN 256 + +extern const char *EQ; +const char *parse_cmd(int (*sendfun)(const char*), char *buf); diff --git a/G4:G431/Servo/hardware.c b/G4:G431/Servo/hardware.c new file mode 100644 index 0000000..48bece7 --- /dev/null +++ b/G4:G431/Servo/hardware.c @@ -0,0 +1,106 @@ +/* + * This file is part of the usart project. + * Copyright 2026 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include + +#include "hardware.h" +#include "servo.h" + +volatile uint32_t Tms = 0; +volatile bool tim_triggered[SERVO_AMOUNT]; + +/* Called when systick fires */ +void sys_tick_handler(){ + ++Tms; +} + +// try to set servo; if wrong return false +bool set_servo(uint8_t N, uint16_t val){ + if(N >= SERVO_AMOUNT) return false; + if(val < SG90_MINPULSE || val > SG90_MAXPULSE) return false; + volatile uint32_t *CCR = &(TIM3->CCR1); + CCR[N] = val; + return true; +} + +bool get_servo(uint8_t N, uint16_t *val){ + if(N >= SERVO_AMOUNT) return false; + volatile uint32_t *CCR = &(TIM3->CCR1); + if(val) *val = CCR[N]; + return true; +} + +static void tim3_setup(){ + RCC->APB1ENR1 |= RCC_APB1ENR1_TIM3EN; + __DSB(); + // PWM mode 1 (active -> inactive) on all three channels; preload enabled + TIM3->CCMR1 = TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_1 | + TIM_CCMR1_OC2M_2 | TIM_CCMR1_OC2M_1 | + TIM_CCMR1_OC1PE | TIM_CCMR1_OC2PE; + TIM3->CCMR2 = TIM_CCMR2_OC3M_2 | TIM_CCMR2_OC3M_1 | + TIM_CCMR2_OC4M_2 | TIM_CCMR2_OC4M_1 | + TIM_CCMR2_OC3PE | TIM_CCMR2_OC4PE; + // frequency + TIM3->PSC = 169; // 1MHz -> 1us per tick + // ARR for PWM + TIM3->ARR = 19999; // 20ms, 50Hz + // CCRx - minimal position + TIM3->CCR1 = TIM3->CCR2 = TIM3->CCR3 = TIM3->CCR4 = SG90_MINPULSE; + // enable CCx interrupts + TIM3->DIER = TIM_DIER_CC1IE | TIM_DIER_CC2IE | TIM_DIER_CC3IE | TIM_DIER_CC4IE; + // enable main output + //TIM3->BDTR |= TIM_BDTR_MOE; + // enable PWM output + TIM3->CCER = TIM_CCER_CC1E | TIM_CCER_CC2E |TIM_CCER_CC3E | TIM_CCER_CC4E; + // enable timer & ARR buffering + TIM3->CR1 |= TIM_CR1_CEN | TIM_CR1_ARPE; + // update buffers + TIM3->EGR = TIM_EGR_UG; + // and enable interrupt + NVIC_EnableIRQ(TIM3_IRQn); +} + +// TIM3 channels: 1 - PA6 (AF2), 2 - PA7 (AF2), 3 - PB0 (AF2), 4 - PB1 (AF2) +void gpio_setup(){ + RCC->AHB2ENR = RCC_AHB2ENR_GPIOAEN | RCC_AHB2ENR_GPIOBEN | RCC_AHB2ENR_GPIOCEN; + __DSB(); // Data Synchronization Barrier - wait until data will be really written to AHB2ENR + // set PC6 as push-pull output, PC13 is pulldown input, other as default (AIN) + GPIOC->MODER = (0xffffffff & ~(GPIO_MODER_MODE6 | GPIO_MODER_MODE13)) | MODER_O(6) | MODER_I(13); + GPIOC->PUPDR = PUPD_PD(13); // pulldown + GPIOC->OTYPER = OTYPER_PP(6); // push-pull (default) + // PA9 (Tx) and PA10 (Rx) (don't forget about SWDIO) + GPIOA->MODER = (0xabffffff & ~(GPIO_MODER_MODE6 | GPIO_MODER_MODE7 | GPIO_MODER_MODE9 | GPIO_MODER_MODE10)) | + MODER_AF(6) | MODER_AF(7) | MODER_AF(9) | MODER_AF(10); + GPIOA->AFR[0] = AFRf(2, 6) | AFRf(2, 7); + GPIOA->AFR[1] = AFRf(7, 9) | AFRf(7, 10); // SWDIO is 0 by default; USART1 is AF7 + GPIOB->MODER = (0xffffffff & ~(GPIO_MODER_MODE0 | GPIO_MODER_MODE1)) | + MODER_AF(0) | MODER_AF(1); + GPIOB->AFR[0] = AFRf(2, 0) | AFRf(2, 1); + // count milliseconds + SysTick_Config(SysFreq / 1000); // arg should be < 0xffffff + tim3_setup(); +} + +// update event flags +void tim3_isr(){ + if(TIM3->SR & TIM_SR_CC1IF) tim_triggered[0] = true; + if(TIM3->SR & TIM_SR_CC2IF) tim_triggered[1] = true; + if(TIM3->SR & TIM_SR_CC3IF) tim_triggered[2] = true; + if(TIM3->SR & TIM_SR_CC4IF) tim_triggered[3] = true; + TIM3->SR = 0; +} diff --git a/G4:G431/Servo/hardware.h b/G4:G431/Servo/hardware.h new file mode 100644 index 0000000..3eb6e4e --- /dev/null +++ b/G4:G431/Servo/hardware.h @@ -0,0 +1,40 @@ +/* + * This file is part of the usart project. + * Copyright 2026 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +// KEY (intpullup->0) - PC13 +// LED - PC6 +#define KEY_PORT GPIOC +#define KEY_PIN (1<<13) +#define LED_PORT GPIOC +#define LED_PIN (1<<6) +#define KEY_PRESSED() (pin_read(KEY_PORT, KEY_PIN) == 1) +#define LED_ON() do{pin_set(LED_PORT, LED_PIN);}while(0) +#define LED_OFF() do{pin_clear(LED_PORT, LED_PIN);}while(0) +#define LED_TOGG() do{pin_toggle(LED_PORT, LED_PIN);}while(0) + +#define SERVO_AMOUNT 4 + +extern volatile uint32_t Tms; +extern volatile bool tim_triggered[SERVO_AMOUNT]; + +void gpio_setup(); + +bool set_servo(uint8_t N, uint16_t val); +bool get_servo(uint8_t N, uint16_t *val); diff --git a/G4:G431/Servo/main.c b/G4:G431/Servo/main.c new file mode 100644 index 0000000..c5851fa --- /dev/null +++ b/G4:G431/Servo/main.c @@ -0,0 +1,63 @@ +/* + * This file is part of the blink project. + * Copyright 2023 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include + +#include "commproto.h" +#include "hardware.h" +#include "servo.h" +#include "usart.h" + +// 40ms for debouncing +#define TDEBOUNCE (40) + +int main(void){ + StartHSE(); + gpio_setup(); + usart_setup(115200); + bool pressed = false; + uint32_t Tblink = 0, Tkey = 0; // blink and key pressed time + while(1){ + USART_flags_t f = usart_process(); + if(f.rxovrfl) usart_sendstr("Rx buffer overflow!\n"); + if(f.txerr) usart_sendstr("Tx error!\n"); + char *str = usart_getline(); + if(str){ + const char *ans = parse_cmd(usart_sendstr, str); + if(ans) usart_sendstr(ans); + } + process_servo(); + if(Tms - Tblink > 499){ + LED_TOGG(); + Tblink = Tms; + } + if(KEY_PRESSED()){ + if(!pressed){ // new event + pressed = true; + usart_sendstr("Key pressed\n"); + } + Tkey = Tms; + }else{ // key released + if(pressed && (Tms - Tkey) > TDEBOUNCE){ // wait for debounce + pressed = false; + usart_sendstr("Key released\n"); + } + } + } +} diff --git a/G4:G431/Servo/openocd.cfg b/G4:G431/Servo/openocd.cfg new file mode 100644 index 0000000..22582a6 --- /dev/null +++ b/G4:G431/Servo/openocd.cfg @@ -0,0 +1,4 @@ +set FLASH_SIZE 0x20000 + +source [find interface/stlink-v2-1.cfg] +source [find target/stm32g4x.cfg] diff --git a/G4:G431/Servo/ringbuffer.c b/G4:G431/Servo/ringbuffer.c new file mode 100644 index 0000000..5e28ee0 --- /dev/null +++ b/G4:G431/Servo/ringbuffer.c @@ -0,0 +1,203 @@ +/* + * Copyright 2023 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include +#include "ringbuffer.h" + +#define CHK(b) do{if(!b) return -1;}while(0) + +static int datalen(ringbuffer *b){ + if(b->tail >= b->head) return (b->tail - b->head); + else return (b->length - b->head + b->tail); +} + +// stored data length +int RB_datalen(ringbuffer *b){ + CHK(b); + if(0 == datalen(b)) return 0; // don't block for empty RO operations + if(b->busy) return -1; + b->busy = true; + int l = datalen(b); + b->busy = false; + return l; +} + +static int hasbyte(ringbuffer *b, uint8_t byte){ + if(b->head == b->tail) return -1; // no data in buffer + int startidx = b->head; + if(b->head > b->tail){ // + for(int found = b->head; found < b->length; ++found) + if(b->data[found] == byte) return found; + startidx = 0; + } + for(int found = startidx; found < b->tail; ++found) + if(b->data[found] == byte) return found; + return -1; +} + +/** + * @brief RB_hasbyte - check if buffer has given byte stored + * @param b - buffer + * @param byte - byte to find + * @return index if found, -1 if none or busy + */ +int RB_hasbyte(ringbuffer *b, uint8_t byte){ + CHK(b); + if(b->busy) return -1; + b->busy = true; + int ret = hasbyte(b, byte); + b->busy = false; + return ret; +} + +// increment head or tail +TRUE_INLINE void incr(ringbuffer *b, volatile int *what, int n){ + *what += n; + if(*what >= b->length) *what -= b->length; +} + +static int read(ringbuffer *b, uint8_t *s, int len){ + int l = datalen(b); + if(!l) return 0; + if(l > len) l = len; + int _1st = b->length - b->head; + if(_1st > l) _1st = l; + if(_1st > len) _1st = len; + memcpy(s, b->data + b->head, _1st); + if(_1st < len && l > _1st){ + memcpy(s+_1st, b->data, l - _1st); + incr(b, &b->head, l); + return l; + } + incr(b, &b->head, _1st); + return _1st; +} + +/** + * @brief RB_read - read data from ringbuffer + * @param b - buffer + * @param s - array to write data + * @param len - max len of `s` + * @return bytes read or -1 if busy + */ +int RB_read(ringbuffer *b, uint8_t *s, int len){ + CHK(b); + if(!s || len < 1) return -1; + if(0 == datalen(b)) return 0; + if(b->busy) return -1; + b->busy = true; + int r = read(b, s, len); + b->busy = false; + return r; +} + +// length of data from current position to `byte` (including byte) +static int lento(ringbuffer *b, uint8_t byte){ + int idx = hasbyte(b, byte); + if(idx < 0) return 0; + int partlen = idx + 1 - b->head; + // now calculate length of new data portion + if(idx < b->head) partlen += b->length; + return partlen; +} + +static int readto(ringbuffer *b, uint8_t byte, uint8_t *s, int len){ + int partlen = lento(b, byte); + if(!partlen) return 0; + if(partlen > len) return -1; + return read(b, s, partlen); +} + +/** + * @brief RB_readto fill array `s` with data until byte `byte` (with it) + * @param b - ringbuffer + * @param byte - check byte + * @param s - buffer to write data or NULL to clear data + * @param len - length of `s` or 0 to clear data + * @return amount of bytes written (negative, if lenbusy) return -1; + b->busy = true; + int n = 0; + if(s && len > 0){ + n = readto(b, byte, s, len); + }else{ + incr(b, &b->head, lento(b, byte)); // just throw data out + } + b->busy = false; + return n; +} + +int RB_datalento(ringbuffer *b, uint8_t byte){ + CHK(b); + if(0 == datalen(b)) return 0; + if(b->busy) return -1; + b->busy = true; + int n = lento(b, byte); + b->busy = false; + return n; +} + +// if l < rest of buffer, truncate and return actually written bytes +static int write(ringbuffer *b, const uint8_t *str, int l){ + int r = b->length - 1 - datalen(b); // rest length + if(r < 1) return 0; + if(l > r) l = r; + int _1st = b->length - b->tail; + if(_1st > l) _1st = l; + memcpy(b->data + b->tail, str, _1st); + if(_1st < l){ // add another piece from start + memcpy(b->data, str+_1st, l-_1st); + } + incr(b, &b->tail, l); + return l; +} + +/** + * @brief RB_write - write some data to ringbuffer + * @param b - buffer + * @param str - data + * @param l - length + * @return amount of bytes written or -1 if busy + */ +int RB_write(ringbuffer *b, const uint8_t *str, int l){ + CHK(b); + if(!str || l < 1) return -1; + if(b->length - datalen(b) < 2) return 0; + if(b->busy) return -1; + b->busy = true; + int w = write(b, str, l); + b->busy = false; + return w; +} + +// just delete all information in buffer `b` +int RB_clearbuf(ringbuffer *b){ + CHK(b); + if(b->busy) return -1; + b->busy = true; + b->head = 0; + b->tail = 0; + memset(b->data, 0, b->length); + b->busy = false; + return 1; +} diff --git a/G4:G431/Servo/ringbuffer.h b/G4:G431/Servo/ringbuffer.h new file mode 100644 index 0000000..fd859bd --- /dev/null +++ b/G4:G431/Servo/ringbuffer.h @@ -0,0 +1,36 @@ +/* + * Copyright 2023 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include + +typedef struct{ + uint8_t *data; // data buffer + const int length; // its length + int head; // head index + int tail; // tail index + volatile bool busy; // == TRUE if buffer is busy now +} ringbuffer; + +int RB_read(ringbuffer *b, uint8_t *s, int len); +int RB_readto(ringbuffer *b, uint8_t byte, uint8_t *s, int len); +int RB_hasbyte(ringbuffer *b, uint8_t byte); +int RB_write(ringbuffer *b, const uint8_t *str, int l); +int RB_datalen(ringbuffer *b); +int RB_datalento(ringbuffer *b, uint8_t byte); +int RB_clearbuf(ringbuffer *b); diff --git a/G4:G431/Servo/servo.bin b/G4:G431/Servo/servo.bin new file mode 100755 index 0000000000000000000000000000000000000000..e7a0cdc04425fe77906fc7d364b17c73e9ed7ff9 GIT binary patch literal 6108 zcmbU_3v?4@y8p~1P5Pj(fTn3LPCKA!Q`*oX6r*c84bwI$R6!KcJx!lhmy) zy54Qim+$``|M&b~3GyQPS4yNt|HR)2fz{&ZMQA@zf|(8sR-KXxwt1HTJz;p0~J zr&GqVnK8^9|D$=+@@ZQ?hJQ$R+W$|PJ8fg;obQh@OQSH)fm*vs4U`Tj6;Pd#+HRussL?I>-d&Mqn{ z)wxJpwI~m?3^T(e~WoOo8Yix;nu|Jhty5x-#PwLn^CgYnez;8aHX{&RNTo zhjDBOk&$^k%)L2|eEv)1!d+~EKeb;w#tI#5`ezD(Fe!p$5@SgU$@db0+GT40TbxEB z3KR0A!QX`luE=gsO^WAT< zw{ypRtZ+v6weEebg4@B(I>HHrFu?8)QgH`-_i+c=1HJ<8H|%@decS=x-CTwxk26Dj z?b2^pVr==cq|qLsMle`<_#UVm2~?M^hu3ohUr5{D3o6NMK^;^gLfj%@J=^#6mUEZJ zrT&bnT94)!ny=@i0bGxBm*SZY_P8&5{Z3t%?lCPgi2m24d?U!>km1Y0{3ve@S2l*%0w?TcKlDxLPJ#E!IwDWOEgU1CAw?N=9(T_@t3!rrksx!L z^2XkT`uq#mtm|iK;Q&naUSTlG3FRV?7v0TX^@uIBY%tvmkzHP-HCA7O*L~85R zf6r3~=IOi^rA6yE2|dAhN@36pr&Ro2LftuBE+QZwJ<@CG z({k)HI$vp{f@bQ7^jgu(`B6E=CEy6$eGq|in2Ooi`$Ei0#D;1(yH^ph(e?5S3m{9i zBIY^i-6H)lBjEi-XVXVp}2TM`j6lKX;HWPC1?hy|fx z*qb(-2ae;@)2B~Up3Gy4K&nLCpV&(qu8%}=^layl0c!4b^^+VKNC)Yzh}39UKe)<6 ziJzl2de+|)GAbU(`g5NuZ~h2k5_#9aY!AwB_Wx`}Du?DrtX6}L^0nS~LdIle@OkP# zL-8pJs9F4QKT8gOGkACYS0fE#Ygr=CbFP8s*jfB_ziR;2TuywTFNB^D&&&be`yq$o zUhM5JeX4va_)(lVa)3?X|KiuftPT%ZgbXG>_L&w?7W9Pdia8_7{67vqGOs`6eWE9* zpiZBmg338KOJq0|i90^OCsZ0YK5U%#)%cQmUyegg6l-`3vRj??>s~!XkPtw5)nBsY z!O?{VJX7ssVdrH=;vn+Hk>??T9*7oa&U0^%M=DUhckkW|v7hp6JFMiAIR%$_EIlwA zX17G@x4@7%NGu}WqhWtk$=g}jX-g6Y^&E9r9EfsDBV0l?=_k67AvCHAQxnB-riugj z?m(!xa=|5UMWVon-p1z-C-RBH8-!}8&_)CZ6Xi#~1mRWU+PS>SM4@cbTa4vU zX2@Tst_|f_naYm(GSR!L!vVG?Pt+#zHs@nX)lk;AnpWp(<+XFSSL$r1zzXO#Vry>^ zs&krebrOW##I>PxYjULq`~_;uL5|F-uc6IeXPEeo)$D#JAyZ0psx- zs`PV&Eir&nN%KuP1tAwQYgQvTB}p^fe~<}i(XXncp@WqNZ72 zM{4)hK3ZFPIW29N`|2)a~kBQp3kc8F}1fp6cyJoQXPsi-8{>(Vw-SL|h_lit#_ zb=_f#+FY5LbPD37Yi<0#E8dkSm|c7)bJp&*t*SY|bnu&O8oQSHNJ1qU@Tn-VT%JY3 zDl1Ol6KmwDn_-k)J&9`R9I|~qZ{Xo31AmU?L_2DKiG$m3TiLRszMS=iTc4NI-5>h) z!41+AR8w8rW88mDm{s^xFt+{eMl_4LZ3OdhMf5J*8%FvQm*=*5mz@L~!0SdKg2MAp z`nt4)!90=PCQn&zr1lzwPo&hMO52c>Jjk%v zuLvcf#FM+=PAt_u26rMmcU5opvxF!opZ;_SiIC90?O5$7aGTByGQ3N}$^u`M;)rG`6+K^s3X12(BVx;?u*YySe9Vcztc0l&B4(N5#qUJxK(T-&()P_>i3t6EfGo_NvQT5!PEdS@gm zRo5|s5h|S!6VBKYKFr?L96wlKsjYNWydceXBQZU!?v&O);k-Q!B1V*TiGT-u_zELNMnpk?^G3{!^!C4S7#;HPOZgT78Hho|ED5MWNmK^s5#*`V8y+}k05MvQj@aAX|6(jRm`SQasW7_)te%o7c(dH@ zH-Tkx)a0ZL#2Cq#119Ha-&W_VeQBH)I!lHrZX4V}IC8>IF~*2o6-p*=B_|m=kZ#Kq zJi?yxR9lu{f~@2zGuY5_-R1C|Qb@9<3#r_m@_3MjcVg+nm{b8$WyPLyHAJ2Tk?)54 zTMci}$DyCyw;}Zp{)CcjULu<2>m6a2}t=r2lLl)KAJ|&3EGb`+ltb zj{Mencz!ew=TFXKTWnn=$2$J~{w4rF(*GG2<6<37igb9ozkIC6-u~xeHsz^1ZYp6V zsAGF#dyGE^Y|a52hzawVfi$4fE3XR=N<}nYRkMIj0coO5CnQydg;A7fc?@FMU9JL2 z0!YH}n@5-r3ra&^1h2HzhPm+4qvipBbfwMEeIu)Av#0`~~3vP^u-`bV?2)TDd##U=|S(fKB@ zY&MW1E$fpFUyrFmYO-a$#sa^mxJa-_6q|UgfzJ$#X~X1*NeJ6PkHX*NvCyBXzaXYihl@selohGRTMAU@VZ zA&sSiTFKdg@y(D4t=$FGjTp6?LCh$tMHY}$q4#*FRZCQ z^x+)~zXTc5dtYL&4Z?S|T&)j(FC{G$*TEv-P!f@OZ9Ku!&Z3Z9j5(?OX%b-resw#{ zNgbdf7EoSzP^%+Kzy@0Bp=+LAx6a|F8=4*NbsHKs!#f539`eu*x4XfODVt*p?Q%OB z8y$5K8K&Ki4Gw#ZAviX`RfSNa(6E_q5}b{6lv7{(G|X}|VOeBAFE3k-^&IZz2Eb1` znhHToN9YxFvwg$Uj(ocQsYY7A&fQQ?FEJYFrUtrbq0yL&ZAA234UKsGy}3Xz+KTx_df*&reN;SZ{A^a<~g0s5_XC>g+)zg|OU*^bqQwi0OBNKBV6*V!8;w?P zd}?FE=8ZHMt+#Ki%cq;%Tj;xRukMQItbw-%kKY{t&j9QI*bQ(3;55Ju05zN^ivaEg zP=MXp02Dy^iq&XC-5Vc#@v}#UwvnTq#aSnoE7BIf54v_|L#ae$J3v1 j2K*flrEf(ZW%U0c@U;ej2M_>U0M7vI0N4%i2EhLTmnTJB literal 0 HcmV?d00001 diff --git a/G4:G431/Servo/servo.c b/G4:G431/Servo/servo.c new file mode 100644 index 0000000..0df3a79 --- /dev/null +++ b/G4:G431/Servo/servo.c @@ -0,0 +1,70 @@ +/* + * This file is part of the servo project. + * Copyright 2026 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include + +#include "hardware.h" +#include "servo.h" + +static uint16_t servo_tagpos[SERVO_AMOUNT] = {SG90_MIDPULSE, SG90_MIDPULSE, SG90_MIDPULSE, SG90_MIDPULSE}; +static uint16_t servo_speed[SERVO_AMOUNT] = {0, 0, 0, 0}; + +void process_servo(){ + for(uint8_t i = 0; i < SERVO_AMOUNT; ++i){ + uint16_t curpos; + if(!get_servo(i, &curpos) || !tim_triggered[i]) continue; + tim_triggered[i] = false; + if(servo_tagpos[i] == curpos || servo_speed[i] < 1) continue; + int32_t newpos = curpos; + if(servo_tagpos[i] > curpos){ + newpos += servo_speed[i]; + if(newpos > servo_tagpos[i]) newpos = servo_tagpos[i]; + }else{ + newpos -= servo_speed[i]; + if(newpos < servo_tagpos[i]) newpos = servo_tagpos[i]; + } + set_servo(i, (uint16_t) newpos); + } +} + +// set speed in steps per tick +bool servo_set_speed(uint8_t N, uint16_t s){ + if(N >= SERVO_AMOUNT) return false; + if(s < 1 || s > SG90_MAXSPEED) return false; + servo_speed[N] = s; + return true; +} + +bool servo_get_speed(uint8_t N, uint16_t *s){ + if(N >= SERVO_AMOUNT) return false; + if(s) *s = servo_speed[N]; + return true; +} + +bool servo_set_tagpos(uint8_t N, uint16_t pos){ + if(N >= SERVO_AMOUNT) return false; + if(pos < SG90_MINPULSE || pos > SG90_MAXPULSE) return false; + servo_tagpos[N] = pos; + return true; +} + +bool servo_get_tagpos(uint8_t N, uint16_t *pos){ + if(N >= SERVO_AMOUNT) return false; + if(pos) *pos = servo_tagpos[N]; + return true; +} diff --git a/G4:G431/Servo/servo.cflags b/G4:G431/Servo/servo.cflags new file mode 100644 index 0000000..a07354f --- /dev/null +++ b/G4:G431/Servo/servo.cflags @@ -0,0 +1 @@ +-std=c23 \ No newline at end of file diff --git a/G4:G431/Servo/servo.config b/G4:G431/Servo/servo.config new file mode 100644 index 0000000..c4aadd9 --- /dev/null +++ b/G4:G431/Servo/servo.config @@ -0,0 +1,3 @@ +#define EBUG +#define STM32G4 +#define STM32G431xx diff --git a/G4:G431/Servo/servo.creator b/G4:G431/Servo/servo.creator new file mode 100644 index 0000000..e94cbbd --- /dev/null +++ b/G4:G431/Servo/servo.creator @@ -0,0 +1 @@ +[General] diff --git a/G4:G431/Servo/servo.cxxflags b/G4:G431/Servo/servo.cxxflags new file mode 100644 index 0000000..ea5966d --- /dev/null +++ b/G4:G431/Servo/servo.cxxflags @@ -0,0 +1 @@ +-std=c++23 \ No newline at end of file diff --git a/G4:G431/Servo/servo.files b/G4:G431/Servo/servo.files new file mode 100644 index 0000000..4d8ca89 --- /dev/null +++ b/G4:G431/Servo/servo.files @@ -0,0 +1,13 @@ +commproto.cpp +commproto.h +hardware.c +hardware.h +main.c +ringbuffer.c +ringbuffer.h +servo.c +servo.h +strfunc.c +strfunc.h +usart.c +usart.h diff --git a/G4:G431/Servo/servo.h b/G4:G431/Servo/servo.h new file mode 100644 index 0000000..0dbea5e --- /dev/null +++ b/G4:G431/Servo/servo.h @@ -0,0 +1,34 @@ +/* + * This file is part of the servo project. + * Copyright 2026 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +// minimal and maximal pulse length for SG90 +#define SG90_MINPULSE 700 +#define SG90_MAXPULSE 2100 +#define SG90_MIDPULSE ((SG90_MINPULSE+SG90_MAXPULSE)/2) +#define SG90_AMPL (SG90_MAXPULSE-SG90_MINPULSE) +// maximal speed: 0.1s (5 ticks) per 60degr (1/3 of range): (SG90_AMPL/15) +#define SG90_MAXSPEED 93 + + +void process_servo(); +bool servo_set_speed(uint8_t N, uint16_t s); +bool servo_get_speed(uint8_t N, uint16_t *s); +bool servo_set_tagpos(uint8_t N, uint16_t pos); +bool servo_get_tagpos(uint8_t N, uint16_t *pos); diff --git a/G4:G431/Servo/servo.includes b/G4:G431/Servo/servo.includes new file mode 100644 index 0000000..641fd56 --- /dev/null +++ b/G4:G431/Servo/servo.includes @@ -0,0 +1,4 @@ +. +../inc +../inc/Fx +../inc/cm diff --git a/G4:G431/Servo/strfunc.c b/G4:G431/Servo/strfunc.c new file mode 100644 index 0000000..22c0c06 --- /dev/null +++ b/G4:G431/Servo/strfunc.c @@ -0,0 +1,265 @@ +/* + * This file is part of the encoders project. + * Copyright 2025 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include + +/** + * @brief hexdump - dump hex array by 16 bytes in string + * @param arr - array to dump + * @param len - length of `arr` + */ +void hexdump(int (*sendfun)(const char *), uint8_t *arr, uint16_t len){ + char buf[52], *bptr = buf; + for(uint16_t l = 0; l < len; ++l, ++arr){ + for(int16_t j = 1; j > -1; --j){ + register uint8_t half = (*arr >> (4*j)) & 0x0f; + if(half < 10) *bptr++ = half + '0'; + else *bptr++ = half - 10 + 'a'; + } + if(l % 16 == 15){ + *bptr++ = '\n'; + *bptr = 0; + sendfun(buf); + bptr = buf; + }else *bptr++ = ' '; + } + if(bptr != buf){ + *bptr++ = '\n'; + *bptr = 0; + sendfun(buf); + } +} + +/** + * @brief _2str - convert value into string buffer + * @param val - |value| + * @param minus - ==0 if value > 0 + * @return buffer with number + */ +static char *_2str(uint32_t val, uint8_t minus){ + static char strbuf[12]; + char *bufptr = &strbuf[11]; + *bufptr = 0; + if(!val){ + *(--bufptr) = '0'; + }else{ + while(val){ + uint32_t x = val / 10; + *(--bufptr) = (val - 10*x) + '0'; + val = x; + } + } + if(minus) *(--bufptr) = '-'; + return bufptr; +} + +// return string with number `val` +char *u2str(uint32_t val){ + return _2str(val, 0); +} +char *i2str(int32_t i){ + uint8_t minus = 0; + uint32_t val; + if(i < 0){ + minus = 1; + val = -i; + }else val = i; + return _2str(val, minus); +} + +/** + * @brief uhex2str - print 32bit unsigned int as hex + * @param val - value + * @return string with number + */ +char *uhex2str(uint32_t val){ + static char buf[12] = "0x"; + int npos = 2; + uint8_t *ptr = (uint8_t*)&val + 3; + int8_t i, j, z=1; + for(i = 0; i < 4; ++i, --ptr){ + if(*ptr == 0){ // omit leading zeros + if(i == 3) z = 0; + if(z) continue; + } + else z = 0; + for(j = 1; j > -1; --j){ + uint8_t half = (*ptr >> (4*j)) & 0x0f; + if(half < 10) buf[npos++] = half + '0'; + else buf[npos++] = half - 10 + 'a'; + } + } + buf[npos] = 0; + return buf; +} + +/** + * @brief omit_spaces - eliminate leading spaces and other trash in string + * @param buf - string + * @return - pointer to first character in `buf` > ' ' + */ +char *omit_spaces(char *buf){ + while(*buf){ + if(*buf > ' ') break; + ++buf; + } + return buf; +} + +/** + * @brief getdec - read decimal number & return pointer to next non-number symbol + * @param buf - string + * @param N - number read + * @return Next non-number symbol. In case of overflow return `buf` and N==0xffffffff + */ +static char *getdec(char *buf, uint32_t *N){ + char *start = (char*)buf; + uint32_t num = 0; + while(*buf){ + char c = *buf; + if(c < '0' || c > '9'){ + break; + } + if(num > 429496729 || (num == 429496729 && c > '5')){ // overflow + *N = 0xffffff; + return start; + } + num *= 10; + num += c - '0'; + ++buf; + } + *N = num; + return buf; +} +// read hexadecimal number (without 0x prefix!) +char *gethex(char *buf, uint32_t *N){ + char *start = buf; + uint32_t num = 0; + while(*buf){ + char c = *buf; + uint8_t M = 0; + if(c >= '0' && c <= '9'){ + M = '0'; + }else if(c >= 'A' && c <= 'F'){ + M = 'A' - 10; + }else if(c >= 'a' && c <= 'f'){ + M = 'a' - 10; + } + if(M){ + if(num & 0xf0000000){ // overflow + *N = 0xffffff; + return start; + } + num <<= 4; + num += c - M; + }else{ + break; + } + ++buf; + } + *N = num; + return buf; +} +// read octal number (without 0 prefix!) +static char *getoct(char *buf, uint32_t *N){ + char *start = (char*)buf; + uint32_t num = 0; + while(*buf){ + char c = *buf; + if(c < '0' || c > '7'){ + break; + } + if(num & 0xe0000000){ // overflow + *N = 0xffffff; + return start; + } + num <<= 3; + num += c - '0'; + ++buf; + } + *N = num; + return buf; +} +// read binary number (without b prefix!) +static char *getbin(char *buf, uint32_t *N){ + char *start = (char*)buf; + uint32_t num = 0; + while(*buf){ + char c = *buf; + if(c < '0' || c > '1'){ + break; + } + if(num & 0x80000000){ // overflow + *N = 0xffffff; + return start; + } + num <<= 1; + if(c == '1') num |= 1; + ++buf; + } + *N = num; + return buf; +} + +/** + * @brief getnum - read uint32_t from string (dec, hex or bin: 127, 0x7f, 0b1111111) + * @param buf - buffer with number and so on + * @param N - the number read + * @return pointer to first non-number symbol in buf + * (if it is == buf, there's no number or if *N==0xffffffff there was overflow) + */ +char *getnum(char *txt, uint32_t *N){ + char *nxt = NULL; + char *s = omit_spaces(txt); + if(*s == '0'){ // hex, oct or 0 + if(s[1] == 'x' || s[1] == 'X'){ // hex + nxt = gethex(s+2, N); + if(nxt == s+2) nxt = (char*)txt; + }else if(s[1] > '0'-1 && s[1] < '8'){ // oct + nxt = getoct(s+1, N); + if(nxt == s+1) nxt = (char*)txt; + }else{ // 0 + nxt = s+1; + *N = 0; + } + }else if(*s == 'b' || *s == 'B'){ + nxt = getbin(s+1, N); + if(nxt == s+1) nxt = (char*)txt; + }else{ + nxt = getdec(s, N); + if(nxt == s) nxt = (char*)txt; + } + return nxt; +} + +// get signed integer +char *getint(char *txt, int32_t *I){ + char *s = omit_spaces(txt); + int32_t sign = 1; + uint32_t U; + if(*s == '-'){ + sign = -1; + ++s; + } + char *nxt = getnum(s, &U); + if(nxt == s) return txt; + if(U & 0x80000000) return txt; // overfull + *I = sign * (int32_t)U; + return nxt; +} diff --git a/G4:G431/Servo/strfunc.h b/G4:G431/Servo/strfunc.h new file mode 100644 index 0000000..f08db63 --- /dev/null +++ b/G4:G431/Servo/strfunc.h @@ -0,0 +1,31 @@ +/* + * This file is part of the encoders project. + * Copyright 2025 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include + +void hexdump(int (*sendfun)(const char*), uint8_t *arr, uint16_t len); +char *u2str(uint32_t val); +char *i2str(int32_t i); +char *uhex2str(uint32_t val); +char *gethex(const char *buf, uint32_t *N); +char *getnum(char *txt, uint32_t *N); +char *omit_spaces(char *buf); +char *getint(char *txt, int32_t *I); + diff --git a/G4:G431/Servo/usart.c b/G4:G431/Servo/usart.c new file mode 100644 index 0000000..94a72d8 --- /dev/null +++ b/G4:G431/Servo/usart.c @@ -0,0 +1,257 @@ +/* + * This file is part of the test project. + * Copyright 2026 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include + +#include "hardware.h" // Tms +#include "ringbuffer.h" +#include "usart.h" + +// USART for text-based protocol, each data portion ends with '\n' +// RX works over circular DMA + +// USART-depending part -------> +// USART1 @ PA9 (Tx) - MUX25 - and PA10 (Rx) - MUX24 +// select USART and its DMA channels +#define USARTx USART1 +#define USARTxAPB APB2ENR +#define USARTxEN RCC_APB2ENR_USART1EN +#define USART_APBEN RCC_APB2 +// DMAMUX channels: 24 - USART1Rx, 25 - USART1Tx +#define DMAMUXRXN (24) +#define DMAMUXTXN (25) +// DMA channels: 1 (0 in MUX) - Rx, 2 (1 in MUX) - Tx; TC and error flags +// use DMA ch2/3 because they both have single IRQ +#define DMAx DMA1 +#define DMAxEN (RCC_AHB1ENR_DMA1EN | RCC_AHB1ENR_DMAMUX1EN) +#define DMACHRX DMA1_Channel1 +#define DMARXTCF DMA_ISR_TCIF1 +#define DMARXEF DMA_ISR_TEIF1 +#define DMACHTX DMA1_Channel2 +#define DMATXTCF DMA_ISR_TCIF2 +#define DMATXEF DMA_ISR_TEIF2 +#define DMAMUXRX DMAMUX1_Channel0 +#define DMAMUXTX DMAMUX1_Channel1 +#define USARTIRQn USART1_IRQn +#define DMARXIRQ DMA1_Channel1_IRQn +#define DMATXIRQ DMA1_Channel2_IRQn + +// interrupt aliases +static void usart_isr(); +static void dmatx_isr(); +static void dmarx_isr(); +void usart1_isr() __attribute__ ((alias ("usart_isr"))); +void dma1_channel1_isr() __attribute__ ((alias ("dmarx_isr"))); +void dma1_channel2_isr() __attribute__ ((alias ("dmatx_isr"))); +// <-------- USART-depending part + +// RX/TX DMA->CCR without EN flag +#define DMARXCCR (DMA_CCR_MINC | DMA_CCR_CIRC | DMA_CCR_TEIE) +#define DMATXCCR (DMA_CCR_MINC | DMA_CCR_DIR | DMA_CCR_TCIE | DMA_CCR_TEIE) + +static volatile bool gotstring = true; // got '\n' in input stream -> force data reading to inbuf +static volatile bool txrdy = true; // Tx DMA not busy +static volatile USART_flags_t curflags; // current flags (cleared in `usart_process`) +// rx/tx DMA buffers +static uint8_t dmarxbuf[USARTRXDMABUFSZ]; +static uint8_t dmatxbuf[USARTTXDMABUFSZ]; +// index of last DMA read position +static uint32_t dma_read_idx = 0; +// for ringbuffer +static uint8_t rbrxbuf[USARTRXBUFSZ], rbtxbuf[USARTTXBUFSZ]; +static ringbuffer TxRB = {.data = rbtxbuf, .length = USARTTXBUFSZ}; +static ringbuffer RxRB = {.data = rbrxbuf, .length = USARTRXBUFSZ}; + +#define USART_BRR(speed) ((SysFreq + (speed)/2) / (speed)) + +static void reinit_rx_dma(){ + dma_read_idx = 0; + RB_clearbuf(&RxRB); + DMACHRX->CCR = DMARXCCR; // stop to reload + DMACHRX->CNDTR = USARTRXDMABUFSZ; + DMACHRX->CMAR = (uint32_t) dmarxbuf; + DMACHRX->CCR = DMARXCCR | DMA_CCR_EN; +} + +void usart_setup(uint32_t speed){ + RCC->AHB1ENR |= DMAxEN; // enable DMA + // enable USART clocking + RCC->USARTxAPB |= USARTxEN; + // baudrate + USARTx->BRR = USART_BRR(speed); + // eol character: '/n' + USARTx->CR2 = USART_CR2_ADD_VAL('\n'); + // enable DMA transmission + USARTx->CR3 = USART_CR3_DMAT | USART_CR3_DMAR; + // set up DMA channels + // Tx channel: mem++, mem->periph, 8bit, compl.&err. irq + DMACHTX->CCR = DMATXCCR; + DMACHTX->CPAR = (uint32_t) &USARTx->TDR; // peripherial address + // Rx channel: mem++, periph->mem, 8bit, compl.&err. irq + DMACHRX->CCR = DMARXCCR; + DMACHRX->CPAR = (uint32_t) &USARTx->RDR; // peripherial address + // set up DMAMUX channels + // enumeration of DMAMUX starts from 0 (DMA - from 1)! + DMAMUXRX->CCR = DMAMUXRXN; + DMAMUXTX->CCR = DMAMUXTXN; + // charmatch interrupt, enable transmitter and receiver, enable usart + USARTx->CR1 = USART_CR1_CMIE | USART_CR1_TE | USART_CR1_RE | USART_CR1_UE; + USARTx->ICR = 0xffffffff; // clear all flags + reinit_rx_dma(); + NVIC_EnableIRQ(USARTIRQn); + NVIC_EnableIRQ(DMARXIRQ); + NVIC_EnableIRQ(DMATXIRQ); +} + +/** + * @brief usart_sendbuf - send next data portion + * @return true if sent something + */ +static bool usart_sendbuf(){ + if(!txrdy) return false; + int rd = RB_read(&TxRB, dmatxbuf, USARTTXDMABUFSZ); + if(rd < 1) return false; // nothing to write or busy + // set up DMA + DMACHTX->CCR = DMATXCCR; + DMACHTX->CMAR = (uint32_t) dmatxbuf; + DMACHTX->CNDTR = rd; + USARTx->ICR = USART_ICR_TCCF; // clear TC flag + txrdy = false; + // activate DMA + DMACHTX->CCR = DMATXCCR | DMA_CCR_EN; + return true; +} + +int usart_send(const char *str, int len){ + if(!str || len < 1) return 0; + uint32_t t = Tms; + int sent = 0; + do{ + IWDG->KR = IWDG_REFRESH; + int put = RB_write(&TxRB, (uint8_t*)str, len); + if(put < 0) continue; // busy + else if(put == 0){ + usart_sendbuf(); // no place + t = Tms; + }else{ + len -= put; + sent += put; + str += put; + } + }while(len && (Tms - t) < USARTBLKTMOUT); // not more than `block` ms! + return sent; +} + +int usart_sendstr(const char *str){ + int l = strlen(str); + return usart_send(str, l); +} + +static void addtoreadidx(int adder){ + dma_read_idx += adder; + if(dma_read_idx >= USARTRXDMABUFSZ) dma_read_idx -= USARTRXDMABUFSZ; +} + +// return current flags +USART_flags_t usart_process(){ + static uint32_t Tlast = 0; + USART_flags_t flags = curflags; + curflags.all = 0; + if(RB_datalento(&TxRB, '\n') > 1 || Tms - Tlast >= USARTSENDTMOUT){ // send buffer as we found '\n' or each 10ms + if(usart_sendbuf()) Tlast = Tms; + } + int remained = DMACHRX->CNDTR; + int write_idx = USARTRXDMABUFSZ - remained; // next symbol to be written + int available = (write_idx - dma_read_idx); // length of data available + if(available < 0) available += USARTRXDMABUFSZ; // write to the left of read + if(available == 0) return flags; + // add next data portion to RX ring buffer + if(available >= (USARTRXDMABUFSZ / 2) || gotstring){ + // copy data in one or two chunks (wrap handling) + // check if we can write to RB `available` bytes + int rballow = RxRB.length - 1 - RB_datalen(&RxRB); + if(rballow < available){ + if(available > USARTRXDMABUFSZ - 2){ // near overfull + flags.rxovrfl = 1; + reinit_rx_dma(); + RB_clearbuf(&RxRB); + return flags; + } + if(rballow < 1) return flags; + available = rballow; // read at least as we can + } + if(dma_read_idx + available <= USARTRXDMABUFSZ){ // head before tail + int written = RB_write(&RxRB, &dmarxbuf[dma_read_idx], available); + if(written == available && dmarxbuf[dma_read_idx+available-1] == '\n') gotstring = 0; + if(written > 0) addtoreadidx(written); + }else{ // head after tail - two chunks + int first = USARTRXDMABUFSZ - dma_read_idx; + int written = RB_write(&RxRB, &dmarxbuf[dma_read_idx], first); + if(written != first){ // could write only part - just increase read index + if(written > 0) addtoreadidx(written); + }else{ + dma_read_idx = 0; + int last = available - first; + written = RB_write(&RxRB, dmarxbuf, last); + if(written == last && dmarxbuf[last-1] == '\n') gotstring = 0; + if(written > 0) addtoreadidx(written); + } + } + } + return flags; +} + +char *usart_getline(){ + static char buff[MAX_INPLEN]; + int l = RB_datalento(&RxRB, '\n'); + if(l < 1){ + l = RB_datalen(&RxRB); // Rx ringbuffer could be near overflow but without '\n' + if(l < MAX_INPLEN-1) return NULL; // allow to wait for last symbols + } + if(l > MAX_INPLEN-1){ // overflow -> read at least part of the string + l = MAX_INPLEN-1; + } + if(l != RB_read(&RxRB, (uint8_t*)buff, l)) return NULL; + buff[l] = 0; // return with '\n' at end of line (so user can detect non-finished overflowed lines) + return buff; +} + +// interrupt by '\n' +static void usart_isr(){ + if(USARTx->ISR & USART_ISR_CMF){ // got '\n' @ USARTx + gotstring = true; + } + USARTx->ICR = 0xffffffff; // clear all flags +} + +static void dmarx_isr(){ + volatile uint32_t isr = DMAx->ISR; + if(isr & DMARXEF){ // error + reinit_rx_dma(); + curflags.rxovrfl = 1; + } + DMAx->IFCR = DMARXEF; // clear all flags +} + +static void dmatx_isr(){ + volatile uint32_t isr = DMAx->ISR; + if(isr & DMATXTCF) txrdy = true; + if(isr & DMATXEF) curflags.txerr = 1; + DMAx->IFCR = DMATXTCF | DMATXEF; // clear all flags +} diff --git a/G4:G431/Servo/usart.h b/G4:G431/Servo/usart.h new file mode 100644 index 0000000..2152af0 --- /dev/null +++ b/G4:G431/Servo/usart.h @@ -0,0 +1,49 @@ +/* + * This file is part of the test project. + * Copyright 2026 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include + +// maximal length of input string (including '\n' and terminating zero) +#define MAX_INPLEN (128) +// Rx/Tx ring buffer size +#define USARTTXBUFSZ (512) +#define USARTRXBUFSZ (512) +// Rx/Tx DMA buffer size +#define USARTTXDMABUFSZ (256) +#define USARTRXDMABUFSZ (256) + +// blocking timeout - not more than 5ms +#define USARTBLKTMOUT (5) +// send buffer each 10ms +#define USARTSENDTMOUT (10) + +typedef union{ + struct{ + uint8_t txerr : 1; // transmit error + uint8_t rxovrfl : 1; // receive buffer overflow + }; + uint8_t all; +} USART_flags_t; + +void usart_setup(uint32_t speed); +int usart_send(const char *str, int len); +char *usart_getline(); +int usart_sendstr(const char *str); +USART_flags_t usart_process(); diff --git a/G4:G431/Servo/version.inc b/G4:G431/Servo/version.inc new file mode 100644 index 0000000..03a3241 --- /dev/null +++ b/G4:G431/Servo/version.inc @@ -0,0 +1,2 @@ +#define BUILD_NUMBER "9" +#define BUILD_DATE "2026-07-28"