diff --git a/F3:F303/MLX90640-allsky/Makefile b/F3:F303/MLX90640-allsky/Makefile new file mode 100644 index 0000000..094adbe --- /dev/null +++ b/F3:F303/MLX90640-allsky/Makefile @@ -0,0 +1,10 @@ +BINARY := allsky +# MCU code +MCU := F303xb +# change this linking script depending on particular MCU model, +LDSCRIPT := stm32f303xB.ld +DEFINES := -DUSB1_16 +LDLIBS := -lm + +include ../makefile.f3 +include ../../makefile.stm32 diff --git a/F3:F303/MLX90640-allsky/Readme.md b/F3:F303/MLX90640-allsky/Readme.md new file mode 100644 index 0000000..f05327d --- /dev/null +++ b/F3:F303/MLX90640-allsky/Readme.md @@ -0,0 +1,35 @@ +IR allsky-camera project using 5 sensors MLX90640 +================================================= + +When attached, udev will create symlink /dev/ir-allsky0. This is the udev rule: + +``` + +ACTION=="add", ENV{USB_IDS}=="0483:5740", ATTRS{interface}=="?*", PROGRAM="/bin/bash -c \"ls /dev | grep $attr{interface} | wc -l \"", SYMLINK+="$attr{interface}%c", MODE="0666", GROUP="tty" + +``` + +Protocol: + +``` + +aa - change I2C address to a (a should be non-shifted value!!!) +c - continue MLX +d - draw image in ASCII +i0..4 - setup I2C with speed 10k, 100k, 400k, 1M or 2M (experimental!) +p - pause MLX +r0..3 - change resolution (0 - 16bit, 3 - 19-bit) +t - show temperature map +C - "cartoon" mode on/off (show each new image) +D - dump MLX parameters +G - get MLX state +Ia addr - set device address +Ir reg n - read n words from 16-bit register +Iw words - send words (hex/dec/oct/bin) to I2C +Is - scan I2C bus +T - print current Tms + + +``` + +To call this help just print '?', 'h' or 'H' in terminal. diff --git a/F3:F303/MLX90640-allsky/allsky.bin b/F3:F303/MLX90640-allsky/allsky.bin new file mode 100755 index 0000000..2ad859f Binary files /dev/null and b/F3:F303/MLX90640-allsky/allsky.bin differ diff --git a/F3:F303/MLX90640-allsky/hardware.c b/F3:F303/MLX90640-allsky/hardware.c new file mode 100644 index 0000000..c5623e9 --- /dev/null +++ b/F3:F303/MLX90640-allsky/hardware.c @@ -0,0 +1,34 @@ +/* + * This file is part of the ir-allsky 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 "hardware.h" + +static inline void gpio_setup(){ + RCC->AHBENR |= RCC_AHBENR_GPIOAEN | RCC_AHBENR_GPIOBEN; // for USART and LEDs + for(int i = 0; i < 10000; ++i) nop(); + // USB - alternate function 14 @ pins PA11/PA12; SWD - AF0 @PA13/14 + GPIOA->AFR[1] = AFRf(14, 11) | AFRf(14, 12); + GPIOA->MODER = MODER_AF(11) | MODER_AF(12) | MODER_AF(13) | MODER_AF(14) | MODER_O(15); + GPIOB->MODER = MODER_O(0) | MODER_O(1); + GPIOB->ODR = 1; +} + +void hw_setup(){ + gpio_setup(); +} + diff --git a/F3:F303/MLX90640-allsky/hardware.h b/F3:F303/MLX90640-allsky/hardware.h new file mode 100644 index 0000000..e3a96f8 --- /dev/null +++ b/F3:F303/MLX90640-allsky/hardware.h @@ -0,0 +1,31 @@ +/* + * This file is part of the ir-allsky 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 + +#define USBPU_port GPIOA +#define USBPU_pin (1<<15) +#define USBPU_ON() pin_clear(USBPU_port, USBPU_pin) +#define USBPU_OFF() pin_set(USBPU_port, USBPU_pin) + +extern volatile uint32_t Tms; + +void hw_setup(); + diff --git a/F3:F303/MLX90640-allsky/i2c.c b/F3:F303/MLX90640-allsky/i2c.c new file mode 100644 index 0000000..32a115a --- /dev/null +++ b/F3:F303/MLX90640-allsky/i2c.c @@ -0,0 +1,386 @@ +/* + * This file is part of the i2cscan 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 "i2c.h" +#include "strfunc.h" // hexdump +#include "usb_dev.h" + +i2c_speed_t i2c_curspeed = I2C_SPEED_AMOUNT; +extern volatile uint32_t Tms; +static uint32_t cntr; +volatile uint8_t i2c_scanmode = 0; // == 1 when I2C is in scan mode +static volatile uint8_t i2c_got_DMA = 0; // got DMA data +static uint8_t i2caddr = I2C_ADDREND; // current address in scan mode +static volatile int I2Cbusy = 0, goterr = 0; // busy==1 when DMA active, goterr==1 if 't was error @ last sent +static uint16_t I2Cbuf[I2C_BUFSIZE]; +static uint16_t i2cbuflen = 0; // buffer for DMA rx and its len +static volatile uint16_t dma_remain = 0; // remain bytes of DMA read/write +static uint8_t dmaaddr = 0; // address to continuous read by DMA + +// macros for I2C rx/tx +#define DMARXCCR (DMA_CCR_MINC | DMA_CCR_TCIE | DMA_CCR_TEIE) +#define DMATXCCR (DMA_CCR_MINC | DMA_CCR_DIR | DMA_CCR_TCIE | DMA_CCR_TEIE) +// macro for I2CCR1 +#define I2CCR1 (I2C_CR1_PE | I2C_CR1_RXDMAEN | I2C_CR1_TXDMAEN) + +// return 1 if I2Cbusy is set & timeout reached +static inline int isI2Cbusy(){ + cntr = Tms; + do{ + if(Tms - cntr > I2C_TIMEOUT){ USND("Timeout, DMA transfer in progress?"); return 1;} + }while(I2Cbusy); + return 0; +} + +static void swapbytes(uint16_t *data, uint16_t datalen){ + if(!datalen) return; + for(int i = 0; i < datalen; ++i) + data[i] = __REV16(data[i]); +} + +// GPIO Resources: I2C1_SCL - PB6 (AF4), I2C1_SDA - PB7 (AF4) +void i2c_setup(i2c_speed_t speed){ + uint8_t PRESC, SCLDEL = 0x04, SDADEL = 0x03, SCLH, SCLL; // I2C1->TIMINGR fields + switch(speed){ + case I2C_SPEED_10K: + PRESC = 0x0F; + SCLH = 0xDA; + SCLL = 0xE0; + break; + case I2C_SPEED_100K: + PRESC = 0x0F; + SCLH = 0x13; + SCLL = 0x16; + break; + case I2C_SPEED_400K: + PRESC = 0x07; + SCLH = 0x08; + SCLL = 0x09; + break; + case I2C_SPEED_1M: + SDADEL = 1; + SCLDEL = 2; + PRESC = 0x3; + SCLH = 0x4; + SCLL = 0x6; + break; + case I2C_SPEED_2M: + SDADEL = 0; + SCLDEL = 1; + PRESC = 0x0; + SCLH = 0x1; + SCLL = 0x2; + break; + default: + USND("Wrong I2C speed!"); + return; // wrong speed + } + RCC->AHBENR |= RCC_AHBENR_GPIOBEN; + I2C1->CR1 = 0; // disable I2C for setup + I2C1->ICR = 0x3f38; // clear all errors + GPIOB->AFR[0] = (GPIOB->AFR[0] & ~(GPIO_AFRL_AFRL6 | GPIO_AFRL_AFRL7)) | + AFRf(4, 6) | AFRf(4, 7); + GPIOB->MODER = (GPIOB->MODER & ~(GPIO_MODER_MODER6 | GPIO_MODER_MODER7)) | + MODER_AF(6) | MODER_AF(7); + GPIOB->PUPDR = (GPIOB->PUPDR & !(GPIO_PUPDR_PUPDR6 | GPIO_PUPDR_PUPDR7)) | + PUPD_PU(6) | PUPD_PU(7); // pullup (what if there's no external pullup?) + GPIOB->OTYPER |= OTYPER_OD(6) | OTYPER_OD(7); // both open-drain outputs + GPIOB->OSPEEDR = (GPIOB->OSPEEDR & OSPEED_CLR(6) & OSPEED_CLR(7)) | + OSPEED_HI(6) | OSPEED_HI(7); + // I2C (default timing from sys clock - 72MHz) + RCC->APB1ENR |= RCC_APB1ENR_I2C1EN; // clocking + if(speed < I2C_SPEED_400K){ // slow cpeed - common mode + SYSCFG->CFGR1 &= ~(SYSCFG_CFGR1_I2C1_FMP | SYSCFG_CFGR1_I2C_PB6_FMP | SYSCFG_CFGR1_I2C_PB7_FMP); + }else{ // activate "fast mode plus" + SYSCFG->CFGR1 |= SYSCFG_CFGR1_I2C1_FMP | SYSCFG_CFGR1_I2C_PB6_FMP | SYSCFG_CFGR1_I2C_PB7_FMP; + } + I2C1->TIMINGR = (PRESC<CR1 = I2CCR1; + RCC->AHBENR |= RCC_AHBENR_DMA1EN; + NVIC_EnableIRQ(DMA1_Channel6_IRQn); + NVIC_EnableIRQ(DMA1_Channel7_IRQn); + I2Cbusy = 0; + i2c_curspeed = speed; +} + +// setup DMA for rx (tx==0) or tx (tx==1) +// DMA channels: 7 - I2C1_Rx, 6 - I2C1_Tx +static void i2cDMAsetup(int tx, uint16_t len){ + i2cbuflen = len; + if(len > 255) len = 255; + if(tx){ + DMA1_Channel6->CCR = DMATXCCR; + DMA1_Channel6->CPAR = (uint32_t) &I2C1->TXDR; + DMA1_Channel6->CMAR = (uint32_t) I2Cbuf; + DMA1_Channel6->CNDTR = len; + }else{ + DMA1_Channel7->CCR = DMARXCCR; + DMA1_Channel7->CPAR = (uint32_t) &I2C1->RXDR; + DMA1_Channel7->CMAR = (uint32_t) I2Cbuf; + DMA1_Channel7->CNDTR = len; + } +} + +// wait until bit set or clear; return 1 if OK, 0 in case of timeout +static uint8_t waitISRbit(uint32_t bit, uint8_t isset){ + uint32_t waitwhile = (isset) ? 0 : bit; // wait until != + cntr = Tms; + while((I2C1->ISR & bit) == waitwhile){ + IWDG->KR = IWDG_REFRESH; + if(I2C1->ISR & I2C_ISR_NACKF){ + goto goterr; + } + if(Tms - cntr > I2C_TIMEOUT){ + goto goterr; + } + } + return 1; +goterr: + I2C1->ICR = 0xff; + return 0; +} + +// start writing +static uint8_t i2c_startw(uint8_t addr, uint8_t nbytes, uint8_t stop){ + if(!waitISRbit(I2C_ISR_BUSY, 0)) return 0; + uint32_t cr2 = nbytes << 16 | addr | I2C_CR2_START; + if(stop) cr2 |= I2C_CR2_AUTOEND; + // now start transfer + I2C1->CR2 = cr2; + return 1; +} + +/** + * write command byte to I2C + * @param addr - device address + * @param data - bytes to write + * @param nbytes - amount of bytes to write + * @param stop - to set STOP + * @return 0 if error + */ +static uint8_t i2c_writes(uint8_t addr, uint8_t *data, uint8_t nbytes, uint8_t stop){ + if(!i2c_startw(addr, nbytes, stop)) return 0; + for(int i = 0; i < nbytes; ++i){ + cntr = Tms; + while(!(I2C1->ISR & I2C_ISR_TXIS)){ // ready to transmit + IWDG->KR = IWDG_REFRESH; + if(I2C1->ISR & I2C_ISR_NACKF){ + I2C1->ICR |= I2C_ICR_NACKCF; + return 0; + } + if(Tms - cntr > I2C_TIMEOUT){ + return 0; + } + } + I2C1->TXDR = data[i]; // send data + } + cntr = Tms; + if(stop){ + if(!waitISRbit(I2C_ISR_BUSY, 0)) return 0; + }else{ // repeated start + if(!waitISRbit(I2C_ISR_TC, 1)) return 0; + } + return 1; +} + +uint8_t i2c_write(uint8_t addr, uint16_t *data, uint8_t nwords){ + if(nwords < 1 || nwords > 127) return 0; + if(isI2Cbusy()) return 0; + uint16_t nbytes = nwords << 1; + swapbytes(data, nwords); + return i2c_writes(addr, (uint8_t*)data, nbytes, 1); +} + +uint8_t i2c_write_dma16(uint8_t addr, uint16_t *data, uint8_t nwords){ + if(!data || nwords < 1 || nwords > 127) return 0; + if(isI2Cbusy()) return 0; + uint16_t nbytes = nwords << 1; + swapbytes(data, nwords); + i2cDMAsetup(1, nbytes); + goterr = 0; + if(!i2c_startw(addr, nbytes, 1)) return 0; + I2Cbusy = 1; + DMA1_Channel6->CCR = DMATXCCR | DMA_CCR_EN; // start transfer + return 1; +} + +// start reading of `nbytes` from `addr`; if `start`==`, set START +static uint8_t i2c_startr(uint8_t addr, uint16_t nbytes, uint8_t start){ + uint32_t cr2 = addr | I2C_CR2_RD_WRN; + if(nbytes > 255) cr2 |= I2C_CR2_RELOAD | (0xff0000); + else cr2 |= I2C_CR2_AUTOEND | (nbytes << 16); + I2C1->CR2 = (start) ? cr2 | I2C_CR2_START : cr2; + return 1; +} + +/** + * read nbytes of data from I2C line + * all functions with `addr` should have addr = address << 1 + * `data` should be an array with at least `nbytes` length + * @return 1 if all OK, 0 if NACK or no device found + */ +static uint8_t *i2c_readb(uint8_t addr, uint16_t nbytes){ + uint8_t start = 1; + uint8_t *bptr = (uint8_t*)I2Cbuf; + while(nbytes){ + if(!i2c_startr(addr, nbytes, start)) return NULL; + if(nbytes < 256){ + for(int i = 0; i < nbytes; ++i){ + if(!waitISRbit(I2C_ISR_RXNE, 1)) return NULL; + *bptr++ = I2C1->RXDR; + } + break; + }else while(!(I2C1->ISR & I2C_ISR_TCR)){ // until first part read + if(!waitISRbit(I2C_ISR_RXNE, 1)) return NULL; + *bptr++ = I2C1->RXDR; + } + nbytes -= 255; + start = 0; + } + return (uint8_t*)I2Cbuf; +} + +uint8_t *i2c_read(uint8_t addr, uint16_t nbytes){ + if(isI2Cbusy() || !waitISRbit(I2C_ISR_BUSY, 0) || nbytes < 1 || nbytes > I2C_BUFSIZE*2) return 0; + return i2c_readb(addr, nbytes); +} + +static uint8_t dmard(uint8_t addr, uint16_t nbytes){ + if(nbytes < 1 || nbytes > I2C_BUFSIZE*2) return 0; + i2cDMAsetup(0, nbytes); + goterr = 0; + i2c_got_DMA = 0; + (void) I2C1->RXDR; // avoid wrong first byte + DMA1_Channel7->CCR = DMARXCCR | DMA_CCR_EN; // init DMA before START sequence + if(!i2c_startr(addr, nbytes, 1)) return 0; + dmaaddr = addr; + dma_remain = nbytes > 255 ? nbytes - 255 : 0; // remainder after first read finish + I2Cbusy = 1; + return 1; +} + +uint8_t i2c_read_dma16(uint8_t addr, uint16_t nwords){ + if(nwords > I2C_BUFSIZE) return 0; // what if `nwords` is very large? we should check it + if(isI2Cbusy() || !waitISRbit(I2C_ISR_BUSY, 0)) return 0; + return dmard(addr, nwords<<1); +} + +// read 16bit register reg +uint16_t *i2c_read_reg16(uint8_t addr, uint16_t reg16, uint16_t nwords, uint8_t isdma){ + if(isI2Cbusy() || !waitISRbit(I2C_ISR_BUSY, 0) || nwords < 1 || nwords > I2C_BUFSIZE) return 0; + reg16 = __REV16(reg16); + if(!i2c_writes(addr, (uint8_t*)®16, 2, 0)) return NULL; + if(isdma){ + if(dmard(addr, nwords<<1)) return I2Cbuf; + return NULL; + } + if(!i2c_readb(addr, nwords<<1)) return NULL; + swapbytes((uint16_t*)I2Cbuf, nwords); + return (uint16_t*)I2Cbuf; +} + +void i2c_init_scan_mode(){ + i2caddr = 1; // start from 1 as 0 is a broadcast address + i2c_scanmode = 1; +} + +// return 1 if next addr is active & return in as `addr` +// if addresses are over, return 1 and set addr to I2C_NOADDR +// if scan mode inactive, return 0 and set addr to I2C_NOADDR +int i2c_scan_next_addr(uint8_t *addr){ + if(isI2Cbusy()) return 0; + *addr = i2caddr; + if(i2caddr == I2C_ADDREND){ + *addr = I2C_ADDREND; + i2c_scanmode = 0; + return 0; + } + if(!i2c_read((i2caddr++)<<1, 1)) return 0; + return 1; +} + +// dump I2Cbuf +void i2c_bufdudump(){ + if(goterr){ + USND("DMARDERR"); + goterr = 0; + } + if(i2cbuflen < 1) return; + USND("DMARD="); + hexdump16(USB_sendstr, (uint16_t*)I2Cbuf, i2cbuflen); +} + +// get DMA buffer with conversion to little-endian (if transfer was for 16-bit) +uint16_t *i2c_dma_getbuf(uint16_t *len){ + //if(i2c_got_DMA) USND("DMA GOT!"); + //U("T="); U(u2str(Tms)); U("; cndtr: "); USND(u2str(DMA1_Channel7->CNDTR)); + if(!i2c_got_DMA || i2cbuflen < 1) return NULL; + i2c_got_DMA = 0; + i2cbuflen >>= 1; // for hexdump16 - now buffer have uint16_t! + swapbytes((uint16_t*)I2Cbuf, i2cbuflen); + if(len) *len = i2cbuflen; + return I2Cbuf; +} + +int i2c_dma_haderr(){ + int r = goterr; + goterr = 0; + return r; +} + +int i2c_busy(){ return I2Cbusy;} + +// Rx (7) /Tx (6) interrupts +static void I2C_isr(int rx){ + uint32_t isr = DMA1->ISR; + DMA_Channel_TypeDef *ch = (rx) ? DMA1_Channel7 : DMA1_Channel6; + ch->CCR &= ~DMA_CCR_EN; // clear enable for further settings + if(isr & (DMA_ISR_TEIF6 | DMA_ISR_TEIF7)){ + goterr = 1; goto ret; + } + if(dma_remain){ // receive/send next portion + uint16_t len = (dma_remain > 255) ? 255 : dma_remain; + ch->CNDTR = len; + if(rx){ + if(!i2c_startr(dmaaddr, dma_remain, 0)){ + goterr = 1; goto ret; + } + ch->CMAR += 255; + } + dma_remain -= len; + ch->CCR |= DMA_CCR_EN; + DMA1->IFCR = DMA_IFCR_CTCIF6 | DMA_IFCR_CTCIF7; + return; + }else if(rx) i2c_got_DMA = 1; // last transfer was Rx and all data read +ret: + ch->CCR = 0; + I2Cbusy = 0; + DMA1->IFCR = 0x0ff00000; // clear all flags for channel6/7 +} + +void dma1_channel6_isr(){ + I2C_isr(0); +} + +void dma1_channel7_isr(){ + I2C_isr(1); +} diff --git a/F3:F303/MLX90640-allsky/i2c.h b/F3:F303/MLX90640-allsky/i2c.h new file mode 100644 index 0000000..f525880 --- /dev/null +++ b/F3:F303/MLX90640-allsky/i2c.h @@ -0,0 +1,57 @@ +/* + * This file is part of the i2cscan 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 . + */ + +#pragma once +#include + +#define I2C_ADDREND (0x80) +// size in words +#define I2C_BUFSIZE (1024) + +typedef enum{ + I2C_SPEED_10K, + I2C_SPEED_100K, + I2C_SPEED_400K, + I2C_SPEED_1M, + I2C_SPEED_2M, // EXPERIMENTAL! Could be unstable!!! (speed near 1.9Mbaud) + I2C_SPEED_AMOUNT +} i2c_speed_t; + +extern i2c_speed_t i2c_curspeed; +extern volatile uint8_t i2c_scanmode; + +// timeout of I2C bus in ms +#define I2C_TIMEOUT (5) + +void i2c_setup(i2c_speed_t speed); +int i2c_busy(); + +uint8_t *i2c_read(uint8_t addr, uint16_t nbytes); +uint8_t i2c_read_dma16(uint8_t addr, uint16_t nwords); +uint16_t *i2c_read_reg16(uint8_t addr, uint16_t reg16, uint16_t nwords, uint8_t isdma); + +uint8_t i2c_write(uint8_t addr, uint16_t *data, uint8_t nwords); +uint8_t i2c_write_dma16(uint8_t addr, uint16_t *data, uint8_t nwords); + +void i2c_bufdudump(); +int i2c_dma_haderr(); +uint16_t *i2c_dma_getbuf(uint16_t *len); +int i2c_getwords(uint16_t *buf, int bufsz); + +void i2c_init_scan_mode(); +int i2c_scan_next_addr(uint8_t *addr); diff --git a/F3:F303/MLX90640-allsky/ir-allsky.cflags b/F3:F303/MLX90640-allsky/ir-allsky.cflags new file mode 100644 index 0000000..68d5165 --- /dev/null +++ b/F3:F303/MLX90640-allsky/ir-allsky.cflags @@ -0,0 +1 @@ +-std=c17 \ No newline at end of file diff --git a/F3:F303/MLX90640-allsky/ir-allsky.config b/F3:F303/MLX90640-allsky/ir-allsky.config new file mode 100644 index 0000000..1cf1964 --- /dev/null +++ b/F3:F303/MLX90640-allsky/ir-allsky.config @@ -0,0 +1,7 @@ +// Add predefined macros for your project here. For example: +// #define THE_ANSWER 42 +#define EBUG +#define STM32F3 +#define STM32F303xb +#define __thumb2__ 1 +#define __ARM_ARCH_7M__ diff --git a/F3:F303/MLX90640-allsky/ir-allsky.creator b/F3:F303/MLX90640-allsky/ir-allsky.creator new file mode 100644 index 0000000..e94cbbd --- /dev/null +++ b/F3:F303/MLX90640-allsky/ir-allsky.creator @@ -0,0 +1 @@ +[General] diff --git a/F3:F303/MLX90640-allsky/ir-allsky.creator.user b/F3:F303/MLX90640-allsky/ir-allsky.creator.user new file mode 100644 index 0000000..65f6698 --- /dev/null +++ b/F3:F303/MLX90640-allsky/ir-allsky.creator.user @@ -0,0 +1,215 @@ + + + + + + EnvironmentId + {7bd84e39-ca37-46d3-be9d-99ebea85bc0d} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + true + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + KOI8-R + false + 4 + false + 0 + 80 + true + true + 1 + 0 + false + true + false + 0 + true + true + 0 + 8 + true + false + 1 + true + false + true + *.md, *.MD, Makefile + false + true + true + + + + ProjectExplorer.Project.PluginSettings + + + true + false + true + true + true + true + + false + + + 0 + true + + true + true + Builtin.DefaultTidyAndClazy + 2 + false + + + + true + + + + + ProjectExplorer.Project.Target.0 + + Desktop + true + Desktop + Desktop + {65a14f9e-e008-4c1b-89df-4eaa4774b6e3} + 0 + 0 + 0 + + /Big/Data/00__Electronics/STM32/F303-nolib/blink + + + + all + + true + GenericProjectManager.GenericMakeStep + + 1 + Build + Build + ProjectExplorer.BuildSteps.Build + + + + + clean + + true + GenericProjectManager.GenericMakeStep + + 1 + Clean + Clean + ProjectExplorer.BuildSteps.Clean + + 2 + false + + false + + Default + GenericProjectManager.GenericBuildConfiguration + 0 + 0 + + + 0 + Deploy + Deploy + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + true + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + true + true + + 1 + + 1 + + + 0 + Deploy + Deploy + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + true + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + true + true + + 1 + + + + ProjectExplorer.Project.TargetCount + 1 + + + ProjectExplorer.Project.Updater.FileVersion + 22 + + + Version + 22 + + diff --git a/F3:F303/MLX90640-allsky/ir-allsky.cxxflags b/F3:F303/MLX90640-allsky/ir-allsky.cxxflags new file mode 100644 index 0000000..6435dfc --- /dev/null +++ b/F3:F303/MLX90640-allsky/ir-allsky.cxxflags @@ -0,0 +1 @@ +-std=c++17 \ No newline at end of file diff --git a/F3:F303/MLX90640-allsky/ir-allsky.files b/F3:F303/MLX90640-allsky/ir-allsky.files new file mode 100644 index 0000000..522e29b --- /dev/null +++ b/F3:F303/MLX90640-allsky/ir-allsky.files @@ -0,0 +1,30 @@ +hardware.c +hardware.h +i2c.c +i2c.h +main.c +ir-allsky.c +ir-allsky.h +ir-allsky_regs.h +mlx90640.c +mlx90640.h +mlx90640_regs.h +mlxproc.c +mlxproc.h +proto.c +proto.h +ringbuffer.c +ringbuffer.h +strfunc.c +strfunc.h +usart.c +usart.h +usb.c +usb.h +usb_descr.c +usb_descr.h +usb_dev.c +usb_dev.h +usb_lib.c +usb_lib.h +usbhw.h diff --git a/F3:F303/MLX90640-allsky/ir-allsky.includes b/F3:F303/MLX90640-allsky/ir-allsky.includes new file mode 100644 index 0000000..06d1130 --- /dev/null +++ b/F3:F303/MLX90640-allsky/ir-allsky.includes @@ -0,0 +1,6 @@ +. +../inc +../inc/Fx +../inc/cm +../inc/ld +../inc/startup diff --git a/F3:F303/MLX90640-allsky/main.c b/F3:F303/MLX90640-allsky/main.c new file mode 100644 index 0000000..e4fc63c --- /dev/null +++ b/F3:F303/MLX90640-allsky/main.c @@ -0,0 +1,90 @@ +/* + * This file is part of the ir-allsky 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 "hardware.h" +#include "i2c.h" +#include "mlxproc.h" +#include "proto.h" +#include "strfunc.h" +#include "usart.h" +#include "usb_dev.h" + +#define MAXSTRLEN RBINSZ + +volatile uint32_t Tms = 0; + +void sys_tick_handler(void){ + ++Tms; +} + +int main(void){ + char inbuff[MAXSTRLEN+1]; + if(StartHSE()){ + SysTick_Config((uint32_t)72000); // 1ms + }else{ + StartHSI(); + SysTick_Config((uint32_t)48000); // 1ms + } + USBPU_OFF(); + hw_setup(); + i2c_setup(I2C_SPEED_400K); + USB_setup(); + usart_setup(115200); + USBPU_ON(); + uint32_t ctr = Tms, Tlastima[N_SESORS] = {0}; + mlx_continue(); // init state machine + while(1){ + IWDG->KR = IWDG_REFRESH; + if(Tms - ctr > 499){ + ctr = Tms; + if(!mlx_nactive()){ mlx_stop(); mlx_continue(); } + pin_toggle(GPIOB, 1 << 1 | 1 << 0); // toggle LED @ PB0 + } + int l = USB_receivestr(inbuff, MAXSTRLEN); + if(l < 0) USB_sendstr("USBOVERFLOW\n"); + else if(l){ + const char *ans = parse_cmd(inbuff); + if(ans) USB_sendstr(ans); + } + if(i2c_scanmode){ + uint8_t addr; + int ok = i2c_scan_next_addr(&addr); + if(addr == I2C_ADDREND) USND("SCANEND"); + else if(ok){ + U("FOUNDID="); + USND(uhex2str(addr)); + } + } + mlx_process(); + if(cartoon) for(int i = 0; i < N_SESORS; ++i){ + uint32_t Tnow = mlx_lastimT(i); + if(Tnow != Tlastima[i]){ + fp_t *im = mlx_getimage(i); + if(im){ + U(Sensno); USND(i2str(i)); + U(Timage); USND(u2str(Tnow)); drawIma(im); + Tlastima[i] = Tnow; + } + } + } + usart_process(); + if(usart_ovr()) USND("USART_OVERFLOW\n"); + char *got = usart_getline(NULL); + if(got){ U("USART='"); U(got); USND("'"); } + } +} diff --git a/F3:F303/MLX90640-allsky/mlx90640.c b/F3:F303/MLX90640-allsky/mlx90640.c new file mode 100644 index 0000000..7b974af --- /dev/null +++ b/F3:F303/MLX90640-allsky/mlx90640.c @@ -0,0 +1,340 @@ +/* + * This file is part of the ir-allsky 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 +#include + +#include "strfunc.h" + +#include "mlx90640.h" +#include "mlx90640_regs.h" +#include "mlxproc.h" + +// tolerance of floating point comparison +#define FP_TOLERANCE (1e-3) + +// 3072 bytes +static fp_t mlx_image[MLX_PIXNO] = {0}; // ready image +// 10100 bytes: +static MLX90640_params params; // calculated parameters (in heap, not stack!) for other functions + +void dumpIma(const fp_t im[MLX_PIXNO]){ + for(int row = 0; row < MLX_H; ++row){ + for(int col = 0; col < MLX_W; ++col){ + printfl(*im++, 1); + USB_putbyte(' '); + } + newline(); + } +} + +#define GRAY_LEVELS (16) +// 16-level character set ordered by fill percentage (provided by user) +static const char* CHARS_16 = " .':;+*oxX#&%B$@"; +void drawIma(const fp_t im[MLX_PIXNO]){ + // Find min and max values + fp_t min_val = im[0], max_val = im[0]; + const fp_t *iptr = im; + for(int row = 0; row < MLX_H; ++row){ + for(int col = 0; col < MLX_W; ++col){ + fp_t cur = *iptr++; + if(cur < min_val) min_val = cur; + else if(cur > max_val) max_val = cur; + } + } + fp_t range = max_val - min_val; + U("RANGE="); USND(float2str(range, 3)); + U("MIN="); USND(float2str(min_val, 3)); + U("MAX="); USND(float2str(max_val, 3)); + if(fabsf(range) < 0.001) range = 1.; // solid fill -> blank + // Generate and print ASCII art + iptr = im; + for(int row = 0; row < MLX_H; ++row){ + for(int col = 0; col < MLX_W; ++col){ + fp_t normalized = ((*iptr++) - min_val) / range; + // Map to character index (0 to 15) + int index = (int)(normalized * GRAY_LEVELS); + // Ensure we stay within bounds + if(index < 0) index = 0; + else if(index > (GRAY_LEVELS-1)) index = (GRAY_LEVELS-1); + USB_putbyte(CHARS_16[index]); + } + newline(); + } + newline(); +} + +/***************************************************************************** + Calculate parameters & values + *****************************************************************************/ + +// fill OCC/ACC row/col arrays +static void occacc(int8_t *arr, int l, const uint16_t *regstart){ + int n = l >> 2; // divide by 4 + int8_t *p = arr; + for(int i = 0; i < n; ++i){ + register uint16_t val = *regstart++; + *p++ = (val & 0x000F) >> 0; + *p++ = (val & 0x00F0) >> 4; + *p++ = (val & 0x0F00) >> 8; + *p++ = (val ) >> 12; + } + for(int i = 0; i < l; ++i, ++arr){ + if(*arr > 0x07) *arr -= 0x10; + } +} + +// get all parameters' values from `dataarray`, return FALSE if something failed +MLX90640_params *get_parameters(const uint16_t dataarray[MLX_DMA_MAXLEN]){ + #define CREG_VAL(reg) dataarray[CREG_IDX(reg)] + int8_t i8; + int16_t i16; + uint16_t *pu16; + uint16_t val = CREG_VAL(REG_VDD); + i8 = (int8_t) (val >> 8); + params.kVdd = i8 * 32; // keep sign + if(params.kVdd == 0){USND("kvdd=0"); return NULL;} + i16 = val & 0xFF; + params.vdd25 = ((i16 - 0x100) * 32) - (1<<13); + val = CREG_VAL(REG_KVTPTAT); + i16 = (val & 0xFC00) >> 10; + if(i16 > 0x1F) i16 -= 0x40; + params.KvPTAT = (fp_t)i16 / (1<<12); + i16 = (val & 0x03FF); + if(i16 > 0x1FF) i16 -= 0x400; + params.KtPTAT = (fp_t)i16 / 8.; + params.vPTAT25 = (int16_t) CREG_VAL(REG_PTAT); + val = CREG_VAL(REG_APTATOCCS) >> 12; + params.alphaPTAT = val / 4. + 8.; + params.gainEE = (int16_t)CREG_VAL(REG_GAIN); + if(params.gainEE == 0){USND("gainee=0"); return NULL;} + int8_t occRow[MLX_H]; + int8_t occColumn[MLX_W]; + occacc(occRow, MLX_H, &CREG_VAL(REG_OCCROW14)); + occacc(occColumn, MLX_W, &CREG_VAL(REG_OCCCOL14)); + int8_t accRow[MLX_H]; + int8_t accColumn[MLX_W]; + occacc(accRow, MLX_H, &CREG_VAL(REG_ACCROW14)); + occacc(accColumn, MLX_W, &CREG_VAL(REG_ACCCOL14)); + val = CREG_VAL(REG_APTATOCCS); + // need to do multiplication instead of bitshift, so: + fp_t occRemScale = 1<<(val&0x0F), + occColumnScale = 1<<((val>>4)&0x0F), + occRowScale = 1<<((val>>8)&0x0F); + int16_t offavg = (int16_t) CREG_VAL(REG_OSAVG); + // even/odd column/row numbers are for starting from 1, so for starting from 0 we should swap them: + // even - for 1,3,5,...; odd - for 0,2,4,... etc + int8_t ktaavg[4]; + // 0 - odd row, odd col; 1 - odd row even col; 2 - even row, odd col; 3 - even row, even col + val = CREG_VAL(REG_KTAAVGODDCOL); + ktaavg[2] = (int8_t)(val & 0xFF); // odd col (1,3,..), even row (2,4,..) -> col 0,2,..; row 1,3,.. + ktaavg[0] = (int8_t)(val >> 8); // odd col, odd row -> col 0,2,..; row 0,2,.. + val = CREG_VAL(REG_KTAAVGEVENCOL); + ktaavg[3] = (int8_t)(val & 0xFF); // even col, even row -> col 1,3,..; row 1,3,.. + ktaavg[1] = (int8_t)(val >> 8); // even col, odd row -> col 1,3,..; row 0,2,.. + // so index of ktaavg is 2*(row&1)+(col&1) + val = CREG_VAL(REG_KTAVSCALE); + uint8_t scale1 = ((val & 0xFF)>>4) + 8, scale2 = (val&0xF); + if(scale1 == 0 || scale2 == 0){USND("scale1/2=0"); return NULL;} + fp_t mul = (fp_t)(1<> 12); + fp_t diva = (fp_t)(diva32); + diva *= (fp_t)(1<<30); // alpha_scale + fp_t accRowScale = 1<<((val & 0x0f00)>>8), + accColumnScale = 1<<((val & 0x00f0)>>4), + accRemScale = 1<<(val & 0x0f); + pu16 = (uint16_t*)&CREG_VAL(REG_OFFAK1); + fp_t *kta = params.kta, *offset = params.offset; + //uint8_t *ol = params.outliers; + for(int row = 0; row < MLX_H; ++row){ + int idx = (row&1)<<1; + for(int col = 0; col < MLX_W; ++col){ + // offset + register uint16_t rv = *pu16++; + i16 = (rv & 0xFC00) >> 10; + if(i16 > 0x1F) i16 -= 0x40; + *offset++ = (fp_t)offavg + (fp_t)occRow[row]*occRowScale + (fp_t)occColumn[col]*occColumnScale + (fp_t)i16*occRemScale; + // kta + i16 = (rv & 0xF) >> 1; + if(i16 > 0x03) i16 -= 0x08; + *kta++ = (ktaavg[idx|(col&1)] + i16*mul) / div; + // alpha + i16 = (rv & 0x3F0) >> 4; + if(i16 > 0x1F) i16 -= 0x40; + fp_t oft = (fp_t)a_r + accRow[row]*accRowScale + accColumn[col]*accColumnScale +i16*accRemScale; + *a++ = oft / diva; + //*ol++ = (rv&1) ? 1 : 0; + } + } + scale1 = (CREG_VAL(REG_KTAVSCALE) >> 8) & 0xF; // kvscale + div = (fp_t)(1<> 12; if(i16 > 0x07) i16 -= 0x10; + ktaavg[0] = (int8_t)i16; // odd col, odd row + i16 = (val & 0xF0) >> 4; if(i16 > 0x07) i16 -= 0x10; + ktaavg[1] = (int8_t)i16; // even col, odd row + i16 = (val & 0x0F00) >> 8; if(i16 > 0x07) i16 -= 0x10; + ktaavg[2] = (int8_t)i16; // odd col, even row + i16 = val & 0x0F; if(i16 > 0x07) i16 -= 0x10; + ktaavg[3] = (int8_t)i16; // even col, even row + for(int i = 0; i < 4; ++i) params.kv[i] = ktaavg[i] / div; + val = CREG_VAL(REG_CPOFF); + params.cpOffset[0] = (val & 0x03ff); + if(params.cpOffset[0] > 0x1ff) params.cpOffset[0] -= 0x400; + params.cpOffset[1] = val >> 10; + if(params.cpOffset[1] > 0x1f) params.cpOffset[1] -= 0x40; + params.cpOffset[1] += params.cpOffset[0]; + val = ((CREG_VAL(REG_KTAVSCALE) & 0xF0) >> 4) + 8; + i8 = (int8_t)(CREG_VAL(REG_KVTACP) & 0xFF); + params.cpKta = (fp_t)i8 / (1<> 8; + i16 = CREG_VAL(REG_KVTACP) >> 8; + if(i16 > 0x7F) i16 -= 0x100; + params.cpKv = (fp_t)i16 / (1< 0x7F) i16 -= 0x100; + params.tgc = (fp_t)i16; + params.tgc /= 32.; + val = (CREG_VAL(REG_SCALEACC)>>12); // alpha_scale_CP + i16 = CREG_VAL(REG_ALPHA)>>10; // cp_P1_P0_ratio + if(i16 > 0x1F) i16 -= 0x40; + div = (fp_t)(1<> 8); + params.KsTa = (fp_t)i8/(1<<13); + div = 1<<((CREG_VAL(REG_CT34) & 0x0F) + 8); // kstoscale + val = CREG_VAL(REG_KSTO12); + i8 = (int8_t)(val & 0xFF); + params.KsTo[0] = i8 / div; + i8 = (int8_t)(val >> 8); + params.KsTo[1] = i8 / div; + val = CREG_VAL(REG_KSTO34); + i8 = (int8_t)(val & 0xFF); + params.KsTo[2] = i8 / div; + i8 = (int8_t)(val >> 8); + params.KsTo[3] = i8 / div; + // CT1 = -40, CT2 = 0 -> start from zero index, so CT[0] is CT2, CT[1] is CT3, CT[2] is CT4 + params.CT[0] = 0.; // 0degr - between ranges 1 and 2 + val = CREG_VAL(REG_CT34); + mul = ((val & 0x3000)>>12)*10.; // step + params.CT[1] = ((val & 0xF0)>>4)*mul; // CT3 - between ranges 2 and 3 + params.CT[2] = ((val & 0x0F00) >> 8)*mul + params.CT[1]; // CT4 - between ranges 3 and 4 + // alphacorr for each range: 11.1.11 + params.alphacorr[0] = 1./(1. + params.KsTo[0] * 40.); + params.alphacorr[1] = 1.; + params.alphacorr[2] = (1. + params.KsTo[1] * params.CT[1]); + params.alphacorr[3] = (1. + params.KsTo[2] * (params.CT[2] - params.CT[1])) * params.alphacorr[2]; + params.resolEE = (uint8_t)((CREG_VAL(REG_KTAVSCALE) & 0x3000) >> 12); + // Don't forget to check 'outlier' flags for wide purpose + return ¶ms; +#undef CREG_VAL +} + +/** + * @brief process_image - process both subpages (image data of sp0 lays in sp1, service data is in spare array) + * @param params + * @param subpages + * @param Service0 + * @param subpageno + * @return + */ +fp_t *process_image(const int16_t subpage1[REG_IMAGEDATA_LEN]){ +#define IMD_VAL(reg) subpage1[IMD_IDX(reg)] + // 11.2.2.1. Resolution restore + //fp_t resol_corr = (fp_t)(1<. + * + * 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 + +#define REG_STATUS 0x8000 +#define REG_STATUS_OVWEN (1<<4) +#define REG_STATUS_NEWDATA (1<<3) +#define REG_STATUS_SPNO (1<<0) +#define REG_STATUS_SPMASK (3<<0) +#define REG_CONTROL 0x800D +#define REG_CONTROL_CHESS (1<<12) +#define REG_CONTROL_RES16 (0<<10) +#define REG_CONTROL_RES17 (1<<10) +#define REG_CONTROL_RES18 (2<<10) +#define REG_CONTROL_RES19 (3<<10) +#define REG_CONTROL_RESMASK (3<<10) +#define REG_CONTROL_REFR_05HZ (0<<7) +#define REG_CONTROL_REFR_1HZ (1<<7) +#define REG_CONTROL_REFR_2HZ (2<<7) +#define REG_CONTROL_REFR_4HZ (3<<7) +#define REG_CONTROL_REFR_8HZ (4<<7) +#define REG_CONTROL_REFR_16HZ (5<<7) +#define REG_CONTROL_REFR_32HZ (6<<7) +#define REG_CONTROL_REFR_64HZ (7<<7) +#define REG_CONTROL_SUBP1 (1<<4) +#define REG_CONTROL_SUBPMASK (3<<4) +#define REG_CONTROL_SUBPSEL (1<<3) +#define REG_CONTROL_DATAHOLD (1<<2) +#define REG_CONTROL_SUBPEN (1<<0) +#define REG_MLXADDR_MASK (0xff) + +// default value +#define REG_CONTROL_DEFAULT (REG_CONTROL_CHESS|REG_CONTROL_RES18|REG_CONTROL_REFR_2HZ|REG_CONTROL_SUBPEN) + +// calibration data start & len +#define REG_CALIDATA 0x2400 +#define REG_CALIDATA_LEN 832 + +// address in EEPROM (writing to 0x8010 will only change address in RAM) +#define REG_MLXADDR 0x240f + +#define REG_APTATOCCS 0x2410 +#define REG_OSAVG 0x2411 +#define REG_OCCROW14 0x2412 +#define REG_OCCCOL14 0x2418 +#define REG_SCALEACC 0x2420 +#define REG_SENSIVITY 0x2421 +#define REG_ACCROW14 0x2422 +#define REG_ACCCOL14 0x2428 +#define REG_GAIN 0x2430 +#define REG_PTAT 0x2431 +#define REG_KVTPTAT 0x2432 +#define REG_VDD 0x2433 +#define REG_KVAVG 0x2434 +#define REG_ILCHESS 0x2435 +#define REG_KTAAVGODDCOL 0x2436 +#define REG_KTAAVGEVENCOL 0x2437 +#define REG_KTAVSCALE 0x2438 +#define REG_ALPHA 0x2439 +#define REG_CPOFF 0x243A +#define REG_KVTACP 0x243B +#define REG_KSTATGC 0x243C +#define REG_KSTO12 0x243D +#define REG_KSTO34 0x243E +#define REG_CT34 0x243F +#define REG_OFFAK1 0x2440 +// index of register in array (from REG_CALIDATA) +#define CREG_IDX(addr) ((addr)-REG_CALIDATA) + +// full amount of IMAGE (or calibration) data + EXTRA data (counts of uint16_t!) +#define MLX_DMA_MAXLEN 834 + +// RAM register of image data +#define REG_IMAGEDATA 0x0400 +#define REG_IMAGEDATA_LEN 832 +// RAM register of service data +#define REG_SERVICE 0x0700 +#define REG_SERVICE_LEN 64 +#define REG_ITAVBE 0x0700 +#define REG_ICPSP0 0x0708 +#define REG_IGAIN 0x070A +#define REG_ITAPTAT 0x0720 +#define REG_ICPSP1 0x0728 +#define REG_IVDDPIX 0x072A +// index of register in array (from REG_IMAGEDATA) +#define IMD_IDX(addr) ((addr)-REG_IMAGEDATA) +// and for subpage 0 - only service data +#define SERVICE_IDX(addr) ((addr)-REG_SERVICE) diff --git a/F3:F303/MLX90640-allsky/mlxproc.c b/F3:F303/MLX90640-allsky/mlxproc.c new file mode 100644 index 0000000..d44ce5c --- /dev/null +++ b/F3:F303/MLX90640-allsky/mlxproc.c @@ -0,0 +1,270 @@ +/* + * This file is part of the ir-allsky 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 "i2c.h" +#include "mlxproc.h" +#include "mlx90640_regs.h" + +//#define DEBUGPROC + +#ifdef DEBUGPROC +#include "usb_dev.h" +#include "strfunc.h" +#define D(x) U(x) +#define DN(x) USND(x) +#define DB(x) USB_putbute(x) +#else +#define D(x) +#define DN(x) +#define DB(x) +#endif + +extern volatile uint32_t Tms; + +// current state and state before `stop` called +static mlx_state_t MLX_state = MLX_RELAX, MLX_oldstate = MLX_RELAX; +static int errctr = 0; // errors counter - cleared by mlx_continue +static uint32_t Tlastimage[N_SESORS] = {0}; + +// subpages and configs of all sensors +// 8320 bytes: +static int16_t imdata[N_SESORS][REG_IMAGEDATA_LEN]; +// 8340 bytes: +static uint16_t confdata[N_SESORS][MLX_DMA_MAXLEN]; +static uint8_t sens_addresses[N_SESORS] = {0x10<<1, 0x11<<1, 0x12<<1, 0x13<<1, 0x14<<1}; // addresses of all sensors (if 0 - omit this one) +static uint8_t sensaddr[N_SESORS]; + +// get compile-time size: (gcc shows it in error message) +//char (*__kaboom)[sizeof( confdata )] = 1; + +// return `sensaddr` +uint8_t *mlx_activeids(){return sensaddr;} + +static int sensno = -1; + +// get current state +mlx_state_t mlx_state(){ return MLX_state; } +// set address +int mlx_setaddr(int n, uint8_t addr){ + if(n < 0 || n > N_SESORS) return 0; + if(addr > 0x7f) return 0; + sens_addresses[n] = addr << 1; + Tlastimage[n] = Tms; // refresh counter for autoreset I2C in case of error + return 1; +} +// pause state machine and stop +void mlx_pause(){ + MLX_oldstate = MLX_state; + MLX_state = MLX_RELAX; +} +// TODO: add here power management +void mlx_stop(){ + MLX_oldstate = MLX_NOTINIT; + MLX_state = MLX_RELAX; +} + +// continue processing +// TODO: add here power management +void mlx_continue(){ + errctr = 0; + switch(MLX_oldstate){ + case MLX_WAITSUBPAGE: + case MLX_READSUBPAGE: + MLX_state = MLX_WAITSUBPAGE; + break; + //case MLX_NOTINIT: + //case MLX_WAITPARAMS: + default: + i2c_setup(i2c_curspeed); // restart I2C (what if there was errors?) + memcpy(sensaddr, sens_addresses, sizeof(sens_addresses)); + MLX_state = MLX_NOTINIT; + sensno = -1; + break; + } +} + +static int nextsensno(int s){ + if(mlx_nactive() == 0){ + mlx_stop(); + return -1; + } + int next = s + 1; + for(; next < N_SESORS; ++next) if(sensaddr[next]) break; + if(next == N_SESORS) return nextsensno(-1); // roll to start + D(i2str(next)); DB('('); D(i2str(s)); DB(')'); DN(" - new sensor number"); + return next; +} + +// count active sensors +int mlx_nactive(){ + int N = 0; + for(int i = 0; i < N_SESORS; ++i) if(sensaddr[i]) ++N; + return N; +} + +/** + * @brief mlx_process - main state machine + * 1. Process conf data for each sensor + * 2. Start image processing and store subpage1 for each sensor + */ +void mlx_process(){ + static uint32_t TT = 0; + if(Tms == TT) return; + TT = Tms; + // static uint32_t Tlast = 0; + static int subpage = 0; + if(MLX_state == MLX_RELAX) return; + if(sensno == -1){ // init + sensno = nextsensno(-1); + if(-1 == sensno) return; // no sensors found + } + switch(MLX_state){ + case MLX_NOTINIT: // start reading parameters + if(i2c_read_reg16(sensaddr[sensno], REG_CALIDATA, MLX_DMA_MAXLEN, 1)){ + D(i2str(sensno)); DN(" wait conf"); + errctr = 0; + MLX_state = MLX_WAITPARAMS; + }else ++errctr; + break; + case MLX_WAITPARAMS: // check DMA ends and calculate parameters + if(i2c_dma_haderr()){ MLX_state = MLX_NOTINIT; DN("DMA err");} + else{ + uint16_t len, *buf = i2c_dma_getbuf(&len); + if(!buf) break; + DN("READ"); + if(len != MLX_DMA_MAXLEN){ MLX_state = MLX_NOTINIT; break; } + memcpy(confdata[sensno], buf, MLX_DMA_MAXLEN * sizeof(uint16_t)); + D(i2str(sensno)); DN(" got conf"); + int next = nextsensno(sensno); + errctr = 0; + if(next <= sensno) MLX_state = MLX_WAITSUBPAGE; // all configuration read + else MLX_state = MLX_NOTINIT; // read next + sensno = next; + return; + } + break; + case MLX_WAITSUBPAGE: // wait for subpage 1 ready + {uint16_t *got = i2c_read_reg16(sensaddr[sensno], REG_STATUS, 1, 0); + if(got && *got & REG_STATUS_NEWDATA){ + if(subpage == (*got & REG_STATUS_SPNO)){ + errctr = 0; + if(subpage == 0){ // omit zero subpage for each sensor + DN("omit 0 -> next sens"); + int next = nextsensno(sensno); + if(next <= sensno){ // all scanned - now wait for page 1 + subpage = 1; + DN("Wait for 1"); + } + sensno = next; + break; + } + D(i2str(sensno)); DN(" - ask for image"); + if(i2c_read_reg16(sensaddr[sensno], REG_IMAGEDATA, REG_IMAGEDATA_LEN, 1)){ + errctr = 0; + MLX_state = MLX_READSUBPAGE; + // D("spstart"); DB('0'+subpage); DB('='); DN(u2str(Tms - Tlast)); + }else ++errctr; + } + }else ++errctr; + } + break; + case MLX_READSUBPAGE: // wait ends of DMA read and calculate subpage + if(i2c_dma_haderr()) MLX_state = MLX_WAITSUBPAGE; + else{ + uint16_t len, *buf = i2c_dma_getbuf(&len); + if(buf){ + // D("spread="); DN(u2str(Tms - Tlast)); + if(len != REG_IMAGEDATA_LEN){ + ++errctr; + }else{ // fine! we could check next sensor + errctr = 0; + memcpy(imdata[sensno], buf, REG_IMAGEDATA_LEN * sizeof(int16_t)); + // D("spgot="); DN(u2str(Tms - Tlast)); + Tlastimage[sensno] = Tms; + // D("imgot="); DN(u2str(Tms - Tlast)); Tlast = Tms; + int next = nextsensno(sensno); + if(next <= sensno){ + subpage = 0; // roll to start - omit page 0 for all + DN("All got -> start from 0"); + } + sensno = next; + } + MLX_state = MLX_WAITSUBPAGE; + } + } + break; + default: + return; + } + //if(MLX_state != MLX_RELAX && Tms - Tlastimage[sensno] > MLX_I2CERR_TMOUT){ i2c_setup(i2c_curspeed); Tlastimage[sensno] = Tms; } + if(errctr > MLX_MAX_ERRORS){ + errctr = 0; + sensaddr[sensno] = 0; // throw out this value + sensno = nextsensno(sensno); + } +} + +// recalculate parameters +MLX90640_params *mlx_getparams(int n){ + MLX90640_params *p = get_parameters(confdata[n]); + return p; +} + +uint32_t mlx_lastimT(int n){ return Tlastimage[n]; } + +fp_t *mlx_getimage(int n){ + if(n < 0 || n >= N_SESORS || !sensaddr[n]) return NULL; + MLX90640_params *p = get_parameters(confdata[n]); + if(!p) return NULL; + fp_t *ready_image = process_image(imdata[n]); + if(!ready_image) return NULL; + return ready_image; +} + +// this function can be run only when state machine is paused/stopped! +// WARNING: `MLX_address` is shifted, `addr` - NOT! +int mlx_sethwaddr(uint8_t MLX_address, uint8_t addr){ + if(addr > 0x7f) return 0; + uint16_t data[2], *ptr; + if(!(ptr = i2c_read_reg16(MLX_address, REG_MLXADDR, 1, 0))) return 0; + //D("Old address: "); DN(uhex2str(*ptr)); + data[0] = REG_MLXADDR; data[1] = 0; + uint16_t oldreg = *ptr; + if(!i2c_write(MLX_address, data, 2)) return 0; // clear address + uint32_t Told = Tms; + while(Tms - Told < 10); + ptr = i2c_read_reg16(MLX_address, REG_MLXADDR, 1, 0); + // should be zero + if(!ptr){ + data[0] = REG_MLXADDR; data[1] = oldreg; + i2c_write(MLX_address, data, 2); // leave old address + return 0; + } + data[0] = REG_MLXADDR; // i2c_write swaps bytes, so we need init data again + data[1] = (oldreg & ~REG_MLXADDR_MASK) | addr; + //D("Write address: "); D(uhex2str(data[0])); D(", "); DN(uhex2str(data[1])); + if(!i2c_write(MLX_address, data, 2)) return 0; + while(Tms - Told < 10); + if(!(ptr = i2c_read_reg16(MLX_address, REG_MLXADDR, 1, 0))) return 0; + //D("Got address: "); DN(uhex2str(*ptr)); + if((*ptr & REG_MLXADDR_MASK) != addr) return 0; + return 1; +} + diff --git a/F3:F303/MLX90640-allsky/mlxproc.h b/F3:F303/MLX90640-allsky/mlxproc.h new file mode 100644 index 0000000..de7a920 --- /dev/null +++ b/F3:F303/MLX90640-allsky/mlxproc.h @@ -0,0 +1,52 @@ +/* + * This file is part of the ir-allsky 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 + +#include "mlx90640.h" + +// amount of sensors processing +#define N_SESORS (5) + +// maximal errors number to stop processing +#define MLX_MAX_ERRORS (11) +// if there's no new data by this time - reset bus +#define MLX_I2CERR_TMOUT (5000) + +typedef enum{ + MLX_NOTINIT, // just start - need to get parameters + MLX_WAITPARAMS, // wait for parameters DMA reading + MLX_WAITSUBPAGE, // wait for subpage changing + MLX_READSUBPAGE, // wait ending of subpage DMA reading + MLX_RELAX // do nothing - pause +} mlx_state_t; + +int mlx_setaddr(int n, uint8_t addr); +mlx_state_t mlx_state(); +int mlx_nactive(); +uint8_t *mlx_activeids(); +void mlx_pause(); +void mlx_stop(); +void mlx_continue(); +void mlx_process(); +MLX90640_params *mlx_getparams(int sensno); +fp_t *mlx_getimage(int sensno); +int mlx_sethwaddr(uint8_t MLX_address, uint8_t addr); +uint32_t mlx_lastimT(int sensno); diff --git a/F3:F303/MLX90640-allsky/openocd.cfg b/F3:F303/MLX90640-allsky/openocd.cfg new file mode 100644 index 0000000..56ccc2e --- /dev/null +++ b/F3:F303/MLX90640-allsky/openocd.cfg @@ -0,0 +1,119 @@ +# SPDX-License-Identifier: GPL-2.0-or-later + +# script for stm32f3x family + +# +# stm32 devices support both JTAG and SWD transports. +# +source [find interface/stlink-v2-1.cfg] +source [find target/swj-dp.tcl] +source [find mem_helper.tcl] + +if { [info exists CHIPNAME] } { + set _CHIPNAME $CHIPNAME +} else { + set _CHIPNAME stm32f3x +} + +set _ENDIAN little + +# Work-area is a space in RAM used for flash programming +# By default use 16kB +if { [info exists WORKAREASIZE] } { + set _WORKAREASIZE $WORKAREASIZE +} else { + set _WORKAREASIZE 0x4000 +} + +# JTAG speed should be <= F_CPU/6. F_CPU after reset is 8MHz, so use F_JTAG = 1MHz +# +# Since we may be running of an RC oscilator, we crank down the speed a +# bit more to be on the safe side. Perhaps superstition, but if are +# running off a crystal, we can run closer to the limit. Note +# that there can be a pretty wide band where things are more or less stable. +adapter speed 1000 + +adapter srst delay 100 +if {[using_jtag]} { + jtag_ntrst_delay 100 +} + +#jtag scan chain +if { [info exists CPUTAPID] } { + set _CPUTAPID $CPUTAPID +} else { + if { [using_jtag] } { + # See STM Document RM0316 + # Section 29.6.3 - corresponds to Cortex-M4 r0p1 + set _CPUTAPID 0x4ba00477 + } { + set _CPUTAPID 0x2ba01477 + } +} + +swj_newdap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_CPUTAPID +dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.cpu + +if {[using_jtag]} { + jtag newtap $_CHIPNAME bs -irlen 5 +} + +set _TARGETNAME $_CHIPNAME.cpu +target create $_TARGETNAME cortex_m -endian $_ENDIAN -dap $_CHIPNAME.dap + +$_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0 + +set _FLASHNAME $_CHIPNAME.flash +flash bank $_FLASHNAME stm32f1x 0 0 0 0 $_TARGETNAME + +reset_config srst_nogate + +if {![using_hla]} { + # if srst is not fitted use SYSRESETREQ to + # perform a soft reset + cortex_m reset_config sysresetreq +} + +proc stm32f3x_default_reset_start {} { + # Reset clock is HSI (8 MHz) + adapter speed 1000 +} + +proc stm32f3x_default_examine_end {} { + # Enable debug during low power modes (uses more power) + mmw 0xe0042004 0x00000007 0 ;# DBGMCU_CR |= DBG_STANDBY | DBG_STOP | DBG_SLEEP + + # Stop watchdog counters during halt + mmw 0xe0042008 0x00001800 0 ;# DBGMCU_APB1_FZ |= DBG_IWDG_STOP | DBG_WWDG_STOP +} + +proc stm32f3x_default_reset_init {} { + # Configure PLL to boost clock to HSI x 8 (64 MHz) + mww 0x40021004 0x00380400 ;# RCC_CFGR = PLLMUL[3:1] | PPRE1[2] + mmw 0x40021000 0x01000000 0 ;# RCC_CR |= PLLON + mww 0x40022000 0x00000012 ;# FLASH_ACR = PRFTBE | LATENCY[1] + sleep 10 ;# Wait for PLL to lock + mmw 0x40021004 0x00000002 0 ;# RCC_CFGR |= SW[1] + + # Boost JTAG frequency + adapter speed 8000 +} + +# Default hooks +$_TARGETNAME configure -event examine-end { stm32f3x_default_examine_end } +$_TARGETNAME configure -event reset-start { stm32f3x_default_reset_start } +$_TARGETNAME configure -event reset-init { stm32f3x_default_reset_init } + +tpiu create $_CHIPNAME.tpiu -dap $_CHIPNAME.dap -ap-num 0 -baseaddr 0xE0040000 + +lappend _telnet_autocomplete_skip _proc_pre_enable_$_CHIPNAME.tpiu +proc _proc_pre_enable_$_CHIPNAME.tpiu {_targetname} { + targets $_targetname + + # Set TRACE_IOEN; TRACE_MODE is set to async; when using sync + # change this value accordingly to configure trace pins + # assignment + mmw 0xe0042004 0x00000020 0 +} + +$_CHIPNAME.tpiu configure -event pre-enable "_proc_pre_enable_$_CHIPNAME.tpiu $_TARGETNAME" diff --git a/F3:F303/MLX90640-allsky/proto.c b/F3:F303/MLX90640-allsky/proto.c new file mode 100644 index 0000000..7857960 --- /dev/null +++ b/F3:F303/MLX90640-allsky/proto.c @@ -0,0 +1,366 @@ +/* + * This file is part of the ir-allsky 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 + +#include "i2c.h" +#include "mlxproc.h" +#include "strfunc.h" +#include "usart.h" +#include "usb_dev.h" +#include "version.inc" + +#define LOCBUFFSZ (32) +// local buffer for I2C data to send +static uint16_t locBuffer[LOCBUFFSZ]; +static uint8_t I2Caddress = 0x33 << 1; +extern volatile uint32_t Tms; +uint8_t cartoon = 0; // "cartoon" mode: refresh image each time we get new + +// common names for frequent keys +const char *Timage = "TIMAGE="; +const char *Sensno = "SENSNO="; + +static const char *OK = "OK\n", *ERR = "ERR\n"; +const char *helpstring = + "https://github.com/eddyem/stm32samples/tree/master/F3:F303/MLX90640multi build#" BUILD_NUMBER " @ " BUILD_DATE "\n" + " management of single IR bolometer MLX90640\n" + "aa - change I2C address to a (a should be non-shifted value!!!)\n" + "c - continue MLX\n" + "dn - draw nth image in ASCII\n" + "gn - get nth image 'as is' - float array of 768x4 bytes\n" + "i0..4 - setup I2C with speed 10k, 100k, 400k, 1M or 2M (experimental!)\n" + "l - list active sensors IDs\n" + "tn - show temperature map of nth image\n" + "p - pause MLX\n" + "s - stop MLX (and start from zero @ 'c'\n" + "tn - show nth image aquisition time\n" + "C - \"cartoon\" mode on/off (show each new image)\n" + "Dn - dump MLX parameters for sensor number n\n" + "G - get MLX state\n" + "Ia addr [n] - set device address for interactive work or (with n) change address of n'th sensor\n" + "Ir reg n - read n words from 16-bit register\n" + "Iw words - send words (hex/dec/oct/bin) to I2C\n" + "Is - scan I2C bus\n" + "T - print current Tms\n" + "Us - send string 's' to USART\n" +; + +TRUE_INLINE const char *setupI2C(char *buf){ + static const char *speeds[I2C_SPEED_AMOUNT] = { + [I2C_SPEED_10K] = "10K", + [I2C_SPEED_100K] = "100K", + [I2C_SPEED_400K] = "400K", + [I2C_SPEED_1M] = "1M", + [I2C_SPEED_2M] = "2M" + }; + if(buf && *buf){ + buf = omit_spaces(buf); + int speed = *buf - '0'; + if(speed < 0 || speed >= I2C_SPEED_AMOUNT){ + return ERR; + } + i2c_setup((i2c_speed_t)speed); + } + U("I2CSPEED="); USND(speeds[i2c_curspeed]); + return NULL; +} + +TRUE_INLINE const char *chhwaddr(const char *buf){ + uint32_t a; + if(buf && *buf){ + const char *nxt = getnum(buf, &a); + if(nxt && nxt != buf){ + if(!mlx_sethwaddr(I2Caddress, a)) return ERR; + }else{ + USND("Wrong number"); + return ERR; + } + }else{ + USND("Need address"); + return ERR; + } + return OK; +} + +// read sensor's number from `buf`; return -1 if error +static int getsensnum(const char *buf){ + if(!buf || !*buf) return -1; + uint32_t num; + const char *nxt = getnum(buf, &num); + if(!nxt || nxt == buf || num >= N_SESORS) return -1; + return (int) num; +} + +TRUE_INLINE const char *chaddr(const char *buf){ + uint32_t addr; + const char *nxt = getnum(buf, &addr); + if(nxt && nxt != buf){ + if(addr > 0x7f) return ERR; + I2Caddress = (uint8_t) addr << 1; + int n = getsensnum(nxt); + if(n > -1) mlx_setaddr(n, addr); + }else addr = I2Caddress >> 1; + U("I2CADDR="); USND(uhex2str(addr)); + return NULL; +} + +// read I2C register[s] - only blocking read! (DMA allowable just for config/image reading in main process) +static const char *rdI2C(const char *buf){ + uint32_t N = 0; + const char *nxt = getnum(buf, &N); + if(!nxt || buf == nxt || N > 0xffff) return ERR; + buf = nxt; + uint16_t reg = N, *b16 = NULL; + nxt = getnum(buf, &N); + if(!nxt || buf == nxt || N == 0 || N > I2C_BUFSIZE) return ERR; + if(!(b16 = i2c_read_reg16(I2Caddress, reg, N, 0))) return ERR; + if(N == 1){ + char b[5]; + u16s(*b16, b); + b[4] = 0; + USND(b); + }else hexdump16(USB_sendstr, b16, N); + return NULL; +} + +// read N numbers from buf, @return 0 if wrong or none +TRUE_INLINE uint16_t readNnumbers(const char *buf){ + uint32_t D; + const char *nxt; + uint16_t N = 0; + while((nxt = getnum(buf, &D)) && nxt != buf && N < LOCBUFFSZ){ + buf = nxt; + locBuffer[N++] = (uint16_t) D; + } + return N; +} + +static const char *wrI2C(const char *buf){ + uint16_t N = readNnumbers(buf); + if(N == 0) return ERR; + for(int i = 0; i < N; ++i){ + U("byte "); U(u2str(i)); U(" :"); USND(uhex2str(locBuffer[i])); + } + if(!i2c_write(I2Caddress, locBuffer, N)) return ERR; + return OK; +} + +static void dumpfarr(float *arr){ + for(int row = 0; row < 24; ++row){ + for(int col = 0; col < 32; ++col){ + printfl(*arr++, 2); USB_putbyte(' '); + } + newline(); + } +} +// dump MLX parameters +TRUE_INLINE void dumpparams(const char *buf){ + int N = getsensnum(buf); + if(N < 0){ U(ERR); return; } + MLX90640_params *params = mlx_getparams(N); + if(!params){ U(ERR); return; } + U(Sensno); USND(i2str(N)); + U("\nkVdd="); printi(params->kVdd); + U("\nvdd25="); printi(params->vdd25); + U("\nKvPTAT="); printfl(params->KvPTAT, 4); + U("\nKtPTAT="); printfl(params->KtPTAT, 4); + U("\nvPTAT25="); printi(params->vPTAT25); + U("\nalphaPTAT="); printfl(params->alphaPTAT, 2); + U("\ngainEE="); printi(params->gainEE); + U("\nPixel offset parameters:\n"); + float *offset = params->offset; + for(int row = 0; row < 24; ++row){ + for(int col = 0; col < 32; ++col){ + printfl(*offset++, 2); USB_putbyte(' '); + } + newline(); + } + U("K_talpha:\n"); + dumpfarr(params->kta); + U("Kv: "); + for(int i = 0; i < 4; ++i){ + printfl(params->kv[i], 2); USB_putbyte(' '); + } + U("\ncpOffset="); + printi(params->cpOffset[0]); U(", "); printi(params->cpOffset[1]); + U("\ncpKta="); printfl(params->cpKta, 2); + U("\ncpKv="); printfl(params->cpKv, 2); + U("\ntgc="); printfl(params->tgc, 2); + U("\ncpALpha="); printfl(params->cpAlpha[0], 2); + U(", "); printfl(params->cpAlpha[1], 2); + U("\nKsTa="); printfl(params->KsTa, 2); + U("\nAlpha:\n"); + dumpfarr(params->alpha); + U("\nCT3="); printfl(params->CT[1], 2); + U("\nCT4="); printfl(params->CT[2], 2); + for(int i = 0; i < 4; ++i){ + U("\nKsTo"); USB_putbyte('0'+i); USB_putbyte('='); + printfl(params->KsTo[i], 2); + U("\nalphacorr"); USB_putbyte('0'+i); USB_putbyte('='); + printfl(params->alphacorr[i], 2); + } + newline(); +} +// get MLX state +TRUE_INLINE void getst(){ + static const char *states[] = { + [MLX_NOTINIT] = "not init", + [MLX_WAITPARAMS] = "wait parameters DMA read", + [MLX_WAITSUBPAGE] = "wait subpage", + [MLX_READSUBPAGE] = "wait subpage DMA read", + [MLX_RELAX] = "do nothing" + }; + mlx_state_t s = mlx_state(); + U("MLXSTATE="); + USND(states[s]); +} + +// `draw`==1 - draw, ==0 - show T map, 2 - send raw float array with prefix 'SENSNO=x\nTimage=y\n' and postfix "ENDIMAGE\n" +static const char *drawimg(const char *buf, int draw){ + int sensno = getsensnum(buf); + if(sensno > -1){ + uint32_t T = mlx_lastimT(sensno); + fp_t *img = mlx_getimage(sensno); + if(img){ + U(Sensno); USND(u2str(sensno)); + U(Timage); USND(u2str(T)); + switch(draw){ + case 0: + dumpIma(img); + break; + case 1: + drawIma(img); + break; + case 2: + { + uint8_t *d = (uint8_t*)img; + uint32_t _2send = MLX_PIXNO * sizeof(float); + // send by portions of 256 bytes (as image is larger than ringbuffer) + while(_2send){ + uint32_t portion = (_2send > 256) ? 256 : _2send; + USB_send(d, portion); + _2send -= portion; + d += portion; + } + } + USND("ENDIMAGE"); + } + return NULL; + } + } + return ERR; +} + +TRUE_INLINE void listactive(){ + int N = mlx_nactive(); + if(!N){ USND("No active sensors found!"); return; } + uint8_t *ids = mlx_activeids(); + U("Found "); USB_putbyte('0'+N); USND(" active sensors:"); + for(int i = 0; i < N_SESORS; ++i) + if(ids[i]){ + U("SENSID"); U(u2str(i)); USB_putbyte('='); + U(uhex2str(ids[i] >> 1)); + newline(); + } +} + +static void getimt(const char *buf){ + int sensno = getsensnum(buf); + if(sensno > -1){ + U(Timage); USND(u2str(mlx_lastimT(sensno))); + }else U(ERR); +} + +const char *parse_cmd(char *buf){ + if(!buf || !*buf) return NULL; + if(buf[1]){ + switch(*buf){ // "long" commands + case 'a': + return chhwaddr(buf + 1); + case 'd': + return drawimg(buf+1, 1); + case 'g': + return drawimg(buf+1, 2); + case 'i': + return setupI2C(buf + 1); + case 'm': + return drawimg(buf+1, 0); + case 't': + getimt(buf + 1); return NULL; + case 'D': + dumpparams(buf + 1); + return NULL; + break; + case 'I': + buf = omit_spaces(buf + 1); + switch(*buf){ + case 'a': + return chaddr(buf + 1); + case 'r': + return rdI2C(buf + 1); + case 'w': + return wrI2C(buf + 1); + case 's': + i2c_init_scan_mode(); + return OK; + default: + return ERR; + } + break; + case 'U': + if(usart_sendstr(buf + 1) && usart_putchar('\n')) return OK; + return ERR; + default: + return ERR; + } + } + switch(*buf){ // "short" (one letter) commands + case 'c': + mlx_continue(); return OK; + break; + case 'i': return setupI2C(NULL); // current settings + case 'l': + listactive(); + break; + case 'p': + mlx_pause(); return OK; + break; + case 's': + mlx_stop(); return OK; + case 'C': + cartoon = !cartoon; return OK; + case 'G': + getst(); + break; + case 'T': + U("T="); + USND(u2str(Tms)); + break; + case '?': // help + case 'h': + case 'H': + U(helpstring); + break; + default: + return ERR; + break; + } + return NULL; +} diff --git a/F3:F303/MLX90640-allsky/proto.h b/F3:F303/MLX90640-allsky/proto.h new file mode 100644 index 0000000..c65b78d --- /dev/null +++ b/F3:F303/MLX90640-allsky/proto.h @@ -0,0 +1,24 @@ +/* + * This file is part of the ir-allsky 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 + +extern const char *Timage, *Sensno; + +extern uint8_t cartoon; +char *parse_cmd(char *buf); diff --git a/F3:F303/MLX90640-allsky/ringbuffer.c b/F3:F303/MLX90640-allsky/ringbuffer.c new file mode 100644 index 0000000..41e13ac --- /dev/null +++ b/F3:F303/MLX90640-allsky/ringbuffer.c @@ -0,0 +1,163 @@ +/* + * 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 "ringbuffer.h" + +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){ + if(b->busy) return -1; + b->busy = 1; + int l = datalen(b); + b->busy = 0; + 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){ + if(b->busy) return -1; + b->busy = 1; + int ret = hasbyte(b, byte); + b->busy = 0; + 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){ + if(b->busy) return -1; + b->busy = 1; + int r = read(b, s, len); + b->busy = 0; + return r; +} + +static int readto(ringbuffer *b, uint8_t byte, uint8_t *s, int len){ + 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; + if(partlen > len) return -read(b, s, len); + 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 + * @param len - length of `s` + * @return amount of bytes written (negative, if lenbusy) return -1; + b->busy = 1; + int n = readto(b, byte, s, len); + b->busy = 0; + return n; +} + +static int write(ringbuffer *b, const uint8_t *str, int l){ + int r = b->length - 1 - datalen(b); // rest length + if(l > r || !l) return 0; + 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){ + if(b->busy) return -1; + b->busy = 1; + int w = write(b, str, l); + b->busy = 0; + return w; +} + +// just delete all information in buffer `b` +int RB_clearbuf(ringbuffer *b){ + if(b->busy) return -1; + b->busy = 1; + b->head = 0; + b->tail = 0; + b->busy = 0; + return 1; +} diff --git a/F3:F303/MLX90640-allsky/ringbuffer.h b/F3:F303/MLX90640-allsky/ringbuffer.h new file mode 100644 index 0000000..ed2cf95 --- /dev/null +++ b/F3:F303/MLX90640-allsky/ringbuffer.h @@ -0,0 +1,41 @@ +/* + * 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 + +#if defined STM32F0 +#include +#elif defined STM32F1 +#include +#elif defined STM32F3 +#include +#endif + +typedef struct{ + uint8_t *data; // data buffer + const int length; // its length + int head; // head index + int tail; // tail index + volatile int 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_clearbuf(ringbuffer *b); diff --git a/F3:F303/MLX90640-allsky/strfunc.c b/F3:F303/MLX90640-allsky/strfunc.c new file mode 100644 index 0000000..4d0009e --- /dev/null +++ b/F3:F303/MLX90640-allsky/strfunc.c @@ -0,0 +1,377 @@ +/* + * This file is part of the ir-allsky 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 // isnan / isinf + +#include "strfunc.h" + +// hex line number for hexdumps +void u16s(uint16_t n, char *buf){ + for(int j = 3; j > -1; --j){ + register uint8_t q = n & 0xf; + n >>= 4; + if(q < 10) buf[j] = q + '0'; + else buf[j] = q - 10 + 'a'; + } +} + +/** + * @brief hexdump - dump hex array by 16 bytes in string + * @param sendfun - function to send data + * @param arr - array to dump + * @param len - length of `arr` + */ +void hexdump(int (*sendfun)(const char *s), uint8_t *arr, uint16_t len){ + char buf[64] = "0000 ", *bptr = &buf[6]; + 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 & 0xf) == 0xf){ + *bptr++ = '\n'; + *bptr = 0; + sendfun(buf); + u16s(l + 1, buf); + bptr = &buf[6]; + }else *bptr++ = ' '; + } + if(bptr != &buf[6]){ + *bptr++ = '\n'; + *bptr = 0; + sendfun(buf); + } +} + +// dump uint16_t by 8 values in string +void hexdump16(int (*sendfun)(const char *s), uint16_t *arr, uint16_t len){ + char buf[64] = "0000 ", *bptr = &buf[6]; + for(uint16_t l = 0; l < len; ++l, ++arr){ + //uint16_t val = *arr; + u16s(*arr, bptr); + /*for(int16_t j = 3; j > -1; --j){ + register uint8_t q = val & 0xf; + val >>= 4; + if(q < 10) bptr[j] = q + '0'; + else bptr[j] = q - 10 + 'a'; + }*/ + bptr += 4; + if((l & 7) == 7){ + *bptr++ = '\n'; + *bptr = 0; + sendfun(buf); + u16s((l + 1)*2, buf); // number of byte, not word! + bptr = &buf[6]; + }else *bptr++ = ' '; + } + if(bptr != &buf[6]){ + *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 const 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; + //*(--bufptr) = val % 10 + '0'; + //val /= 10; + } + } + if(minus) *(--bufptr) = '-'; + return bufptr; +} + +// return string with number `val` +const char *u2str(uint32_t val){ + return _2str(val, 0); +} +const 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 + */ +const 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(const char *buf){ + while(*buf){ + if(*buf > ' ') break; + ++buf; + } + return (char*)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 const char *getdec(const 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!) +static const char *gethex(const char *buf, uint32_t *N){ + const 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 const char *getoct(const char *buf, uint32_t *N){ + const 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 const char *getbin(const char *buf, uint32_t *N){ + const 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) + */ +const char *getnum(const char *txt, uint32_t *N){ + const char *nxt = NULL; + const 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 +const char *getint(const char *txt, int32_t *I){ + const char *s = omit_spaces(txt); + int32_t sign = 1; + uint32_t U; + if(*s == '-'){ + sign = -1; + ++s; + } + const char *nxt = getnum(s, &U); + if(nxt == s) return txt; + if(U & 0x80000000) return txt; // overfull + *I = sign * (int32_t)U; + return nxt; +} + + +// be careful: if pow10 would be bigger you should change str[] size! +static const float pwr10[] = {1.f, 10.f, 100.f, 1000.f, 10000.f}; +static const float rounds[] = {0.5f, 0.05f, 0.005f, 0.0005f, 0.00005f}; +#define P10L (sizeof(pwr10)/sizeof(uint32_t) - 1) +char *float2str(float x, uint8_t prec){ + static char str[16] = {0}; // -117.5494E-36\0 - 14 symbols max! + if(prec > P10L) prec = P10L; + if(isnan(x)){ memcpy(str, "NAN", 4); return str;} + else{ + int i = isinf(x); + if(i){memcpy(str, "-INF", 5); if(i == 1) return str+1; else return str;} + } + char *s = str + 14; // go to end of buffer + uint8_t minus = 0; + if(x < 0){ + x = -x; + minus = 1; + } + int pow = 0; // xxxEpow + // now convert float to 1.xxxE3y + while(x > 1000.f){ + x /= 1000.f; + pow += 3; + } + if(x > 0.) while(x < 1.){ + x *= 1000.f; + pow -= 3; + } + // print Eyy + if(pow){ + uint8_t m = 0; + if(pow < 0){pow = -pow; m = 1;} + while(pow){ + register int p10 = pow/10; + *s-- = '0' + (pow - 10*p10); + pow = p10; + } + if(m) *s-- = '-'; + *s-- = 'E'; + } + // now our number is in [1, 1000] + uint32_t units; + if(prec){ + units = (uint32_t) x; + uint32_t decimals = (uint32_t)((x-units+rounds[prec])*pwr10[prec]); + // print decimals + while(prec){ + register int d10 = decimals / 10; + *s-- = '0' + (decimals - 10*d10); + decimals = d10; + --prec; + } + // decimal point + *s-- = '.'; + }else{ // without decimal part + units = (uint32_t) (x + 0.5); + } + // print main units + if(units == 0) *s-- = '0'; + else while(units){ + register uint32_t u10 = units / 10; + *s-- = '0' + (units - 10*u10); + units = u10; + } + if(minus) *s-- = '-'; + return s+1; +} diff --git a/F3:F303/MLX90640-allsky/strfunc.h b/F3:F303/MLX90640-allsky/strfunc.h new file mode 100644 index 0000000..899d713 --- /dev/null +++ b/F3:F303/MLX90640-allsky/strfunc.h @@ -0,0 +1,40 @@ +/* + * This file is part of the ir-allsky 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 +#include + +#include "usb_dev.h" + +#define printu(x) do{USB_sendstr(u2str(x));}while(0) +#define printi(x) do{USB_sendstr(i2str(x));}while(0) +#define printuhex(x) do{USB_sendstr(uhex2str(x));}while(0) +#define printfl(x,n) do{USB_sendstr(float2str(x, n));}while(0) + +void u16s(uint16_t n, char *buf); +void hexdump16(int (*sendfun)(const char *s), uint16_t *arr, uint16_t len); +void hexdump(int (*sendfun)(const char *s), uint8_t *arr, uint16_t len); +const char *u2str(uint32_t val); +const char *i2str(int32_t i); +const char *uhex2str(uint32_t val); +const char *getnum(const char *txt, uint32_t *N); +char *omit_spaces(const char *buf); +const char *getint(const char *txt, int32_t *I); +char *float2str(float x, uint8_t prec); diff --git a/F3:F303/MLX90640-allsky/usart.c b/F3:F303/MLX90640-allsky/usart.c new file mode 100644 index 0000000..239539f --- /dev/null +++ b/F3:F303/MLX90640-allsky/usart.c @@ -0,0 +1,193 @@ +/* + * This file is part of the ir-allsky 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 + +#include "hardware.h" +#include "ringbuffer.h" +#include "usart.h" + +// unlike USB where you can hold NACK until user process frees ringbuffer, here we can't do that +// so USART writing forced by user (ringbuffer full or timeout by `usart_chk`) + +extern volatile uint32_t Tms; +// flags +static volatile uint8_t bufovr = 0, // input buffer overfull + rbufno = 0, // index of active receiving buffer + txrdy = 1; // transmission done + +static char rbuf[2][UARTBUFSZI]; // double receiving buffer +static char *recvdata = NULL; // pointer to last received data +static volatile int recvdatalen = 0; +// to transmit images we reserve circular buffer large enough to hold full image +static uint8_t rbdata[DMARBSZ]; +static ringbuffer dmarb = {.data = rbdata, .length = DMARBSZ, .head = 0, .tail = 0}; + +static int transmit_tbuf(); + +// return 1 if overflow was +int usart_ovr(){ + if(bufovr){ + bufovr = 0; + return 1; + } + return 0; +} + +// check if the buffer was filled >TRANSMIT_DELAY ago (transmit it then) +void usart_process(){ + transmit_tbuf(); +} + +/** + * @brief usart_getline - read one dataportion + * @param buf - user buffer + * @param len - its length + * @return amount of bytes + */ +char *usart_getline(int *len){ + if(!recvdatalen) return NULL; + if(len) *len = recvdatalen; + recvdatalen = 0; + return recvdata; +} + +// transmit next dataportion from ringbuffer +static int transmit_tbuf(){ + static uint8_t tbuf[UARTBUFSZO]; + uint32_t T0 = Tms; + while(!txrdy && Tms - T0 < RXRDY_TMOUT) IWDG->KR = IWDG_REFRESH; + if(!txrdy) return 0; + int l = RB_read(&dmarb, tbuf, UARTBUFSZO); + if(l < 1) return 1; + txrdy = 0; + DMA1_Channel4->CCR &= ~DMA_CCR_EN; + DMA1_Channel4->CMAR = (uint32_t) tbuf; + DMA1_Channel4->CNDTR = l; + DMA1_Channel4->CCR |= DMA_CCR_EN; + return 1; +} + +// return 0 if can't write to ringbuffer +int usart_putchar(const char ch){ + int r = RB_write(&dmarb, (uint8_t*)&ch, 1); + if(r != 1){ + if(transmit_tbuf()) r = RB_write(&dmarb, (uint8_t*)&ch, 1); + } + return r; +} + +// @return amount of written bytes +int usart_send(const uint8_t *data, int len){ + if(len > DMARBSZ) return FALSE; + int L = 0; + do{ + int r = RB_write(&dmarb, data, len - L); + if(r < 1){ + if(!transmit_tbuf()) return L; + else continue; + } + L += r; + data += r; + }while(L < len); + return L; +} + +// WARNING! strlen of `str` should be less than RBout size! +// @return amount of written bytes +int usart_sendstr(const char *str){ + return usart_send((uint8_t*)str, strlen(str)); +} + +// USART1: Rx - PA10 (AF7), Tx - PA9 (AF7) +int usart_setup(uint32_t speed){ + if(speed < 200 || speed > 3000000) return FALSE; + // setup pins: + GPIOA->MODER = (GPIOA->MODER & (MODER_CLR(9) & MODER_CLR(10))) | + MODER_AF(9) | MODER_AF(10); + GPIOA->AFR[1] = (GPIOA->AFR[1] & ~(GPIO_AFRH_AFRH1 | GPIO_AFRH_AFRH2)) | + AFRf(7, 9) | AFRf(7, 10); + // clock + RCC->APB2ENR |= RCC_APB2ENR_USART1EN; + RCC->AHBENR |= RCC_AHBENR_DMA1EN; + USART1->ICR = 0xffffffff; // clear all flags + // Tx DMA + DMA1_Channel4->CCR = 0; + DMA1_Channel4->CPAR = (uint32_t) &USART1->TDR; // periph + DMA1_Channel4->CCR |= DMA_CCR_MINC | DMA_CCR_DIR | DMA_CCR_TCIE; // 8bit, mem++, mem->per, transcompl irq + // Rx DMA + DMA1_Channel5->CCR = 0; + DMA1_Channel5->CPAR = (uint32_t) &USART1->RDR; // periph + DMA1_Channel5->CMAR = (uint32_t) rbuf[0]; + DMA1_Channel5->CNDTR = UARTBUFSZI; + DMA1_Channel5->CCR |= DMA_CCR_MINC | DMA_CCR_TCIE | DMA_CCR_EN; // 8bit, mem++, per->mem, transcompl irq, enable + // setup usart + USART1->BRR = SysFreq / speed; + USART1->CR3 = USART_CR3_DMAT | USART_CR3_DMAR; // enable DMA Tx/Rx + USART1->CR2 = USART_CR2_ADD_VAL('\n'); // init character match register: our input proto is string-based + USART1->CR1 = USART_CR1_TE | USART_CR1_RE | USART_CR1_UE | USART_CR1_CMIE; // 1start,8data,nstop; enable Rx,Tx,USART; enable CharacterMatch Irq + uint32_t tmout = 16000000; + while(!(USART1->ISR & USART_ISR_TC)){if(--tmout == 0) break;} // polling idle frame Transmission + USART1->ICR = 0xffffffff; // clear all flags again + NVIC_EnableIRQ(USART1_IRQn); + NVIC_EnableIRQ(DMA1_Channel4_IRQn); + NVIC_EnableIRQ(DMA1_Channel5_IRQn); + NVIC_SetPriority(DMA1_Channel5_IRQn, 0); + NVIC_SetPriority(USART1_IRQn, 4); // set character match priority lower + return TRUE; +} + +void usart_stop(){ + RCC->APB2ENR &= ~RCC_APB2ENR_USART1EN; +} + +// USART1 character match interrupt +void usart1_exti25_isr(){ + DMA1_Channel5->CCR &= ~DMA_CCR_EN; // temporaly disable DMA + USART1->ICR = USART_ICR_CMCF; // clear character match flag + register int l = UARTBUFSZI - DMA1_Channel5->CNDTR - 1; // substitute '\n' with '\0', omit empty strings! + if(l > 0){ + if(recvdata){ // user didn't read old data - mark as buffer overflow + bufovr = 1; + } + recvdata = rbuf[rbufno]; + recvdata[l] = 0; + rbufno = !rbufno; + recvdatalen = l; + } + DMA1_Channel5->CMAR = (uint32_t) rbuf[rbufno]; + DMA1_Channel5->CNDTR = UARTBUFSZI; + DMA1_Channel5->CCR |= DMA_CCR_EN; +} + +// USART1 Tx complete +void dma1_channel4_isr(){ + DMA1->IFCR |= DMA_IFCR_CTCIF4; + txrdy = 1; +} + +// USART1 Rx buffer overrun +void dma1_channel5_isr(){ + DMA1_Channel5->CCR &= ~DMA_CCR_EN; + DMA1->IFCR |= DMA_IFCR_CTCIF5; + DMA1_Channel5->CMAR = (uint32_t) rbuf[rbufno]; + DMA1_Channel5->CNDTR = UARTBUFSZI; + bufovr = 1; + DMA1_Channel5->CCR |= DMA_CCR_EN; +} diff --git a/F3:F303/MLX90640-allsky/usart.h b/F3:F303/MLX90640-allsky/usart.h new file mode 100644 index 0000000..1b1907d --- /dev/null +++ b/F3:F303/MLX90640-allsky/usart.h @@ -0,0 +1,37 @@ +/* + * This file is part of the ir-allsky 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 "hardware.h" + +// timeout of TXrdy waiting +#define RXRDY_TMOUT (100) +// DMA transmission and reception buffers' size +#define UARTBUFSZI (128) +#define UARTBUFSZO (256) +// and circular buffer for big transmissions - 5k +#define DMARBSZ (5120) + +int usart_ovr(); // RX overfull occured +void usart_process(); // send next data portion +int usart_setup(uint32_t speed); // set USART1 with given speed +char *usart_getline(int *len); // read from rbin to buf +int usart_sendstr(const char *str); +int usart_putchar(const char ch); +void usart_stop(); diff --git a/F3:F303/MLX90640-allsky/usb_descr.c b/F3:F303/MLX90640-allsky/usb_descr.c new file mode 100644 index 0000000..99534de --- /dev/null +++ b/F3:F303/MLX90640-allsky/usb_descr.c @@ -0,0 +1,210 @@ +/* + * Copyright 2024 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 "usb_descr.h" + +// low/high for uint16_t +#define L16(x) (x & 0xff) +#define H16(x) (x >> 8) + +static const uint8_t USB_DeviceDescriptor[] = { + USB_DT_DEVICE_SIZE, // bLength + USB_DT_DEVICE, // bDescriptorType + L16(bcdUSB), // bcdUSB_L + H16(bcdUSB), // bcdUSB_H + USB_CLASS_MISC, // bDeviceClass + bDeviceSubClass, // bDeviceSubClass + bDeviceProtocol, // bDeviceProtocol + USB_EP0BUFSZ, // bMaxPacketSize + L16(idVendor), // idVendor_L + H16(idVendor), // idVendor_H + L16(idProduct), // idProduct_L + H16(idProduct), // idProduct_H + L16(bcdDevice_Ver), // bcdDevice_Ver_L + H16(bcdDevice_Ver), // bcdDevice_Ver_H + iMANUFACTURER_DESCR, // iManufacturer - indexes of string descriptors in array + iPRODUCT_DESCR, // iProduct + iSERIAL_DESCR, // iSerial + bNumConfigurations // bNumConfigurations +}; + +static const uint8_t USB_DeviceQualifierDescriptor[] = { + USB_DT_QUALIFIER_SIZE, //bLength + USB_DT_QUALIFIER, // bDescriptorType + L16(bcdUSB), // bcdUSB_L + H16(bcdUSB), // bcdUSB_H + USB_CLASS_PER_INTERFACE, // bDeviceClass + bDeviceSubClass, // bDeviceSubClass + bDeviceProtocol, // bDeviceProtocol + USB_EP0BUFSZ, // bMaxPacketSize0 + bNumConfigurations, // bNumConfigurations + 0 // Reserved +}; + +#define wTotalLength (USB_DT_CONFIG_SIZE + (bNumInterfaces * USB_DT_INTERFACE_SIZE) + (bTotNumEndpoints * USB_DT_ENDPOINT_SIZE) + (bNumCsInterfaces * USB_DT_CS_INTERFACE_SIZE) - 1) + +static const uint8_t USB_ConfigDescriptor[] = { + // Configuration Descriptor + USB_DT_CONFIG_SIZE, // bLength: Configuration Descriptor size + USB_DT_CONFIG, // bDescriptorType: Configuration + L16(wTotalLength), // wTotalLength.L :no of returned bytes + H16(wTotalLength), // wTotalLength.H + bNumInterfaces, // bNumInterfaces + 1, // bConfigurationValue: Current configuration value + 0, // iConfiguration: Index of string descriptor describing the configuration or 0 + BusPowered, // bmAttributes - Bus powered + 50, // MaxPower in 2mA units + //--------------------------------------------------------------------------- + // Virtual command Interface Descriptor + USB_DT_INTERFACE_SIZE, // bLength: Interface Descriptor size + USB_DT_INTERFACE, // bDescriptorType: Interface + 0, // bInterfaceNumber: Number of Interface + 0, // bAlternateSetting: Alternate setting + 1, // bNumEndpoints: one for this + USB_CLASS_COMM, // bInterfaceClass + 2, // bInterfaceSubClass: ACM + 1, // bInterfaceProtocol: Common AT commands + iINTERFACE_DESCR1, // iInterface + // ---- CS Interfaces + USB_DT_CS_INTERFACE_SIZE, // bLength + USB_DT_CS_INTERFACE, // bDescriptorType: CS_INTERFACE + 0, // bDescriptorSubtype: Header Func Desc + 0x10, // bcdCDC: spec release number + 1, // bDataInterface + USB_DT_CS_INTERFACE_SIZE, // bLength + USB_DT_CS_INTERFACE, // bDescriptorType: CS_INTERFACE + 1, // bDescriptorSubtype: Call Management Func Desc + 0, // bmCapabilities: D0+D1 + 1, // bDataInterface + USB_DT_CS_INTERFACE_SIZE-1, // bLength + USB_DT_CS_INTERFACE, // bDescriptorType: CS_INTERFACE + 2, // bDescriptorSubtype: Abstract Control Management desc + 2, // bmCapabilities + USB_DT_CS_INTERFACE_SIZE, // bLength + USB_DT_CS_INTERFACE, // bDescriptorType: CS_INTERFACE + 6, // bDescriptorSubtype: Union func desc + 0, // bMasterInterface: Communication class interface + 1, // bSlaveInterface0: Data Class Interface + // Virtual endpoint 1 Descriptor + USB_DT_ENDPOINT_SIZE, // bLength: Endpoint Descriptor size + USB_DT_ENDPOINT, // bDescriptorType: Endpoint + 0x8A, // bEndpointAddress IN10 + USB_BM_ATTR_INTERRUPT, // bmAttributes: Interrupt + L16(USB_EP1BUFSZ), // wMaxPacketSize LO + H16(USB_EP1BUFSZ), // wMaxPacketSize HI + 0x10, // bInterval: 16ms + //--------------------------------------------------------------------------- + // Data interface + USB_DT_INTERFACE_SIZE, // bLength: Interface Descriptor size + USB_DT_INTERFACE, // bDescriptorType: Interface + 1, // bInterfaceNumber: Number of Interface + 0, // bAlternateSetting: Alternate setting + 2, // bNumEndpoints: in and out + USB_CLASS_DATA, // bInterfaceClass + 2, // bInterfaceSubClass: ACM + 0, // bInterfaceProtocol + 0, // iInterface + //Endpoint IN1 Descriptor + USB_DT_ENDPOINT_SIZE, // bLength: Endpoint Descriptor size + USB_DT_ENDPOINT, // bDescriptorType: Endpoint + 0x81, // bEndpointAddress: IN1 + USB_BM_ATTR_BULK, // bmAttributes: Bulk + L16(USB_TXBUFSZ), // wMaxPacketSize LO + H16(USB_TXBUFSZ), // wMaxPacketSize HI + 0, // bInterval: ignore for Bulk transfer + // Endpoint OUT1 Descriptor + USB_DT_ENDPOINT_SIZE, // bLength: Endpoint Descriptor size + USB_DT_ENDPOINT, // bDescriptorType: Endpoint + 0x01, // bEndpointAddress: OUT1 + USB_BM_ATTR_BULK, // bmAttributes: Bulk + L16(USB_RXBUFSZ), // wMaxPacketSize LO + H16(USB_RXBUFSZ), // wMaxPacketSize HI + 0, // bInterval: ignore for Bulk transfer + +}; + +//const uint8_t HID_ReportDescriptor[]; + +_USB_LANG_ID_(LD, LANG_US); +_USB_STRING_(SD, u"0.0.1"); +_USB_STRING_(MD, u"eddy@sao.ru"); +_USB_STRING_(PD, u"MLX90640 sensor management"); +_USB_STRING_(ID, u"ir-allsky"); + +static const void* const StringDescriptor[iDESCR_AMOUNT] = { + [iLANGUAGE_DESCR] = &LD, + [iMANUFACTURER_DESCR] = &MD, + [iPRODUCT_DESCR] = &PD, + [iSERIAL_DESCR] = &SD, + [iINTERFACE_DESCR1] = &ID +}; + +static void wr0(const uint8_t *buf, uint16_t size, uint16_t askedsize){ + if(askedsize < size) size = askedsize; // shortened request + if(size < USB_EP0BUFSZ){ + EP_WriteIRQ(0, buf, size); + return; + } + while(size){ + uint16_t l = size; + if(l > USB_EP0BUFSZ) l = USB_EP0BUFSZ; + EP_WriteIRQ(0, buf, l); + buf += l; + size -= l; + uint8_t needzlp = (l == USB_EP0BUFSZ) ? 1 : 0; + if(size || needzlp){ // send last data buffer + uint16_t epstatus = KEEP_DTOG(USB->EPnR[0]); + // keep DTOGs, clear CTR_RX,TX, set TX VALID, leave stat_Rx + USB->EPnR[0] = (epstatus & ~(USB_EPnR_CTR_RX|USB_EPnR_CTR_TX|USB_EPnR_STAT_RX)) + ^ USB_EPnR_STAT_TX; + uint32_t ctr = 1000000; + while(--ctr && (USB->ISTR & USB_ISTR_CTR) == 0){IWDG->KR = IWDG_REFRESH;}; + if((USB->ISTR & USB_ISTR_CTR) == 0){ + return; + } + if(needzlp) EP_WriteIRQ(0, NULL, 0); + } + } +} + +void get_descriptor(config_pack_t *pack){ + uint8_t descrtype = pack->wValue >> 8, + descridx = pack->wValue & 0xff; + switch(descrtype){ + case DEVICE_DESCRIPTOR: + wr0(USB_DeviceDescriptor, sizeof(USB_DeviceDescriptor), pack->wLength); + break; + case CONFIGURATION_DESCRIPTOR: + wr0(USB_ConfigDescriptor, sizeof(USB_ConfigDescriptor), pack->wLength); + break; + case STRING_DESCRIPTOR: + if(descridx < iDESCR_AMOUNT){ + wr0((const uint8_t *)StringDescriptor[descridx], *((uint8_t*)StringDescriptor[descridx]), pack->wLength); + }else{ + EP_WriteIRQ(0, NULL, 0); + } + break; + case DEVICE_QUALIFIER_DESCRIPTOR: + wr0(USB_DeviceQualifierDescriptor, sizeof(USB_DeviceQualifierDescriptor), pack->wLength); + break; + /* case HID_REPORT_DESCRIPTOR: + wr0(HID_ReportDescriptor, sizeof(HID_ReportDescriptor), pack->wLength); + break;*/ + default: + break; + } +} diff --git a/F3:F303/MLX90640-allsky/usb_descr.h b/F3:F303/MLX90640-allsky/usb_descr.h new file mode 100644 index 0000000..d260af2 --- /dev/null +++ b/F3:F303/MLX90640-allsky/usb_descr.h @@ -0,0 +1,62 @@ +/* + * Copyright 2024 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 + +#include "usb_lib.h" + +// definition of parts common for USB_DeviceDescriptor & USB_DeviceQualifierDescriptor +// bcdUSB: 1.10 +#define bcdUSB 0x0110 +// Class - Misc (EF), subclass - common (2), protocol - interface association descr (1) +#define bDeviceSubClass 0x02 +#define bDeviceProtocol 0x01 +#define idVendor 0x0483 +#define idProduct 0x5740 +#define bcdDevice_Ver 0x0200 +#define bNumConfigurations 1 + +// amount of interfaces and endpoints (except 0) used +#define bNumInterfaces 2 +#define bTotNumEndpoints 3 +#define bNumCsInterfaces 4 + +// powered +#define BusPowered (1<<7) +#define SelfPowered (1<<6) +#define RemoteWakeup (1<<5) + +// buffer sizes +// for USB FS EP0 buffers are from 8 to 64 bytes long +#define USB_EP0BUFSZ 64 +#define USB_EP1BUFSZ 10 +// Rx/Tx EPs +#define USB_RXBUFSZ 64 +#define USB_TXBUFSZ 64 + +// string descriptors +enum{ + iLANGUAGE_DESCR, + iMANUFACTURER_DESCR, + iPRODUCT_DESCR, + iSERIAL_DESCR, + iINTERFACE_DESCR1, + iDESCR_AMOUNT +}; + +void get_descriptor(config_pack_t *pack); diff --git a/F3:F303/MLX90640-allsky/usb_dev.c b/F3:F303/MLX90640-allsky/usb_dev.c new file mode 100644 index 0000000..8e519e4 --- /dev/null +++ b/F3:F303/MLX90640-allsky/usb_dev.c @@ -0,0 +1,240 @@ +/* + * Copyright 2024 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 "ringbuffer.h" +#include "usb_descr.h" +#include "usb_dev.h" + +// Class-Specific Control Requests +#define SEND_ENCAPSULATED_COMMAND 0x00 // unused +#define GET_ENCAPSULATED_RESPONSE 0x01 // unused +#define SET_COMM_FEATURE 0x02 // unused +#define GET_COMM_FEATURE 0x03 // unused +#define CLEAR_COMM_FEATURE 0x04 // unused +#define SET_LINE_CODING 0x20 +#define GET_LINE_CODING 0x21 +#define SET_CONTROL_LINE_STATE 0x22 +#define SEND_BREAK 0x23 + +// control line states +#define CONTROL_DTR 0x01 +#define CONTROL_RTS 0x02 + +// inbuf overflow when receiving +static volatile uint8_t bufovrfl = 0; + +// receive buffer: hold data until chkin() call +static uint8_t volatile rcvbuf[USB_RXBUFSZ]; +static uint8_t volatile rcvbuflen = 0; +// line coding +usb_LineCoding WEAK lineCoding = {115200, 0, 0, 8}; +// CDC configured and ready to use +volatile uint8_t CDCready = 0; + +// ring buffers for incoming and outgoing data +static uint8_t obuf[RBOUTSZ], ibuf[RBINSZ]; +static volatile ringbuffer rbout = {.data = obuf, .length = RBOUTSZ, .head = 0, .tail = 0}; +static volatile ringbuffer rbin = {.data = ibuf, .length = RBINSZ, .head = 0, .tail = 0}; +// last send data size +static volatile int lastdsz = 0; + +static void chkin(){ + if(bufovrfl) return; // allow user to know that previous buffer was overflowed and cleared + if(!rcvbuflen) return; + int w = RB_write((ringbuffer*)&rbin, (uint8_t*)rcvbuf, rcvbuflen); + if(w < 0){ + return; + } + if(w != rcvbuflen) bufovrfl = 1; + rcvbuflen = 0; + uint16_t status = KEEP_DTOG(USB->EPnR[1]); // don't change DTOG + USB->EPnR[1] = (status & ~(USB_EPnR_STAT_TX|USB_EPnR_CTR_RX)) ^ USB_EPnR_STAT_RX; // prepare to get next data portion +} + +// called from transmit EP to send next data portion or by user - when new transmission starts +static void send_next(){ + uint8_t usbbuff[USB_TXBUFSZ]; + int buflen = RB_read((ringbuffer*)&rbout, (uint8_t*)usbbuff, USB_TXBUFSZ); + if(buflen == 0){ + if(lastdsz == 64) EP_Write(1, NULL, 0); // send ZLP after 64 bits packet when nothing more to send + lastdsz = 0; + return; + }else if(buflen < 0){ + lastdsz = 0; + return; + } + EP_Write(1, (uint8_t*)usbbuff, buflen); + lastdsz = buflen; +} + +// data IN/OUT handler +static void rxtx_handler(){ + uint16_t epstatus = KEEP_DTOG(USB->EPnR[1]); + if(RX_FLAG(epstatus)){ // receive data + if(rcvbuflen){ + bufovrfl = 1; // lost last data + rcvbuflen = 0; + } + rcvbuflen = EP_Read(1, (uint8_t*)rcvbuf); + USB->EPnR[1] = epstatus & ~(USB_EPnR_CTR_RX | USB_EPnR_STAT_RX | USB_EPnR_STAT_TX); // keep RX in STALL state until read data + chkin(); // try to write current data into RXbuf if it's not busy + }else{ // tx successfull + USB->EPnR[1] = (epstatus & ~(USB_EPnR_CTR_TX | USB_EPnR_STAT_TX)) ^ USB_EPnR_STAT_RX; + send_next(); + } +} + +// weak handlers: change them somewhere else if you want to setup USART +// SET_LINE_CODING +void WEAK linecoding_handler(usb_LineCoding *lc){ + lineCoding = *lc; +} + +// SET_CONTROL_LINE_STATE +void WEAK clstate_handler(uint16_t val){ + CDCready = val; // CONTROL_DTR | CONTROL_RTS -> interface connected; 0 -> disconnected +} + +// SEND_BREAK +void WEAK break_handler(){ + CDCready = 0; +} + + +// USB is configured: setup endpoints +void set_configuration(){ + EP_Init(1, EP_TYPE_BULK, USB_TXBUFSZ, USB_RXBUFSZ, rxtx_handler); // IN1 and OUT1 +} + +// PL2303 CLASS request +void usb_class_request(config_pack_t *req, uint8_t *data, uint16_t datalen){ + uint8_t recipient = REQUEST_RECIPIENT(req->bmRequestType); + uint8_t dev2host = (req->bmRequestType & 0x80) ? 1 : 0; + switch(recipient){ + case REQ_RECIPIENT_INTERFACE: + switch(req->bRequest){ + case SET_LINE_CODING: + if(!data || !datalen) break; // wait for data + if(datalen == sizeof(usb_LineCoding)) + linecoding_handler((usb_LineCoding*)data); + break; + case GET_LINE_CODING: + EP_WriteIRQ(0, (uint8_t*)&lineCoding, sizeof(lineCoding)); + break; + case SET_CONTROL_LINE_STATE: + clstate_handler(req->wValue); + break; + case SEND_BREAK: + break_handler(); + break; + default: + break; + } + break; + default: + if(dev2host) EP_WriteIRQ(0, NULL, 0); + } + if(!dev2host) EP_WriteIRQ(0, NULL, 0); +} + +// blocking send full content of ring buffer +int USB_sendall(){ + while(lastdsz > 0){ + if(!CDCready) return FALSE; + } + return TRUE; +} + +// put `buf` into queue to send +int USB_send(const uint8_t *buf, int len){ + if(!buf || !CDCready || !len) return FALSE; + while(len){ + int a = RB_write((ringbuffer*)&rbout, buf, len); + if(a > 0){ + len -= a; + buf += a; + } else if (a < 0) continue; // do nothing if buffer is in reading state + if(lastdsz == 0) send_next(); // need to run manually - all data sent, so no IRQ on IN + } + return TRUE; +} + +int USB_putbyte(uint8_t byte){ + if(!CDCready) return FALSE; + int l = 0; + while((l = RB_write((ringbuffer*)&rbout, &byte, 1)) != 1){ + if(l < 0) continue; + } + if(lastdsz == 0) send_next(); // need to run manually - all data sent, so no IRQ on IN + return TRUE; +} + +int USB_sendstr(const char *string){ + if(!string || !CDCready) return FALSE; + int len = 0; + const char *b = string; + while(*b++) ++len; + if(!len) return FALSE; + return USB_send((const uint8_t*)string, len); +} + +/** + * @brief USB_receive - get binary data from receiving ring-buffer + * @param buf (i) - buffer for received data + * @param len - length of `buf` + * @return amount of received bytes (negative, if overfull happened) + */ +int USB_receive(uint8_t *buf, int len){ + chkin(); + if(bufovrfl){ + while(1 != RB_clearbuf((ringbuffer*)&rbin)); + bufovrfl = 0; + return -1; + } + int sz = RB_read((ringbuffer*)&rbin, buf, len); + if(sz < 0) return 0; // buffer in writting state + return sz; +} + +/** + * @brief USB_receivestr - get string up to '\n' and replace '\n' with 0 + * @param buf - receiving buffer + * @param len - its length + * @return strlen or negative value indicating overflow (if so, string won't be ends with 0 and buffer should be cleared) + */ +int USB_receivestr(char *buf, int len){ + chkin(); + if(bufovrfl){ + while(1 != RB_clearbuf((ringbuffer*)&rbin)); + bufovrfl = 0; + return -1; + } + int l = RB_readto((ringbuffer*)&rbin, '\n', (uint8_t*)buf, len); + if(l < 1){ + if(rbin.length == RB_datalen((ringbuffer*)&rbin)){ // buffer is full but no '\n' found + while(1 != RB_clearbuf((ringbuffer*)&rbin)); + return -1; + } + return 0; + } + if(l == 0) return 0; + buf[l-1] = 0; // replace '\n' with strend + return l; +} + diff --git a/F3:F303/MLX90640-allsky/usb_dev.h b/F3:F303/MLX90640-allsky/usb_dev.h new file mode 100644 index 0000000..b8a0808 --- /dev/null +++ b/F3:F303/MLX90640-allsky/usb_dev.h @@ -0,0 +1,58 @@ +/* + * Copyright 2024 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 +#include "usb_lib.h" + +typedef struct { + uint32_t dwDTERate; + uint8_t bCharFormat; +#define USB_CDC_1_STOP_BITS 0 +#define USB_CDC_1_5_STOP_BITS 1 +#define USB_CDC_2_STOP_BITS 2 + uint8_t bParityType; +#define USB_CDC_NO_PARITY 0 +#define USB_CDC_ODD_PARITY 1 +#define USB_CDC_EVEN_PARITY 2 +#define USB_CDC_MARK_PARITY 3 +#define USB_CDC_SPACE_PARITY 4 + uint8_t bDataBits; +} __attribute__ ((packed)) usb_LineCoding; + +extern usb_LineCoding lineCoding; +extern volatile uint8_t CDCready; + +void break_handler(); +void clstate_handler(uint16_t val); +void linecoding_handler(usb_LineCoding *lc); + + +// sizes of ringbuffers for outgoing and incoming data +#define RBOUTSZ (1024) +#define RBINSZ (128) + +#define newline() USB_putbyte('\n') +#define USND(s) do{USB_sendstr(s); USB_putbyte('\n');}while(0) +#define U(s) USB_sendstr(s) + +int USB_sendall(); +int USB_send(const uint8_t *buf, int len); +int USB_putbyte(uint8_t byte); +int USB_sendstr(const char *string); +int USB_receive(uint8_t *buf, int len); +int USB_receivestr(char *buf, int len); diff --git a/F3:F303/MLX90640-allsky/usb_lib.c b/F3:F303/MLX90640-allsky/usb_lib.c new file mode 100644 index 0000000..e390b20 --- /dev/null +++ b/F3:F303/MLX90640-allsky/usb_lib.c @@ -0,0 +1,368 @@ +/* + * Copyright 2024 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 "usb_lib.h" +#include "usb_descr.h" +#include "usb_dev.h" + +static ep_t endpoints[STM32ENDPOINTS]; + +static uint16_t USB_Addr = 0; +static uint8_t setupdatabuf[EP0DATABUF_SIZE]; +static config_pack_t *setup_packet = (config_pack_t*) setupdatabuf; +volatile uint8_t usbON = 0; // device is configured and active + +static uint16_t configuration = 0; // reply for GET_CONFIGURATION (==1 if configured) +static inline void std_d2h_req(){ + uint16_t st = 0; + switch(setup_packet->bRequest){ + case GET_DESCRIPTOR: + get_descriptor(setup_packet); + break; + case GET_STATUS: + EP_WriteIRQ(0, (uint8_t *)&st, 2); // send status: Bus Powered + break; + case GET_CONFIGURATION: + EP_WriteIRQ(0, (uint8_t*)&configuration, 1); + break; + default: + EP_WriteIRQ(0, NULL, 0); + break; + } +} + +static inline void std_h2d_req(){ + switch(setup_packet->bRequest){ + case SET_ADDRESS: + // new address will be assigned later - after acknowlegement or request to host + USB_Addr = setup_packet->wValue; + break; + case SET_CONFIGURATION: + // Now device configured + configuration = setup_packet->wValue; + set_configuration(); + usbON = 1; + break; + default: + break; + } +} + +void WEAK usb_standard_request(){ + uint8_t recipient = REQUEST_RECIPIENT(setup_packet->bmRequestType); + uint8_t dev2host = (setup_packet->bmRequestType & 0x80) ? 1 : 0; + switch(recipient){ + case REQ_RECIPIENT_DEVICE: + if(dev2host){ + std_d2h_req(); + }else{ + std_h2d_req(); + } + break; + case REQ_RECIPIENT_INTERFACE: + if(dev2host && setup_packet->bRequest == GET_DESCRIPTOR){ + get_descriptor(setup_packet); + } + break; + case REQ_RECIPIENT_ENDPOINT: + if(setup_packet->bRequest == CLEAR_FEATURE){ + }else{ /* wrong */ } + break; + default: + break; + } + if(!dev2host) EP_WriteIRQ(0, NULL, 0); +} + +void WEAK usb_class_request(config_pack_t *req, uint8_t _U_ *data, uint16_t _U_ datalen){ + switch(req->bRequest){ + case GET_INTERFACE: + break; + case SET_CONFIGURATION: // set featuring by req->wValue + break; + default: + break; + } + if(0 == (setup_packet->bmRequestType & 0x80)) // host2dev + EP_WriteIRQ(0, NULL, 0); +} + +void WEAK usb_vendor_request(config_pack_t _U_ *packet, uint8_t _U_ *data, uint16_t _U_ datalen){ + if(0 == (setup_packet->bmRequestType & 0x80)) // host2dev + EP_WriteIRQ(0, NULL, 0); +} + +/* +bmRequestType: 76543210 +7 direction: 0 - host->device, 1 - device->host +65 type: 0 - standard, 1 - class, 2 - vendor +4..0 getter: 0 - device, 1 - interface, 2 - endpoint, 3 - other +*/ +/** + * Endpoint0 (control) handler + */ +static void EP0_Handler(){ + uint8_t ep0dbuflen = 0; + uint8_t ep0databuf[EP0DATABUF_SIZE]; + uint16_t epstatus = KEEP_DTOG(USB->EPnR[0]); // EP0R on input -> return this value after modifications + int rxflag = RX_FLAG(epstatus); + //if(rxflag){ } + // check direction + if(USB->ISTR & USB_ISTR_DIR){ // OUT interrupt - receive data, CTR_RX==1 (if CTR_TX == 1 - two pending transactions: receive following by transmit) + if(epstatus & USB_EPnR_SETUP){ // setup packet -> copy data to conf_pack + EP_Read(0, setupdatabuf); + // interrupt handler will be called later + }else if(epstatus & USB_EPnR_CTR_RX){ // data packet -> push received data to ep0databuf + //if(endpoints[0].rx_cnt){ } + ep0dbuflen = EP_Read(0, ep0databuf); + } + } + if(rxflag){ + uint8_t reqtype = REQUEST_TYPE(setup_packet->bmRequestType); + switch(reqtype){ + case REQ_TYPE_STANDARD: + if(SETUP_FLAG(epstatus)){ + usb_standard_request(); + }else{ } + break; + case REQ_TYPE_CLASS: + usb_class_request(setup_packet, ep0databuf, ep0dbuflen); + break; + case REQ_TYPE_VENDOR: + usb_vendor_request(setup_packet, ep0databuf, ep0dbuflen); + break; + default: + EP_WriteIRQ(0, NULL, 0); + break; + } + } + if(TX_FLAG(epstatus)){ + // now we can change address after enumeration + if ((USB->DADDR & USB_DADDR_ADD) != USB_Addr){ + USB->DADDR = USB_DADDR_EF | USB_Addr; + usbON = 0; + } + } + //epstatus = KEEP_DTOG(USB->EPnR[0]); + if(rxflag) epstatus ^= USB_EPnR_STAT_TX; // start ZLP or data transmission + else epstatus &= ~USB_EPnR_STAT_TX; // or leave unchanged + // keep DTOGs, clear CTR_RX,TX, set RX VALID + USB->EPnR[0] = (epstatus & ~(USB_EPnR_CTR_RX|USB_EPnR_CTR_TX)) ^ USB_EPnR_STAT_RX; +} + +/** + * Write data to EP buffer (called from IRQ handler) + * @param number - EP number + * @param *buf - array with data + * @param size - its size + */ +void EP_WriteIRQ(uint8_t number, const uint8_t *buf, uint16_t size){ + if(size > endpoints[number].txbufsz) size = endpoints[number].txbufsz; + uint16_t N2 = (size + 1) >> 1; + // the buffer is 16-bit, so we should copy data as it would be uint16_t + uint16_t *buf16 = (uint16_t *)buf; +#if defined USB1_16 + // very bad: what if `size` is odd? + uint32_t *out = (uint32_t *)endpoints[number].tx_buf; + for(int i = 0; i < N2; ++i, ++out){ + *out = buf16[i]; + } +#elif defined USB2_16 + // use mememcpy instead? + for(int i = 0; i < N2; i++){ + endpoints[number].tx_buf[i] = buf16[i]; + } +#else +#error "Define USB1_16 or USB2_16" +#endif + USB_BTABLE->EP[number].USB_COUNT_TX = size; +} + +/** + * Write data to EP buffer (called outside IRQ handler) + * @param number - EP number + * @param *buf - array with data + * @param size - its size + */ +void EP_Write(uint8_t number, const uint8_t *buf, uint16_t size){ + EP_WriteIRQ(number, buf, size); + uint16_t epstatus = KEEP_DTOG(USB->EPnR[number]); + // keep DTOGs and RX stat, clear CTR_TX & set TX VALID to start transmission + USB->EPnR[number] = (epstatus & ~(USB_EPnR_CTR_TX | USB_EPnR_STAT_RX)) ^ USB_EPnR_STAT_TX; +} + +/* + * Copy data from EP buffer into user buffer area + * @param *buf - user array for data + * @return amount of data read + */ +int EP_Read(uint8_t number, uint8_t *buf){ + int sz = endpoints[number].rx_cnt; + if(!sz) return 0; + endpoints[number].rx_cnt = 0; +#if defined USB1_16 + int n = (sz + 1) >> 1; + uint32_t *in = (uint32_t*)endpoints[number].rx_buf; + uint16_t *out = (uint16_t*)buf; + for(int i = 0; i < n; ++i, ++in) + out[i] = *(uint16_t*)in; +#elif defined USB2_16 + // use mememcpy instead? + for(int i = 0; i < sz; ++i) + buf[i] = endpoints[number].rx_buf[i]; +#else +#error "Define USB1_16 or USB2_16" +#endif + return sz; +} + + +static uint16_t lastaddr = LASTADDR_DEFAULT; +/** + * Endpoint initialisation + * @param number - EP num (0...7) + * @param type - EP type (EP_TYPE_BULK, EP_TYPE_CONTROL, EP_TYPE_ISO, EP_TYPE_INTERRUPT) + * @param txsz - transmission buffer size @ USB/CAN buffer + * @param rxsz - reception buffer size @ USB/CAN buffer + * @param uint16_t (*func)(ep_t *ep) - EP handler function + * @return 0 if all OK + */ +int EP_Init(uint8_t number, uint8_t type, uint16_t txsz, uint16_t rxsz, void (*func)(ep_t ep)){ + if(number >= STM32ENDPOINTS) return 4; // out of configured amount + if(txsz > USB_BTABLE_SIZE/ACCESSZ || rxsz > USB_BTABLE_SIZE/ACCESSZ) return 1; // buffer too large + if(lastaddr + txsz + rxsz >= USB_BTABLE_SIZE/ACCESSZ) return 2; // out of btable + USB->EPnR[number] = (type << 9) | (number & USB_EPnR_EA); + USB->EPnR[number] ^= USB_EPnR_STAT_RX | USB_EPnR_STAT_TX_1; + if(rxsz & 1) return 3; // wrong rx buffer size + uint16_t countrx = 0; + if(rxsz < 64) countrx = rxsz / 2; + else{ + if(rxsz & 0x1f) return 3; // should be multiple of 32 + countrx = 31 + rxsz / 32; + } + USB_BTABLE->EP[number].USB_ADDR_TX = lastaddr; + endpoints[number].tx_buf = (uint16_t *)(USB_BTABLE_BASE + lastaddr * ACCESSZ); + endpoints[number].txbufsz = txsz; + lastaddr += txsz; + USB_BTABLE->EP[number].USB_COUNT_TX = 0; + USB_BTABLE->EP[number].USB_ADDR_RX = lastaddr; + endpoints[number].rx_buf = (uint8_t *)(USB_BTABLE_BASE + lastaddr * ACCESSZ); + lastaddr += rxsz; + USB_BTABLE->EP[number].USB_COUNT_RX = countrx << 10; + endpoints[number].func = func; + return 0; +} + +// standard IRQ handler +void USB_IRQ(){ + uint32_t CNTR = USB->CNTR; + USB->CNTR = 0; + if(USB->ISTR & USB_ISTR_RESET){ + usbON = 0; + // Reinit registers + CNTR = USB_CNTR_RESETM | USB_CNTR_CTRM | USB_CNTR_SUSPM; + // Endpoint 0 - CONTROL + // ON USB LS size of EP0 may be 8 bytes, but on FS it should be 64 bytes! + lastaddr = LASTADDR_DEFAULT; + // clear address, leave only enable bit + USB->DADDR = USB_DADDR_EF; + USB->ISTR = ~USB_ISTR_RESET; + if(EP_Init(0, EP_TYPE_CONTROL, USB_EP0BUFSZ, USB_EP0BUFSZ, EP0_Handler)){ + return; + }; + } + if(USB->ISTR & USB_ISTR_CTR){ + // EP number + uint8_t n = USB->ISTR & USB_ISTR_EPID; + // copy received bytes amount + endpoints[n].rx_cnt = USB_BTABLE->EP[n].USB_COUNT_RX & 0x3FF; // low 10 bits is counter + // call EP handler + if(endpoints[n].func) endpoints[n].func(); + } + if(USB->ISTR & USB_ISTR_WKUP){ // wakeup +#ifndef STM32F0 + CNTR &= ~(USB_CNTR_FSUSP | USB_CNTR_LP_MODE | USB_CNTR_WKUPM); // clear suspend flags +#else + CNTR &= ~(USB_CNTR_FSUSP | USB_CNTR_LPMODE | USB_CNTR_WKUPM); +#endif + USB->ISTR = ~USB_ISTR_WKUP; + } + if(USB->ISTR & USB_ISTR_SUSP){ // suspend -> still no connection, may sleep + usbON = 0; +#ifndef STM32F0 + CNTR |= USB_CNTR_FSUSP | USB_CNTR_LP_MODE | USB_CNTR_WKUPM; +#else + CNTR |= USB_CNTR_FSUSP | USB_CNTR_LPMODE | USB_CNTR_WKUPM; +#endif + CNTR &= ~(USB_CNTR_SUSPM); + USB->ISTR = ~USB_ISTR_SUSP; + } + USB->CNTR = CNTR; // rewoke interrupts +} + +// here we suppose that all PIN settings done in hw_setup earlier +void USB_setup(){ +#if defined STM32F3 + NVIC_DisableIRQ(USB_LP_IRQn); + // remap USB LP & Wakeup interrupts to 75 and 76 - works only on pure F303 + RCC->APB2ENR |= RCC_APB2ENR_SYSCFGEN; // enable tacting of SYSCFG + SYSCFG->CFGR1 |= SYSCFG_CFGR1_USB_IT_RMP; +#elif defined STM32F1 + NVIC_DisableIRQ(USB_LP_CAN1_RX0_IRQn); + NVIC_DisableIRQ(USB_HP_CAN1_TX_IRQn); +#elif defined STM32F0 + NVIC_DisableIRQ(USB_IRQn); + RCC->APB1ENR |= RCC_APB1ENR_CRSEN; + RCC->CFGR3 &= ~RCC_CFGR3_USBSW; // reset USB + RCC->CR2 |= RCC_CR2_HSI48ON; // turn ON HSI48 + uint32_t tmout = 16000000; + while(!(RCC->CR2 & RCC_CR2_HSI48RDY)){if(--tmout == 0) break;} + FLASH->ACR = FLASH_ACR_PRFTBE | FLASH_ACR_LATENCY; + CRS->CFGR &= ~CRS_CFGR_SYNCSRC; + CRS->CFGR |= CRS_CFGR_SYNCSRC_1; // USB SOF selected as sync source + CRS->CR |= CRS_CR_AUTOTRIMEN; // enable auto trim + CRS->CR |= CRS_CR_CEN; // enable freq counter & block CRS->CFGR as read-only + RCC->CFGR |= RCC_CFGR_SW; +#endif + RCC->APB1ENR |= RCC_APB1ENR_USBEN; + //?? + USB->CNTR = USB_CNTR_FRES; // Force USB Reset + for(uint32_t ctr = 0; ctr < 72000; ++ctr) nop(); // wait >1ms + USB->CNTR = 0; + USB->BTABLE = 0; + USB->DADDR = 0; + USB->ISTR = 0; + USB->CNTR = USB_CNTR_RESETM; // allow only reset interrupts +#if defined STM32F3 + NVIC_EnableIRQ(USB_LP_IRQn); +#elif defined STM32F1 + NVIC_EnableIRQ(USB_LP_CAN1_RX0_IRQn); +#elif defined STM32F0 + USB->BCDR |= USB_BCDR_DPPU; + NVIC_EnableIRQ(USB_IRQn); +#endif +} + + +#if defined STM32F3 +void usb_lp_isr() __attribute__ ((alias ("USB_IRQ"))); +#elif defined STM32F1 +void usb_lp_can_rx0_isr() __attribute__ ((alias ("USB_IRQ"))); +#elif defined STM32F0 +void usb_isr() __attribute__ ((alias ("USB_IRQ"))); +#endif diff --git a/F3:F303/MLX90640-allsky/usb_lib.h b/F3:F303/MLX90640-allsky/usb_lib.h new file mode 100644 index 0000000..d55c83f --- /dev/null +++ b/F3:F303/MLX90640-allsky/usb_lib.h @@ -0,0 +1,328 @@ +/* + * Copyright 2024 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 +#include + +#ifndef _U_ +#define _U_ __attribute__((unused)) +#endif + +/****************************************************************** + * Hardware registers etc * + *****************************************************************/ +#if defined STM32F0 +#include +#elif defined STM32F1 +#include +// there's no this define in standard header +#define USB_BASE ((uint32_t)0x40005C00) +#elif defined STM32F3 +#include +#endif + +// max endpoints number +#define STM32ENDPOINTS 8 +/** + * Buffers size definition + **/ + +// F0 - USB2_16; F1 - USB1_16; F3 - 1/2 depending on series +#if !defined USB1_16 && !defined USB2_16 +#if defined STM32F0 +#define USB2_16 +#elif defined STM32F1 +#define USB1_16 +#else +#error "Can't determine USB1_16 or USB2_16, define by hands" +#endif +#endif + +// BTABLE_SIZE FOR STM32F3: +// In STM32F303/302xB/C, 512 bytes SRAM is not shared with CAN. +// In STM32F302x6/x8 and STM32F30xxD/E, 726 bytes dedicated SRAM and 256 bytes shared SRAM with CAN i.e. +// 1Kbytes dedicated SRAM in case CAN is disabled. +// remember, that USB_BTABLE_SIZE will be divided by ACCESSZ, so don't divide it twice for 32-bit addressing + +#ifdef NOCAN +#if defined STM32F0 +#define USB_BTABLE_SIZE 1024 +#elif defined STM32F3 +#define USB_BTABLE_SIZE 726 +//#warning "Please, check real buffer size due to docs" +#else +#error "define STM32F0 or STM32F3" +#endif +#else // !NOCAN: F0/F3 with CAN or F1 (can't simultaneously run CAN and USB) +#if defined STM32F0 +#define USB_BTABLE_SIZE 768 +#elif defined STM32F3 +#define USB_BTABLE_SIZE 726 +//#warning "Please, check real buffer size due to docs" +#else // STM32F103: 1024 bytes but with 32-bit addressing +#define USB_BTABLE_SIZE 1024 +#endif +#endif // NOCAN + +// first 64 bytes of USB_BTABLE are registers! + +#define USB_BTABLE_BASE 0x40006000 +#define USB ((USB_TypeDef *) USB_BASE) + +#ifdef USB_BTABLE +#undef USB_BTABLE +#endif +#define USB_BTABLE ((USB_BtableDef *)(USB_BTABLE_BASE)) +#define USB_ISTR_EPID 0x0000000F +#define USB_FNR_LSOF_0 0x00000800 +#define USB_FNR_lSOF_1 0x00001000 +#define USB_LPMCSR_BESL_0 0x00000010 +#define USB_LPMCSR_BESL_1 0x00000020 +#define USB_LPMCSR_BESL_2 0x00000040 +#define USB_LPMCSR_BESL_3 0x00000080 +#define USB_EPnR_CTR_RX 0x00008000 +#define USB_EPnR_DTOG_RX 0x00004000 +#define USB_EPnR_STAT_RX 0x00003000 +#define USB_EPnR_STAT_RX_0 0x00001000 +#define USB_EPnR_STAT_RX_1 0x00002000 +#define USB_EPnR_SETUP 0x00000800 +#define USB_EPnR_EP_TYPE 0x00000600 +#define USB_EPnR_EP_TYPE_0 0x00000200 +#define USB_EPnR_EP_TYPE_1 0x00000400 +#define USB_EPnR_EP_KIND 0x00000100 +#define USB_EPnR_CTR_TX 0x00000080 +#define USB_EPnR_DTOG_TX 0x00000040 +#define USB_EPnR_STAT_TX 0x00000030 +#define USB_EPnR_STAT_TX_0 0x00000010 +#define USB_EPnR_STAT_TX_1 0x00000020 +#define USB_EPnR_EA 0x0000000F +#define USB_COUNTn_RX_BLSIZE 0x00008000 +#define USB_COUNTn_NUM_BLOCK 0x00007C00 +#define USB_COUNTn_RX 0x0000003F + +#define USB_TypeDef USB_TypeDef_custom + +typedef struct { + __IO uint32_t EPnR[STM32ENDPOINTS]; + __IO uint32_t RESERVED[STM32ENDPOINTS]; + __IO uint32_t CNTR; + __IO uint32_t ISTR; + __IO uint32_t FNR; + __IO uint32_t DADDR; + __IO uint32_t BTABLE; +#ifdef STM32F0 + __IO uint32_t LPMCSR; + __IO uint32_t BCDR; +#endif +} USB_TypeDef; + +// F303 D/E have 2x16 access scheme +typedef struct{ +#if defined USB2_16 + __IO uint16_t USB_ADDR_TX; + __IO uint16_t USB_COUNT_TX; + __IO uint16_t USB_ADDR_RX; + __IO uint16_t USB_COUNT_RX; +#define ACCESSZ (1) +#define BUFTYPE uint8_t +#elif defined USB1_16 + __IO uint32_t USB_ADDR_TX; + __IO uint32_t USB_COUNT_TX; + __IO uint32_t USB_ADDR_RX; + __IO uint32_t USB_COUNT_RX; +#define ACCESSZ (2) +#define BUFTYPE uint16_t +#else +#error "Define USB1_16 or USB2_16" +#endif +} USB_EPDATA_TypeDef; + + +typedef struct{ + __IO USB_EPDATA_TypeDef EP[STM32ENDPOINTS]; +} USB_BtableDef; + +#define EP0DATABUF_SIZE (64) +#define LASTADDR_DEFAULT (STM32ENDPOINTS * 8) + +/****************************************************************** + * Defines from usb.h * + *****************************************************************/ + +/* + * Device and/or Interface Class codes + */ +#define USB_CLASS_PER_INTERFACE 0 +#define USB_CLASS_AUDIO 1 +#define USB_CLASS_COMM 2 +#define USB_CLASS_HID 3 +#define USB_CLASS_PRINTER 7 +#define USB_CLASS_PTP 6 +#define USB_CLASS_MASS_STORAGE 8 +#define USB_CLASS_HUB 9 +#define USB_CLASS_DATA 10 +#define USB_CLASS_MISC 0xef +#define USB_CLASS_VENDOR_SPEC 0xff + +/* + * Descriptor types + */ +#define USB_DT_DEVICE 0x01 +#define USB_DT_CONFIG 0x02 +#define USB_DT_STRING 0x03 +#define USB_DT_INTERFACE 0x04 +#define USB_DT_ENDPOINT 0x05 +#define USB_DT_QUALIFIER 0x06 +#define USB_DT_IAD 0x0B + +#define USB_DT_HID 0x21 +#define USB_DT_REPORT 0x22 +#define USB_DT_PHYSICAL 0x23 +#define USB_DT_CS_INTERFACE 0x24 +#define USB_DT_HUB 0x29 + +/* + * Descriptor sizes per descriptor type + */ +#define USB_DT_DEVICE_SIZE 18 +#define USB_DT_CONFIG_SIZE 9 +#define USB_DT_INTERFACE_SIZE 9 +#define USB_DT_HID_SIZE 9 +#define USB_DT_ENDPOINT_SIZE 7 +#define USB_DT_QUALIFIER_SIZE 10 +#define USB_DT_CS_INTERFACE_SIZE 5 +#define USB_DT_IAD_SIZE 8 + + +// bmRequestType & 0x80 == dev2host (1) or host2dev (0) +// recipient: bmRequestType & 0x1f +#define REQUEST_RECIPIENT(b) (b & 0x1f) +#define REQ_RECIPIENT_DEVICE 0 +#define REQ_RECIPIENT_INTERFACE 1 +#define REQ_RECIPIENT_ENDPOINT 2 +#define REQ_RECIPIENT_OTHER 3 +// type: [bmRequestType & 0x60 >> 5] +#define REQUEST_TYPE(b) ((b&0x60)>>5) +#define REQ_TYPE_STANDARD 0 +#define REQ_TYPE_CLASS 1 +#define REQ_TYPE_VENDOR 2 +#define REQ_TYPE_RESERVED 3 + + +//#define VENDOR_REQUEST 0x01 + +// standard device requests +#define GET_STATUS 0x00 +#define CLEAR_FEATURE 0x01 +#define SET_FEATURE 0x03 +#define SET_ADDRESS 0x05 +#define GET_DESCRIPTOR 0x06 +#define SET_DESCRIPTOR 0x07 +#define GET_CONFIGURATION 0x08 +#define SET_CONFIGURATION 0x09 +// and some standard interface requests +#define GET_INTERFACE 0x0A +#define SET_INTERFACE 0x0B +// and some standard endpoint requests +#define SYNC_FRAME 0x0C + +// Types of descriptors +#define DEVICE_DESCRIPTOR 0x01 +#define CONFIGURATION_DESCRIPTOR 0x02 +#define STRING_DESCRIPTOR 0x03 +#define DEVICE_QUALIFIER_DESCRIPTOR 0x06 +#define DEBUG_DESCRIPTOR 0x0a +#define HID_REPORT_DESCRIPTOR 0x22 + +// EP types for EP_init +#define EP_TYPE_BULK 0x00 +#define EP_TYPE_CONTROL 0x01 +#define EP_TYPE_ISO 0x02 +#define EP_TYPE_INTERRUPT 0x03 + +// EP types for descriptors +#define USB_BM_ATTR_CONTROL 0x00 +#define USB_BM_ATTR_ISO 0x01 +#define USB_BM_ATTR_BULK 0x02 +#define USB_BM_ATTR_INTERRUPT 0x03 + + +/****************************************************************** + * Other stuff * + *****************************************************************/ + +#define RX_FLAG(epstat) (epstat & USB_EPnR_CTR_RX) +#define TX_FLAG(epstat) (epstat & USB_EPnR_CTR_TX) +#define SETUP_FLAG(epstat) (epstat & USB_EPnR_SETUP) + +// EPnR bits manipulation +#define KEEP_DTOG_STAT(EPnR) (EPnR & ~(USB_EPnR_STAT_RX|USB_EPnR_STAT_TX|USB_EPnR_DTOG_RX|USB_EPnR_DTOG_TX)) +#define KEEP_DTOG(EPnR) (EPnR & ~(USB_EPnR_DTOG_RX|USB_EPnR_DTOG_TX)) + +#define LANG_US (uint16_t)0x0409 + +#define _USB_STRING_(name, str) \ +static const struct name \ +{ \ + uint8_t bLength; \ + uint8_t bDescriptorType; \ + uint16_t bString[(sizeof(str) - 2) / 2]; \ + \ +} \ +name = {sizeof(name), 0x03, str} + +#define _USB_LANG_ID_(name, lng_id) \ +static const struct name \ +{ \ + uint8_t bLength; \ + uint8_t bDescriptorType; \ + uint16_t bString; \ + \ +} \ +name = {0x04, 0x03, lng_id} + +// EP0 configuration packet +typedef struct { + uint8_t bmRequestType; + uint8_t bRequest; + uint16_t wValue; + uint16_t wIndex; + uint16_t wLength; +} config_pack_t; + +// endpoints state +typedef struct{ + uint16_t *tx_buf; // transmission buffer address + uint16_t txbufsz; // transmission buffer size + uint8_t *rx_buf; // reception buffer address + void (*func)(); // endpoint action function + unsigned rx_cnt : 10; // received data counter +} ep_t; + +extern volatile uint8_t usbON; + +void USB_setup(); +int EP_Init(uint8_t number, uint8_t type, uint16_t txsz, uint16_t rxsz, void (*func)()); +void EP_WriteIRQ(uint8_t number, const uint8_t *buf, uint16_t size); +void EP_Write(uint8_t number, const uint8_t *buf, uint16_t size); +int EP_Read(uint8_t number, uint8_t *buf); + +// could be [re]defined in usb_dev.c +extern void usb_class_request(config_pack_t *packet, uint8_t *data, uint16_t datalen); +extern void usb_vendor_request(config_pack_t *packet, uint8_t *data, uint16_t datalen); +extern void set_configuration(); diff --git a/F3:F303/MLX90640-allsky/version.inc b/F3:F303/MLX90640-allsky/version.inc new file mode 100644 index 0000000..7b1ccba --- /dev/null +++ b/F3:F303/MLX90640-allsky/version.inc @@ -0,0 +1,2 @@ +#define BUILD_NUMBER "1" +#define BUILD_DATE "2025-09-28"