mirror of
https://github.com/eddyem/stm32samples.git
synced 2026-05-07 05:17:03 +03:00
change proto to text strings; add simplest NTC measurement
This commit is contained in:
@@ -8,3 +8,5 @@ LDLIBS := -lm
|
||||
|
||||
include ../makefile.f3
|
||||
include ../../makefile.stm32
|
||||
|
||||
$(OBJDIR)/commproto.o: commproto.cpp $(VERSION_FILE)
|
||||
|
||||
@@ -16,17 +16,26 @@
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifdef EBUG
|
||||
#include "strfunc.h"
|
||||
#endif
|
||||
|
||||
#include "adc.h"
|
||||
|
||||
/**
|
||||
* @brief ADCx_array - arrays for ADC channels with median filtering:
|
||||
* ADC1:
|
||||
* 0 - Ch0 - ADC1_IN1
|
||||
* 1 - Ch1 - ADC1_IN2
|
||||
* 2 - internal Tsens - ADC1_IN16
|
||||
* 3 - Vref - ADC1_IN18
|
||||
* 0 - Ch0 - ADC1_IN1 - NTC1
|
||||
* 1 - Ch1 - ADC1_IN2 - NTC2
|
||||
* 2 - Ch2 - ADC1_IN3 - NTC3
|
||||
* 3 - Ch3 - ADC1_IN4 - NTC4
|
||||
* 4 - internal Tsens - ADC1_IN16
|
||||
* 5 - Vref - ADC1_IN18
|
||||
* ADC2:
|
||||
* 4 - AIN5/DAC_OUT1 - PA4 - DAC1_OUT1 (onboard heater?), PA5 - ADC2_IN2 (DAC output control)
|
||||
* AIN5/DAC_OUT1 - PA4 - DAC1_OUT1 (onboard heater)
|
||||
* 6 - PA5 - ADC2_IN2 (DAC output control)
|
||||
*/
|
||||
static uint16_t ADC_array[NUMBER_OF_ADC_CHANNELS*9];
|
||||
|
||||
@@ -65,17 +74,19 @@ TRUE_INLINE void enADC(ADC_TypeDef *chnl){
|
||||
* ADC1 - DMA1_ch1
|
||||
* ADC2 - DMA2_ch1
|
||||
*/
|
||||
// Setup ADC and DAC
|
||||
// Setup ADC and DAC; ADC/DAC pins should be prepared in gpio_setup
|
||||
void adc_setup(){
|
||||
RCC->AHBENR |= RCC_AHBENR_ADC12EN; // Enable clocking
|
||||
ADC12_COMMON->CCR = ADC_CCR_TSEN | ADC_CCR_VREFEN | ADC_CCR_CKMODE; // enable Tsens and Vref, HCLK/4
|
||||
calADC(ADC1);
|
||||
calADC(ADC2);
|
||||
// ADC1: channels 1,2,16,18; ADC2: channel 2
|
||||
ADC1->SMPR1 = ADC_SMPR1_SMP0 | ADC_SMPR1_SMP1;
|
||||
// ADC1: channels 1,2,3,4,16,18
|
||||
ADC1->SMPR1 = ADC_SMPR1_SMP0 | ADC_SMPR1_SMP1 | ADC_SMPR1_SMP2 | ADC_SMPR1_SMP3;
|
||||
ADC1->SMPR2 = ADC_SMPR2_SMP15 | ADC_SMPR2_SMP17;
|
||||
// 4 conversions in group: 1->2->16->18
|
||||
ADC1->SQR1 = (1<<6) | (2<<12) | (16<<18) | (18<<24) | (NUMBER_OF_ADC1_CHANNELS-1);
|
||||
// 6 conversions in group: 1->2->3->4->16->18
|
||||
ADC1->SQR1 = (1<<6) | (2<<12) | (3<<18) | (4<<24) | (NUMBER_OF_ADC1_CHANNELS-1);
|
||||
ADC1->SQR2 = (16<<0) | (18<<6);
|
||||
// ADC2: channel 2
|
||||
ADC2->SMPR1 = ADC_SMPR1_SMP1;
|
||||
ADC2->SQR1 = (2<<6) | (NUMBER_OF_ADC2_CHANNELS-1);
|
||||
// configure DMA for ADC
|
||||
@@ -109,7 +120,8 @@ void adc_setup(){
|
||||
* @param nch - number of channel
|
||||
* @return
|
||||
*/
|
||||
uint16_t getADCval(int nch){
|
||||
uint16_t getADCval(uint8_t nch){
|
||||
if(nch >= NUMBER_OF_ADC_CHANNELS) return 0;
|
||||
register uint16_t temp;
|
||||
#define PIX_SORT(a,b) { if ((a)>(b)) PIX_SWAP((a),(b)); }
|
||||
#define PIX_SWAP(a,b) { temp=(a);(a)=(b);(b)=temp; }
|
||||
@@ -131,7 +143,7 @@ uint16_t getADCval(int nch){
|
||||
}
|
||||
|
||||
// get voltage @input nch (V)
|
||||
float getADCvoltage(int nch){
|
||||
float getADCvoltage(uint8_t nch){
|
||||
float v = getADCval(nch);
|
||||
v *= getVdd();
|
||||
v /= 4096.f; // 12bit ADC
|
||||
@@ -155,3 +167,114 @@ float getVdd(){
|
||||
vdd /= getADCval(ADC_VREF);
|
||||
return vdd;
|
||||
}
|
||||
|
||||
// R lookup table for T=-10..59 degreesC
|
||||
#if 0
|
||||
T=[-10:59]+273.15;
|
||||
R=1000*exp(3950*(1./T-1/298.15));
|
||||
for i=1:length(T); printf("\t%.1f,\t// %d \n", R(i), T(i)-273.15); endfor
|
||||
#endif
|
||||
|
||||
static const float Rlut[] = {
|
||||
5824.6, // -10
|
||||
5502.8, // -9
|
||||
5201.1, // -8
|
||||
4917.9, // -7
|
||||
4652.2, // -6
|
||||
4402.6, // -5
|
||||
4168.1, // -4
|
||||
3947.7, // -3
|
||||
3740.5, // -2
|
||||
3545.5, // -1
|
||||
3362.1, // 0
|
||||
3189.3, // 1
|
||||
3026.6, // 2
|
||||
2873.3, // 3
|
||||
2728.8, // 4
|
||||
2592.5, // 5
|
||||
2463.9, // 6
|
||||
2342.5, // 7
|
||||
2227.9, // 8
|
||||
2119.7, // 9
|
||||
2017.5, // 10
|
||||
1920.8, // 11
|
||||
1829.4, // 12
|
||||
1743.0, // 13
|
||||
1661.2, // 14
|
||||
1583.7, // 15
|
||||
1510.4, // 16
|
||||
1440.9, // 17
|
||||
1375.1, // 18
|
||||
1312.7, // 19
|
||||
1253.5, // 20
|
||||
1197.4, // 21
|
||||
1144.1, // 22
|
||||
1093.6, // 23
|
||||
1045.6, // 24
|
||||
1000.0, // 25
|
||||
956.7, // 26
|
||||
915.5, // 27
|
||||
876.4, // 28
|
||||
839.1, // 29
|
||||
803.7, // 30
|
||||
770.0, // 31
|
||||
737.9, // 32
|
||||
707.4, // 33
|
||||
678.3, // 34
|
||||
650.6, // 35
|
||||
624.1, // 36
|
||||
598.9, // 37
|
||||
574.9, // 38
|
||||
552.0, // 39
|
||||
530.1, // 40
|
||||
509.3, // 41
|
||||
489.4, // 42
|
||||
470.3, // 43
|
||||
452.2, // 44
|
||||
434.8, // 45
|
||||
418.2, // 46
|
||||
402.4, // 47
|
||||
387.2, // 48
|
||||
372.7, // 49
|
||||
358.8, // 50
|
||||
345.5, // 51
|
||||
332.8, // 52
|
||||
320.7, // 53
|
||||
309.0, // 54
|
||||
297.8, // 55
|
||||
287.1, // 56
|
||||
276.9, // 57
|
||||
267.1, // 58
|
||||
257.7, // 59
|
||||
};
|
||||
|
||||
#define LUTSZ (sizeof(Rlut) / sizeof(float))
|
||||
|
||||
/**
|
||||
* @brief getNTCtemp - stupid LUT-search and linear approximation of T by R
|
||||
* @param nch - channel of ADC for Tx
|
||||
* @return temperature in degr.C
|
||||
*/
|
||||
float getNTCtemp(uint8_t nch){
|
||||
if(nch > ADC_AIN4) return -300.f; // bad number
|
||||
uint16_t val = getADCval(nch);
|
||||
if(val < 5) return -400.f; // short cirquit
|
||||
else if(val > 4090) return -500.f; // no NTC
|
||||
float R = 1000.f / (4096.f / val - 1.f); // resistance of NTC
|
||||
#ifdef EBUG
|
||||
USB_sendstr("R="); USB_sendstr(float2str(R, 1)); newline();
|
||||
#endif
|
||||
int left = 0, right = LUTSZ-1;
|
||||
if(R > Rlut[0]) right = 1;
|
||||
else if(R < Rlut[LUTSZ-1]) left = LUTSZ-2;
|
||||
while(right - left > 1){
|
||||
int idx = left + (right - left) / 2;
|
||||
float Rl = Rlut[idx];
|
||||
if(Rl > R) left = idx + 1;
|
||||
else right = idx - 1;
|
||||
}
|
||||
if(left >= (int)LUTSZ) return 60.f;
|
||||
float Rleft = Rlut[left], Rright = Rlut[left+1];
|
||||
float T = (float)left - 9.f - (R - Rright) / (Rleft - Rright);
|
||||
return T;
|
||||
}
|
||||
|
||||
@@ -19,22 +19,27 @@
|
||||
#pragma once
|
||||
#include <stm32f3.h>
|
||||
|
||||
#define NUMBER_OF_ADC1_CHANNELS (4)
|
||||
// 4 sensors on 1..4, TS (16) and Vdd (18)
|
||||
#define NUMBER_OF_ADC1_CHANNELS (6)
|
||||
#define NUMBER_OF_ADC2_CHANNELS (1)
|
||||
// total number of channels - for array
|
||||
#define NUMBER_OF_ADC_CHANNELS ((NUMBER_OF_ADC1_CHANNELS+NUMBER_OF_ADC2_CHANNELS))
|
||||
|
||||
// channels of ADC in array
|
||||
#define ADC_AIN0 (0)
|
||||
#define ADC_AIN1 (1)
|
||||
#define ADC_TS (2)
|
||||
#define ADC_VREF (3)
|
||||
#define ADC_AIN5 (4)
|
||||
#define ADC_AIN1 (0)
|
||||
#define ADC_AIN2 (1)
|
||||
#define ADC_AIN3 (2)
|
||||
#define ADC_AIN4 (3)
|
||||
#define ADC_NTCIN(x) ((x)-1)
|
||||
#define ADC_TS (4)
|
||||
#define ADC_VREF (5)
|
||||
#define ADC_DACIN (6)
|
||||
// starting index of ADC2
|
||||
#define ADC2START (9*NUMBER_OF_ADC1_CHANNELS)
|
||||
|
||||
void adc_setup();
|
||||
float getMCUtemp();
|
||||
float getVdd();
|
||||
uint16_t getADCval(int nch);
|
||||
float getADCvoltage(int nch);
|
||||
uint16_t getADCval(uint8_t nch);
|
||||
float getADCvoltage(uint8_t nch);
|
||||
float getNTCtemp(uint8_t nch);
|
||||
|
||||
Binary file not shown.
627
F3:F303/MLX90640-allsky/commproto.cpp
Normal file
627
F3:F303/MLX90640-allsky/commproto.cpp
Normal file
@@ -0,0 +1,627 @@
|
||||
#include <cstring>
|
||||
|
||||
extern "C"{
|
||||
#include <stm32f3.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "adc.h"
|
||||
#include "commproto.h"
|
||||
#include "hardware.h"
|
||||
#include "i2c.h"
|
||||
#include "mlxproc.h"
|
||||
#include "strfunc.h"
|
||||
#include "usart.h"
|
||||
#include "usb_dev.h"
|
||||
}
|
||||
|
||||
// image aquisition time
|
||||
const char* const Timage = "TIMAGE";
|
||||
|
||||
// Global senders to send info into other interface
|
||||
static int (*usb_sender)(const char*) = nullptr;
|
||||
static int (*usart_sender)(const char*) = nullptr;
|
||||
static int (*usb_putb)(uint8_t) = nullptr;
|
||||
static int (*usart_putb)(uint8_t) = nullptr;
|
||||
static int (*usb_sendbin)(const uint8_t*, int) = nullptr;
|
||||
static int (*usart_sendbin)(const uint8_t*, int) = nullptr;
|
||||
|
||||
// Function helpers
|
||||
static int (*SEND)(const char*) = nullptr;
|
||||
static int (*putb)(uint8_t) = nullptr;
|
||||
static int (*sendbin)(const uint8_t*, int) = nullptr;
|
||||
|
||||
#define N() putb('\n')
|
||||
#define printu(x) SEND(u2str(x))
|
||||
#define printi(x) SEND(i2str(x))
|
||||
#define printuhex(x) SEND(uhex2str(x))
|
||||
#define printfl(x,n) SEND(float2str(x, n))
|
||||
|
||||
void set_senders(int (*usbs)(const char *),
|
||||
int (*usbb)(uint8_t),
|
||||
int (*usbbin)(const uint8_t *, int),
|
||||
int (*usarts)(const char *),
|
||||
int (*usartb)(uint8_t),
|
||||
int (*usartbin)(const uint8_t *, int)){
|
||||
usb_sender = usbs;
|
||||
usb_putb = usbb;
|
||||
usb_sendbin = usbbin;
|
||||
usart_sender = usarts;
|
||||
usart_putb = usartb;
|
||||
usart_sendbin = usartbin;
|
||||
}
|
||||
|
||||
// Local buffer for I2C data
|
||||
#define LOCBUFFSZ 32
|
||||
static uint16_t locBuffer[LOCBUFFSZ];
|
||||
// default I2C address of sensor
|
||||
static uint8_t I2Caddress = 0x33 << 1;
|
||||
extern volatile uint32_t Tms;
|
||||
// show `cartoon` - continuously draw ASCII image of current sensor
|
||||
uint8_t cartoon = 0;
|
||||
|
||||
// Command list
|
||||
#define COMMAND_TABLE \
|
||||
COMMAND(help, "show this help") \
|
||||
COMMAND(ascii, "draw nth image in ASCII (n=0..4)") \
|
||||
COMMAND(binary, "get nth image as text array of floats") \
|
||||
COMMAND(listids, "list active sensors IDs") \
|
||||
COMMAND(tempmap, "show temperature map of nth image") \
|
||||
COMMAND(acqtime, "show nth image aquisition time") \
|
||||
COMMAND(bmereinit, "reinit BME280") \
|
||||
COMMAND(environ, "get environment parameters") \
|
||||
COMMAND(state, "get MLX state") \
|
||||
COMMAND(reset, "reset MCU") \
|
||||
COMMAND(time, "print current Tms") \
|
||||
COMMAND(iicaddr, "get/set I2C address (non-shifted)") \
|
||||
COMMAND(mlxcont, "continue MLX") \
|
||||
COMMAND(iicspeed, "get/set I2C speed (0..4)") \
|
||||
COMMAND(mlxpause, "pause MLX") \
|
||||
COMMAND(mlxstop, "stop MLX") \
|
||||
COMMAND(adc, "get n'th ADC values") \
|
||||
COMMAND(ntc, "get n'th NTC temperatures") \
|
||||
COMMAND(cartoon, "toggle cartoon mode") \
|
||||
COMMAND(mlxdump, "dump MLX parameters for sensor n") \
|
||||
COMMAND(mlxaddr, "get/set MLX address of sensor n") \
|
||||
COMMAND(readreg, "read I2C register: readreg reg [= nwords]") \
|
||||
COMMAND(writedata, "write I2C data: writedata = val1 val2 ...") \
|
||||
COMMAND(iicscan, "scan I2C bus") \
|
||||
COMMAND(mcutemp, "get MCU temperature") \
|
||||
COMMAND(mcuvdd, "get MCU Vdd") \
|
||||
COMMAND(dac, "get/set DAC value") \
|
||||
COMMAND(pwm, "get/set PWM for channel n (0..100%)") \
|
||||
COMMAND(sendstr, "send string to other interface: sendstr = text")
|
||||
|
||||
|
||||
// Command prototypes
|
||||
#define COMMAND(name, desc) static errcodes_t cmd_ ## name(const char*, char*);
|
||||
COMMAND_TABLE
|
||||
#undef COMMAND
|
||||
|
||||
// descrtiptions for `help`
|
||||
typedef struct {
|
||||
const char *name;
|
||||
const char *desc;
|
||||
} CmdInfo;
|
||||
|
||||
static const CmdInfo cmdInfo[] = {
|
||||
#define COMMAND(name, desc) { #name, desc },
|
||||
COMMAND_TABLE
|
||||
#undef COMMAND
|
||||
};
|
||||
|
||||
// Text descriptions for error codes
|
||||
static const char* errtxt[ERR_AMOUNT] = {
|
||||
[ERR_OK] = "OK\n",
|
||||
[ERR_BADCMD] = "BADCMD\n",
|
||||
[ERR_BADPAR] = "BADPAR\n",
|
||||
[ERR_BADVAL] = "BADVAL\n",
|
||||
[ERR_WRONGLEN] = "WRONGLEN\n",
|
||||
[ERR_CANTRUN] = "CANTRUN\n",
|
||||
[ERR_BUSY] = "BUSY\n",
|
||||
[ERR_OVERFLOW] = "OVERFLOW\n",
|
||||
};
|
||||
|
||||
const char *EQ = " = "; // equal sign for getters
|
||||
|
||||
// send `command = `
|
||||
#define CMDEQ() do{SEND(cmd); SEND(EQ);}while(0)
|
||||
// send `commandXXX = `
|
||||
#define CMDEQP(x) do{SEND(cmd); SEND(u2str((uint32_t)x)); SEND(EQ);}while(0)
|
||||
|
||||
/**
|
||||
* @brief splitargs - get command parameter and setter from `args`
|
||||
* @param args (i) - rest of string after command (like `1 = PU OD OUT`)
|
||||
* @param parno (o) - parameter number or -1 if none
|
||||
* @return setter (part after `=` without leading spaces) or NULL if none
|
||||
*/
|
||||
static const char *splitargs(char *args, int32_t *parno){
|
||||
if(!args) return NULL;
|
||||
uint32_t U32;
|
||||
const char *next = getnum(args, &U32);
|
||||
int p = -1;
|
||||
if(next != args && U32 <= MAXPARNO) p = U32;
|
||||
if(parno) *parno = p;
|
||||
next = strchr(next, '=');
|
||||
if(next){
|
||||
if(*(++next)) next = omit_spaces(next);
|
||||
if(*next == 0) next = NULL;
|
||||
}
|
||||
return next;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief argsvals - split `args` into `parno` and setter's value
|
||||
* @param args - rest of string after command
|
||||
* @param parno (o) - parameter number or -1 if none
|
||||
* @param parval - integer setter's value
|
||||
* @return false if no setter or it's not a number, true - got setter's num
|
||||
*/
|
||||
static bool argsvals(char *args, int32_t *parno, int32_t *parval){
|
||||
const char *setter = splitargs(args, parno);
|
||||
if(!setter) return false;
|
||||
int32_t I32;
|
||||
const char *next = getint(setter, &I32);
|
||||
if(next != setter && parval){
|
||||
*parval = I32;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
/************* List of proto functions for each command *************/
|
||||
|
||||
static errcodes_t cmd_help(const char*, char*){
|
||||
SEND(REPOURL);
|
||||
for(size_t i = 0; i < sizeof(cmdInfo)/sizeof(cmdInfo[0]); ++i){
|
||||
SEND(cmdInfo[i].name);
|
||||
SEND(" - ");
|
||||
SEND(cmdInfo[i].desc);
|
||||
SEND("\n");
|
||||
}
|
||||
return ERR_AMOUNT;
|
||||
}
|
||||
|
||||
static errcodes_t cmd_time(const char* cmd, char*){
|
||||
CMDEQ();
|
||||
printu(Tms); N();
|
||||
return ERR_AMOUNT;
|
||||
}
|
||||
|
||||
static errcodes_t cmd_reset(const char*, char*){
|
||||
NVIC_SystemReset();
|
||||
return ERR_CANTRUN; // unreacheable
|
||||
}
|
||||
|
||||
// send acquisition time
|
||||
static void imaqtime(uint8_t sensno){
|
||||
uint32_t T = mlx_lastimT(sensno);
|
||||
SEND(Timage); printu(sensno); SEND(EQ);
|
||||
printu(T); N();
|
||||
}
|
||||
|
||||
// Common image command for ASCII/binary/tempmap
|
||||
static errcodes_t image_cmd(char* args, int mode){
|
||||
int32_t sensno = -1;
|
||||
splitargs(args, &sensno);
|
||||
if(sensno < 0 || sensno >= N_SENSORS) return ERR_BADPAR;
|
||||
fp_t *img = mlx_getimage(sensno);
|
||||
if(!img) return ERR_CANTRUN;
|
||||
|
||||
// Frame number
|
||||
imaqtime(sensno);
|
||||
|
||||
switch(mode){
|
||||
case 0: // tempmap
|
||||
dumpIma(img);
|
||||
break;
|
||||
case 1: // ascii
|
||||
drawIma(img);
|
||||
break;
|
||||
case 2: // binary
|
||||
SEND("BINARY"); putb('0'+sensno); putb('=');
|
||||
uint8_t *d = (uint8_t*)img;
|
||||
uint32_t _2send = MLX_PIXNO * sizeof(float);
|
||||
// send by portions of 256 bytes
|
||||
while(_2send){
|
||||
uint32_t portion = (_2send > 256) ? 256 : _2send;
|
||||
if(sendbin(d, portion)){
|
||||
_2send -= portion;
|
||||
d += portion;
|
||||
}
|
||||
}
|
||||
SEND("ENDIMAGE"); N();
|
||||
break;
|
||||
}
|
||||
return ERR_AMOUNT;
|
||||
}
|
||||
|
||||
static errcodes_t cmd_ascii(const char* , char* args){
|
||||
return image_cmd(args, 1);
|
||||
}
|
||||
static errcodes_t cmd_binary(const char* , char* args){
|
||||
return image_cmd(args, 2);
|
||||
}
|
||||
static errcodes_t cmd_tempmap(const char* , char* args){
|
||||
return image_cmd(args, 0);
|
||||
}
|
||||
|
||||
static errcodes_t cmd_acqtime(const char* , char* args){
|
||||
int32_t sensno = -1;
|
||||
splitargs(args, &sensno);
|
||||
if(sensno < 0 || sensno >= N_SENSORS) return ERR_BADPAR;
|
||||
imaqtime(sensno);
|
||||
return ERR_AMOUNT;
|
||||
}
|
||||
|
||||
static errcodes_t cmd_listids(const char*, char*){
|
||||
int N = mlx_nactive();
|
||||
if(!N) return ERR_CANTRUN;
|
||||
uint8_t *ids = mlx_activeids();
|
||||
SEND("Found "); printu(N); SEND(" active sensors:\n");
|
||||
for(int i = 0; i < N_SENSORS; ++i){
|
||||
if(ids[i]){
|
||||
SEND("SENSID"); printu(i); SEND(EQ); printuhex(ids[i]>>1); N();
|
||||
}
|
||||
}
|
||||
return ERR_AMOUNT;
|
||||
}
|
||||
|
||||
static errcodes_t cmd_bmereinit(const char*, char*){
|
||||
if(bme_init()) return ERR_OK;
|
||||
return ERR_CANTRUN;
|
||||
}
|
||||
|
||||
static errcodes_t cmd_environ(const char*, char*){
|
||||
bme280_t env;
|
||||
if(!get_environment(&env)) return ERR_CANTRUN;
|
||||
SEND("TEMPERATURE="); printfl(env.T, 2); N();
|
||||
SEND("SKYTEMPERATURE="); printfl(env.Tsky, 2); N();
|
||||
SEND("PRESSURE_HPA="); printfl(env.P/100.f, 2); N();
|
||||
SEND("PRESSURE_MM="); printfl(env.P * 0.00750062f, 2); N();
|
||||
SEND("HUMIDITY="); printfl(env.H, 2); N();
|
||||
SEND("TEMP_DEW="); printfl(env.Tdew, 1); N();
|
||||
SEND("T_MEASUREMENT="); printu(env.Tmeas); N();
|
||||
return ERR_OK;
|
||||
}
|
||||
|
||||
static errcodes_t cmd_state(const char* cmd, char*){
|
||||
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();
|
||||
CMDEQ(); SEND(states[s]); N();
|
||||
return ERR_AMOUNT;
|
||||
}
|
||||
|
||||
/********** I2C commands **********/
|
||||
|
||||
static errcodes_t cmd_iicaddr(const char* cmd, char* args){
|
||||
int32_t addr;
|
||||
if(argsvals(args, NULL, &addr)){
|
||||
if(addr < 0 || addr > 0x7f) return ERR_BADVAL;
|
||||
I2Caddress = (uint8_t)(addr << 1);
|
||||
mlx_sethwaddr(I2Caddress, addr);
|
||||
return ERR_AMOUNT;
|
||||
}
|
||||
// getter
|
||||
CMDEQ(); printuhex(I2Caddress >> 1); N();
|
||||
return ERR_AMOUNT;
|
||||
}
|
||||
|
||||
static errcodes_t cmd_mlxcont(const char*, char*){
|
||||
mlx_continue();
|
||||
return ERR_OK;
|
||||
}
|
||||
|
||||
static errcodes_t cmd_iicspeed(const char* cmd, char* args){
|
||||
static const char *speeds[] = {"10K","100K","400K","1M","2M"};
|
||||
int32_t speed;
|
||||
// TODO: allow string parameter
|
||||
if(argsvals(args, NULL, &speed)){
|
||||
if (speed < 0 || speed >= I2C_SPEED_AMOUNT) return ERR_BADVAL;
|
||||
i2c_setup((i2c_speed_t)speed);
|
||||
}
|
||||
// getter
|
||||
CMDEQ(); SEND(speeds[i2c_curspeed]); N();
|
||||
return ERR_AMOUNT;
|
||||
}
|
||||
|
||||
static errcodes_t cmd_mlxpause(const char*, char*){
|
||||
mlx_pause();
|
||||
return ERR_OK;
|
||||
}
|
||||
|
||||
static errcodes_t cmd_mlxstop(const char*, char*){
|
||||
mlx_stop();
|
||||
return ERR_OK;
|
||||
}
|
||||
|
||||
static errcodes_t cmd_adc(const char* cmd, char* args){
|
||||
if(!args){ // show all values
|
||||
for(uint8_t i = 0; i < NUMBER_OF_ADC_CHANNELS; ++i){
|
||||
CMDEQP(i); printu(getADCval(i)); N();
|
||||
}
|
||||
return ERR_AMOUNT;
|
||||
}
|
||||
int32_t addr;
|
||||
splitargs(args, &addr);
|
||||
if(addr < 0 || addr >= NUMBER_OF_ADC_CHANNELS) return ERR_BADPAR;
|
||||
CMDEQP(addr); printu(getADCval(static_cast<uint8_t>(addr))); N();
|
||||
return ERR_AMOUNT;
|
||||
}
|
||||
|
||||
static errcodes_t cmd_ntc(const char* cmd, char* args){
|
||||
if(!args){ // show all values
|
||||
for(uint8_t i = 0; i <= ADC_AIN4; ++i){
|
||||
CMDEQP(i); printfl(getNTCtemp(i), 1); N();
|
||||
}
|
||||
return ERR_AMOUNT;
|
||||
}
|
||||
int32_t addr;
|
||||
splitargs(args, &addr);
|
||||
if(addr < 0 || addr > ADC_AIN4) return ERR_BADPAR;
|
||||
CMDEQP(addr); printfl(getNTCtemp(static_cast<uint8_t>(addr)), 1); N();
|
||||
return ERR_AMOUNT;
|
||||
}
|
||||
|
||||
static errcodes_t cmd_cartoon(const char*, char*){
|
||||
// TODO: should be getter/setter!
|
||||
cartoon = !cartoon;
|
||||
return ERR_OK;
|
||||
}
|
||||
|
||||
static errcodes_t cmd_mlxdump(const char*, char* args){
|
||||
int32_t sensno = -1;
|
||||
splitargs(args, &sensno);
|
||||
if (sensno < 0 || sensno >= N_SENSORS) return ERR_BADPAR;
|
||||
MLX90640_params *params = mlx_getparams(sensno);
|
||||
if(!params) return ERR_CANTRUN;
|
||||
|
||||
SEND("SENSNO="); printi(sensno); N();
|
||||
SEND("kVdd="); printi(params->kVdd); N();
|
||||
SEND("vdd25="); printi(params->vdd25); N();
|
||||
SEND("KvPTAT="); printfl(params->KvPTAT, 4); N();
|
||||
SEND("KtPTAT="); printfl(params->KtPTAT, 4); N();
|
||||
SEND("vPTAT25="); printi(params->vPTAT25); N();
|
||||
SEND("alphaPTAT="); printfl(params->alphaPTAT, 2); N();
|
||||
SEND("gainEE="); printi(params->gainEE); N();
|
||||
SEND("Pixel offset parameters:\n");
|
||||
dumpIma(params->offset);
|
||||
SEND("K_talpha:\n");
|
||||
dumpIma(params->kta);
|
||||
SEND("Kv: ");
|
||||
for(int i = 0; i < 4; ++i) { printfl(params->kv[i], 2); putb(' '); }
|
||||
N();
|
||||
SEND("cpOffset="); printi(params->cpOffset[0]); SEND(", "); printi(params->cpOffset[1]); N();
|
||||
SEND("cpKta="); printfl(params->cpKta, 2); N();
|
||||
SEND("cpKv="); printfl(params->cpKv, 2); N();
|
||||
SEND("tgc="); printfl(params->tgc, 2); N();
|
||||
SEND("cpALpha="); printfl(params->cpAlpha[0], 2); SEND(", "); printfl(params->cpAlpha[1], 2); N();
|
||||
SEND("KsTa="); printfl(params->KsTa, 2); N();
|
||||
SEND("Alpha:\n");
|
||||
dumpIma(params->alpha);
|
||||
SEND("CT3="); printfl(params->CT[1], 2); N();
|
||||
SEND("CT4="); printfl(params->CT[2], 2); N();
|
||||
for(int i = 0; i < 4; ++i){
|
||||
SEND("KsTo"); putb('0'+i); putb('='); printfl(params->KsTo[i], 2); N();
|
||||
SEND("alphacorr"); putb('0'+i); putb('='); printfl(params->alphacorr[i], 2); N();
|
||||
}
|
||||
return ERR_AMOUNT;
|
||||
}
|
||||
|
||||
static errcodes_t cmd_mlxaddr(const char* cmd, char* args){
|
||||
int32_t sensno = -1;
|
||||
if(!args || !*args) { // without args: show global address
|
||||
//CMDEQ(); printuhex(I2Caddress>>1); N();
|
||||
//return ERR_AMOUNT;
|
||||
return ERR_BADPAR;
|
||||
}
|
||||
const char *setter = splitargs(args, &sensno);
|
||||
if(sensno < 0 || sensno >= N_SENSORS) return ERR_BADPAR;
|
||||
|
||||
if(setter){ // setter: set current address
|
||||
uint32_t a;
|
||||
const char *nxt = getnum(setter, &a);
|
||||
if(nxt == setter || a > 0x7f) return ERR_BADVAL;
|
||||
mlx_setaddr(sensno, (uint8_t)a);
|
||||
return ERR_AMOUNT;
|
||||
}else{ // getter
|
||||
uint8_t a = mlx_getaddr(sensno);
|
||||
CMDEQP(sensno); printuhex(a); N();
|
||||
return ERR_AMOUNT;
|
||||
}
|
||||
}
|
||||
|
||||
static errcodes_t cmd_readreg(const char* cmd, char* args){
|
||||
int32_t reg = -1, nwords = 1;
|
||||
const char *setter = splitargs(args, ®);
|
||||
if(reg < 0) return ERR_BADPAR;
|
||||
if(setter){ // read more than one byte
|
||||
uint32_t n;
|
||||
const char *nxt = getnum(setter, &n);
|
||||
if (nxt == setter || n == 0 || n > I2C_BUFSIZE) return ERR_BADVAL;
|
||||
nwords = (int32_t)n;
|
||||
}
|
||||
uint16_t *data = i2c_read_reg16(I2Caddress, (uint16_t)reg, nwords, 0);
|
||||
if(!data) return ERR_CANTRUN;
|
||||
if(nwords == 1){
|
||||
CMDEQP(reg); printuhex(*data); N();
|
||||
}else{
|
||||
CMDEQP(reg); N(); hexdump16(SEND, data, nwords);
|
||||
}
|
||||
return ERR_AMOUNT;
|
||||
}
|
||||
|
||||
static errcodes_t cmd_writedata(const char*, char* args){
|
||||
const char *setter = splitargs(args, NULL);
|
||||
if(!setter) return ERR_BADVAL;
|
||||
int N = 0;
|
||||
const char *p = setter;
|
||||
while (*p){
|
||||
if(N >= LOCBUFFSZ) return ERR_AMOUNT;
|
||||
uint32_t val;
|
||||
p = getnum(p, &val);
|
||||
if(p == setter) break; // not a number
|
||||
locBuffer[N++] = (uint16_t)val;
|
||||
p = omit_spaces(p);
|
||||
if (!*p) break;
|
||||
}
|
||||
if(N == 0) return ERR_BADVAL;
|
||||
if(!i2c_write(I2Caddress, locBuffer, N)) return ERR_CANTRUN;
|
||||
return ERR_OK;
|
||||
}
|
||||
|
||||
static errcodes_t cmd_iicscan(const char*, char*) {
|
||||
i2c_init_scan_mode();
|
||||
return ERR_OK;
|
||||
}
|
||||
|
||||
static errcodes_t cmd_mcutemp(const char* cmd, char*){
|
||||
CMDEQ(); printfl(getMCUtemp(), 2); N();
|
||||
return ERR_AMOUNT;
|
||||
}
|
||||
|
||||
static errcodes_t cmd_mcuvdd(const char* cmd, char*){
|
||||
CMDEQ(); printfl(getVdd(), 2); N();
|
||||
return ERR_AMOUNT;
|
||||
}
|
||||
|
||||
static errcodes_t cmd_dac(const char* cmd, char* args){
|
||||
int32_t val;
|
||||
if(argsvals(args, NULL, &val)){
|
||||
if(val < 0 || val > 4095) return ERR_BADVAL;
|
||||
DAC1->DHR12R1 = static_cast<uint32_t>(val);
|
||||
}
|
||||
// getter
|
||||
CMDEQ(); printu(DAC1->DHR12R1); N();
|
||||
return ERR_AMOUNT;
|
||||
}
|
||||
|
||||
static void showpwm(const char* cmd, uint8_t nch){
|
||||
uint16_t ccr;
|
||||
switch(nch){
|
||||
case 0: ccr = TIM3->CCR1; break;
|
||||
case 1: ccr = TIM3->CCR2; break;
|
||||
case 2: ccr = TIM3->CCR3; break;
|
||||
case 3: ccr = TIM3->CCR4; break;
|
||||
default: return;
|
||||
}
|
||||
CMDEQP(nch); printu(ccr); N();
|
||||
}
|
||||
|
||||
// four PWM channels: 1,2 - heaters, 3,4 - info
|
||||
static errcodes_t cmd_pwm(const char* cmd, char* args){
|
||||
int32_t ch = -1, val;
|
||||
const char *setter = splitargs(args, &ch);
|
||||
if(ch < 0 || ch > PWM_CH_MAX){ // all channels
|
||||
for(uint8_t i = 0; i <= PWM_CH_MAX; ++i)
|
||||
showpwm(cmd, i);
|
||||
return ERR_AMOUNT;
|
||||
}
|
||||
if(setter){
|
||||
if(!getint(setter, &val) || val < 0 || val > 100) return ERR_BADVAL;
|
||||
if(!setPWM(static_cast<uint8_t>(ch), static_cast<uint32_t>(val)))
|
||||
return ERR_CANTRUN;
|
||||
}
|
||||
// getter
|
||||
showpwm(cmd, (uint8_t)ch);
|
||||
return ERR_AMOUNT;
|
||||
}
|
||||
|
||||
static errcodes_t cmd_sendstr(const char*, char* args) {
|
||||
int32_t dummy;
|
||||
const char *text = splitargs(args, &dummy);
|
||||
if(!text || !*text) return ERR_BADVAL;
|
||||
// switch to other interface
|
||||
int (*other_sender)(const char*) = (SEND == usb_sender) ? usart_sender : usb_sender;
|
||||
if (!other_sender) return ERR_CANTRUN;
|
||||
other_sender(text);
|
||||
other_sender("\n");
|
||||
return ERR_OK;
|
||||
}
|
||||
|
||||
static constexpr uint32_t hash(const char* str, uint32_t h = 0) {
|
||||
return *str ? hash(str + 1, h + ((h << 7) ^ *str)) : h;
|
||||
}
|
||||
|
||||
|
||||
const char *parse_cmd(int (*sendfun)(const char*), char *buf) {
|
||||
if(!buf || !*buf || !sendfun) return NULL;
|
||||
SEND = sendfun;
|
||||
if(sendfun == usb_sender){
|
||||
putb = usb_putb;
|
||||
sendbin = usb_sendbin;
|
||||
}else{
|
||||
putb = usart_putb;
|
||||
sendbin = usart_sendbin;
|
||||
}
|
||||
char command[CMD_MAXLEN+1];
|
||||
int i = 0;
|
||||
while(*buf > '@' && i < CMD_MAXLEN) command[i++] = *buf++;
|
||||
command[i] = 0;
|
||||
while(*buf && *buf <= ' ') ++buf;
|
||||
char *args = buf;
|
||||
#ifdef EBUG
|
||||
USB_sendstr("__args='"); USB_sendstr(args); USB_sendstr("'\n");
|
||||
#endif
|
||||
if(!*args) args = NULL;
|
||||
|
||||
uint32_t h = hash(command);
|
||||
errcodes_t ecode = ERR_AMOUNT;
|
||||
switch (h){
|
||||
#define COMMAND(name, desc) case hash(#name): ecode = cmd_##name(command, args); break;
|
||||
COMMAND_TABLE
|
||||
#undef COMMAND
|
||||
default:
|
||||
SEND("Unknown command, try 'help'\n");
|
||||
}
|
||||
if(ecode < ERR_AMOUNT) return errtxt[ecode];
|
||||
return NULL;
|
||||
}
|
||||
|
||||
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);
|
||||
putb(' ');
|
||||
}
|
||||
N();
|
||||
}
|
||||
}
|
||||
|
||||
#define GRAY_LEVELS 16
|
||||
static const char *const CHARS_16 = " .':;+*oxX#&%B$@";
|
||||
void drawIma(const fp_t im[MLX_PIXNO]){
|
||||
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;
|
||||
SEND("RANGE="); printfl(range, 3); N();
|
||||
SEND("MIN="); printfl(min_val, 3); N();
|
||||
SEND("MAX="); printfl(max_val, 3); N();
|
||||
if(fabsf(range) < 0.001) range = 1.0f;
|
||||
iptr = im;
|
||||
char string[MLX_W+2];
|
||||
string[MLX_W] = '\n'; string[MLX_W+1] = 0; // end of line
|
||||
for(int row = 0; row < MLX_H; ++row){
|
||||
for(int col = 0; col < MLX_W; ++col){
|
||||
fp_t normalized = ((*iptr++) - min_val) / range;
|
||||
int idx = (int)(normalized * GRAY_LEVELS);
|
||||
if(idx < 0) idx = 0;
|
||||
else if(idx >= GRAY_LEVELS) idx = GRAY_LEVELS-1;
|
||||
string[col] = CHARS_16[idx];
|
||||
}
|
||||
SEND(string);
|
||||
}
|
||||
N();
|
||||
}
|
||||
61
F3:F303/MLX90640-allsky/commproto.h
Normal file
61
F3:F303/MLX90640-allsky/commproto.h
Normal file
@@ -0,0 +1,61 @@
|
||||
/*
|
||||
* This file is part of the as3935 project.
|
||||
* Copyright 2026 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "mlx90640.h"
|
||||
|
||||
#include "version.inc"
|
||||
|
||||
#ifdef EBUG
|
||||
#define RLSDBG "debug"
|
||||
#else
|
||||
#define RLSDBG "release"
|
||||
#endif
|
||||
|
||||
#define REPOURL "https://github.com/eddyem/stm32samples/tree/master/F3:F303/MLX90640-allsky " RLSDBG " build #" BUILD_NUMBER "@" BUILD_DATE "\n"
|
||||
|
||||
|
||||
// error codes for answer message
|
||||
typedef enum{
|
||||
ERR_OK, // all OK
|
||||
ERR_BADCMD, // wrong command
|
||||
ERR_BADPAR, // wrong parameter
|
||||
ERR_BADVAL, // wrong value (for setter)
|
||||
ERR_WRONGLEN, // wrong message length
|
||||
ERR_CANTRUN, // can't run given command due to bad parameters or other
|
||||
ERR_BUSY, // target interface busy, try later
|
||||
ERR_OVERFLOW, // string was too long -> overflow
|
||||
ERR_AMOUNT // amount of error codes or "send nothing"
|
||||
} errcodes_t;
|
||||
|
||||
// maximal length of command (without trailing zero)
|
||||
#define CMD_MAXLEN 15
|
||||
// maximal available parameter number (for 16-bit registers is 0xffff
|
||||
#define MAXPARNO 0xffff
|
||||
|
||||
extern const char *EQ;
|
||||
const char *parse_cmd(int (*sendfun)(const char*), char *buf);
|
||||
void set_senders(int (*usbs)(const char*), int (*usbb)(uint8_t), int (*usbbin)(const uint8_t*, int),
|
||||
int (*usarts)(const char*), int (*usartb)(uint8_t), int (*usartbin)(const uint8_t*, int));
|
||||
|
||||
extern const char *const Timage;
|
||||
|
||||
extern uint8_t cartoon;
|
||||
void dumpIma(const fp_t im[MLX_PIXNO]);
|
||||
void drawIma(const fp_t im[MLX_PIXNO]);
|
||||
@@ -51,16 +51,19 @@ TRUE_INLINE void iwdg_setup(){
|
||||
TRUE_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; USB pullup - PA15
|
||||
// PA6 - PWM for external heater (TIM3_CH1 or TIM16_CH1); PA7 - PWM propto (humidity - 50%)
|
||||
// USB - alternate function 14 @ pins PA11/PA12; SWD - AF0 @PA13/14
|
||||
// PA6,PA7 - PWM for external heaters (TIM3_CH1, TIM3_CH2)
|
||||
// PA0..PA4 - NTC in, PA5 - DAC_OUT1 (board heater), PA6 - ADC in for DAC out
|
||||
// USART pins will be setup in usart.c
|
||||
GPIOA->AFR[0] = AFRf(2, 6) | AFRf(2, 7);
|
||||
GPIOA->AFR[1] = AFRf(14, 11) | AFRf(14, 12);
|
||||
GPIOA->MODER = MODER_AI(0) | MODER_AI(1) | MODER_AI(4) | MODER_AI(5) | MODER_AF(6) |
|
||||
MODER_AF(7) | MODER_AF(11) | MODER_AF(12) | MODER_AF(13) | MODER_AF(14) | MODER_O(15);
|
||||
// PB0 - PWM propto Text (<=20 - 0%, >=30 - 100%), PB1 - PWM propto (Text-Tsky) (<=-5 - 0%, >=+35 - 100%) PB2 - SPI_CS
|
||||
// force USB DP to low level for a while
|
||||
GPIOA->MODER = MODER_AI(0) | MODER_AI(1) | MODER_AI(2) | MODER_AI(3) | MODER_AI(4) | MODER_AI(5) | MODER_AF(6) |
|
||||
MODER_AF(7) | MODER_AF(11) | MODER_O(12) | MODER_AF(13) | MODER_AF(14) | MODER_O(15);
|
||||
// PB0 - PWM propto Text (<=20 - 0%, >=30 - 100%), PB1 - PWM propto (Text-Tsky) (<=-5 - 0%, >=+35 - 100%) PB9 - SPI_CS
|
||||
// SPI and I2C will be setup in spi.c and i2c.c
|
||||
GPIOB->AFR[0] = AFRf(2, 0) | AFRf(2, 1);
|
||||
GPIOB->MODER = MODER_AF(0) | MODER_AF(1) | MODER_O(2);
|
||||
pin_set(GPIOB, 1<<1);
|
||||
GPIOB->MODER = MODER_AF(0) | MODER_AF(1) | MODER_O(9);
|
||||
SPI_CS_1();
|
||||
}
|
||||
|
||||
@@ -85,7 +88,7 @@ TRUE_INLINE void pwm_setup(){
|
||||
|
||||
// change PWM value in percents; return 0 if `val` is bad or `ch` not 0..3
|
||||
int setPWM(uint8_t ch, uint8_t val){
|
||||
if(ch > 3 || val > PWM_CCR_MAX) return 0;
|
||||
if(ch >= PWM_CH_MAX || val > PWM_CCR_MAX) return 0;
|
||||
volatile uint32_t *CCRs = &(TIM3->CCR1);
|
||||
CCRs[ch] = val;
|
||||
return 1;
|
||||
@@ -139,7 +142,6 @@ void bme_process(){
|
||||
// set PWM duty propto humidity
|
||||
float h = (Humidity - 50.f) * 2.f;
|
||||
if(h < 0.f) h = 0.f; else if(h > 100.f) h = 100.f;
|
||||
setPWM(PWM_CH_HUMIDITY, (uint8_t)h);
|
||||
environment.Tmeas = Tms;
|
||||
// set PWM duty propto external T
|
||||
float t = (Temperature + 20.f) * 2.f;
|
||||
|
||||
@@ -25,9 +25,9 @@
|
||||
#define USBPU_ON() pin_clear(USBPU_port, USBPU_pin)
|
||||
#define USBPU_OFF() pin_set(USBPU_port, USBPU_pin)
|
||||
|
||||
// SPI_CS - PB2
|
||||
#define SPI_CS_1() pin_set(GPIOB, 1<<2)
|
||||
#define SPI_CS_0() pin_clear(GPIOB, 1<<2)
|
||||
// SPI_CS - PB9
|
||||
#define SPI_CS_1() pin_set(GPIOB, 1<<9)
|
||||
#define SPI_CS_0() pin_clear(GPIOB, 1<<9)
|
||||
|
||||
// interval of environment measurements, ms
|
||||
#define ENV_MEAS_PERIOD (10000)
|
||||
@@ -36,14 +36,11 @@
|
||||
// Max PWM CCR1 value (->1)
|
||||
#define PWM_CCR_MAX (100)
|
||||
// PWM channels (start from 0 - CH1)
|
||||
// external heater
|
||||
#define PWM_CH_HEATER (0)
|
||||
// propto humidity (the higher - the brighter)
|
||||
#define PWM_CH_HUMIDITY (1)
|
||||
// propto external T (the higher - the brighter)
|
||||
#define PWM_CH_TEXT (2)
|
||||
// propto Tsky - Text (the higher - the brighter)
|
||||
#define PWM_CH_TSKY (3)
|
||||
#define PWM_CH_MAX (3)
|
||||
|
||||
typedef struct{
|
||||
float T; // temperature, degC
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!DOCTYPE QtCreatorProject>
|
||||
<!-- Written by QtCreator 17.0.1, 2025-10-06T23:24:05. -->
|
||||
<!-- Written by QtCreator 19.0.1, 2026-05-06T23:28:28. -->
|
||||
<qtcreator>
|
||||
<data>
|
||||
<variable>EnvironmentId</variable>
|
||||
@@ -86,6 +86,7 @@
|
||||
<valuelist type="QVariantList" key="ClangTools.SuppressedDiagnostics"/>
|
||||
<value type="bool" key="ClangTools.UseGlobalSettings">true</value>
|
||||
</valuemap>
|
||||
<value type="int" key="RcSync">0</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
@@ -153,6 +154,7 @@
|
||||
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
|
||||
<valuelist type="QVariantList" key="Analyzer.Valgrind.SuppressionFiles"/>
|
||||
<valuelist type="QVariantList" key="CustomOutputParsers"/>
|
||||
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
|
||||
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
|
||||
@@ -162,6 +164,7 @@
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
|
||||
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.UniqueId"></value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
|
||||
</valuemap>
|
||||
@@ -185,6 +188,7 @@
|
||||
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
|
||||
<valuelist type="QVariantList" key="Analyzer.Valgrind.SuppressionFiles"/>
|
||||
<valuelist type="QVariantList" key="CustomOutputParsers"/>
|
||||
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
|
||||
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
|
||||
@@ -194,6 +198,7 @@
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
|
||||
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.UniqueId"></value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
|
||||
</valuemap>
|
||||
@@ -204,10 +209,6 @@
|
||||
<variable>ProjectExplorer.Project.TargetCount</variable>
|
||||
<value type="qlonglong">1</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.Updater.FileVersion</variable>
|
||||
<value type="int">22</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>Version</variable>
|
||||
<value type="int">22</value>
|
||||
|
||||
@@ -2,6 +2,8 @@ BMP280.c
|
||||
BMP280.h
|
||||
adc.c
|
||||
adc.h
|
||||
commproto.cpp
|
||||
commproto.h
|
||||
hardware.c
|
||||
hardware.h
|
||||
i2c.c
|
||||
@@ -15,8 +17,6 @@ mlx90640.h
|
||||
mlx90640_regs.h
|
||||
mlxproc.c
|
||||
mlxproc.h
|
||||
proto.c
|
||||
proto.h
|
||||
ringbuffer.c
|
||||
ringbuffer.h
|
||||
spi.c
|
||||
|
||||
|
Before Width: | Height: | Size: 344 B After Width: | Height: | Size: 354 B |
@@ -20,7 +20,7 @@
|
||||
#include "hardware.h"
|
||||
#include "i2c.h"
|
||||
#include "mlxproc.h"
|
||||
#include "proto.h"
|
||||
#include "commproto.h"
|
||||
#include "strfunc.h"
|
||||
#include "usart.h"
|
||||
#include "usb_dev.h"
|
||||
@@ -43,14 +43,18 @@ int main(void){
|
||||
StartHSI();
|
||||
SysTick_Config((uint32_t)48000); // 1ms
|
||||
}
|
||||
USBPU_OFF();
|
||||
USBPU_OFF(); // for development board with managed pullup resistor
|
||||
hw_setup();
|
||||
adc_setup();
|
||||
i2c_setup(I2C_SPEED_400K);
|
||||
bme_init();
|
||||
USB_setup();
|
||||
usart_setup(115200);
|
||||
// setup USB DP as alternate function - for sensors' board with constant pullup resistor
|
||||
GPIOA->MODER = (GPIOA->MODER & ~GPIO_MODER_MODER12) | MODER_AF(12);
|
||||
USBPU_ON();
|
||||
USB_setup();
|
||||
// set senders for abiliby of sending messages between interfaces
|
||||
set_senders(USB_sendstr, USB_putbyte, USB_send, usart_sendstr, usart_putbyte, usart_send);
|
||||
uint32_t ctr = Tms, Tlastima[N_SENSORS] = {0};
|
||||
mlx_continue(); // init state machine
|
||||
while(1){
|
||||
@@ -63,7 +67,7 @@ int main(void){
|
||||
int l = USB_receivestr(inbuff, MAXSTRLEN);
|
||||
if(l < 0) USB_sendstr("USBOVERFLOW\n");
|
||||
else if(l){
|
||||
const char *ans = parse_cmd(inbuff, SEND_USB);
|
||||
const char *ans = parse_cmd(USB_sendstr, inbuff);
|
||||
if(ans) USB_sendstr(ans);
|
||||
}
|
||||
if(i2c_scanmode){ // send this to both
|
||||
@@ -84,7 +88,6 @@ int main(void){
|
||||
if(Tnow != Tlastima[i]){
|
||||
fp_t *im = mlx_getimage(i);
|
||||
if(im){
|
||||
chsendfun(SEND_USB);
|
||||
//U(Sensno); UN(i2str(i));
|
||||
U(Timage); USB_putbyte('0'+i); USB_putbyte('='); UN(u2str(Tnow));
|
||||
drawIma(im);
|
||||
@@ -96,7 +99,7 @@ int main(void){
|
||||
if(usart_ovr()) usart_sendstr("USART_OVERFLOW\n");
|
||||
char *got = usart_getline(NULL);
|
||||
if(got){
|
||||
const char *ans = parse_cmd(got, SEND_USART);
|
||||
const char *ans = parse_cmd(usart_sendstr, got);
|
||||
if(ans) usart_sendstr(ans);
|
||||
}
|
||||
bme_process();
|
||||
|
||||
@@ -63,12 +63,16 @@ static int sensno = -1;
|
||||
mlx_state_t mlx_state(){ return MLX_state; }
|
||||
// set address
|
||||
int mlx_setaddr(int n, uint8_t addr){
|
||||
if(n < 0 || n > N_SENSORS) return 0;
|
||||
if(n < 0 || n >= N_SENSORS) 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;
|
||||
}
|
||||
uint8_t mlx_getaddr(int n){
|
||||
if(n < 0 || n >= N_SENSORS) return 0;
|
||||
return sens_addresses[n];
|
||||
}
|
||||
// pause state machine and stop
|
||||
void mlx_pause(){
|
||||
MLX_oldstate = MLX_state;
|
||||
|
||||
@@ -39,6 +39,7 @@ typedef enum{
|
||||
} mlx_state_t;
|
||||
|
||||
int mlx_setaddr(int n, uint8_t addr);
|
||||
uint8_t mlx_getaddr(int n);
|
||||
mlx_state_t mlx_state();
|
||||
int mlx_nactive();
|
||||
uint8_t *mlx_activeids();
|
||||
|
||||
@@ -1,539 +0,0 @@
|
||||
/*
|
||||
* This file is part of the ir-allsky project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <stm32f3.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "adc.h"
|
||||
#include "hardware.h"
|
||||
#include "i2c.h"
|
||||
#include "mlxproc.h"
|
||||
#include "proto.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
|
||||
|
||||
// functions to send data over USB or USART: to change them use flag in `parse_cmd`
|
||||
typedef struct{
|
||||
int (*S)(const char*); // send string
|
||||
int (*P)(uint8_t); // put byte
|
||||
int (*B)(const uint8_t*, int); // send raw bytes
|
||||
} sendfun_t;
|
||||
|
||||
static sendfun_t usbsend = {
|
||||
.S = USB_sendstr, .P = USB_putbyte, .B = USB_send
|
||||
};
|
||||
static sendfun_t usartsend = {
|
||||
.S = usart_sendstr, .P = usart_putbyte, .B = usart_send
|
||||
};
|
||||
|
||||
static sendfun_t *sendfun = &usbsend;
|
||||
|
||||
void chsendfun(int sendto){
|
||||
if(sendto == SEND_USB) sendfun = &usbsend;
|
||||
else sendfun = &usartsend;
|
||||
}
|
||||
|
||||
// newline
|
||||
#define N() sendfun->P('\n')
|
||||
#define printu(x) do{sendfun->S(u2str(x));}while(0)
|
||||
#define printi(x) do{sendfun->S(i2str(x));}while(0)
|
||||
#define printuhex(x) do{sendfun->S(uhex2str(x));}while(0)
|
||||
#define printfl(x,n) do{sendfun->S(float2str(x, n));}while(0)
|
||||
|
||||
// common names for frequent keys
|
||||
const char* const Timage = "TIMAGE";
|
||||
const char* const Image = "IMAGE";
|
||||
static const char *const Sensno = "SENSNO=";
|
||||
|
||||
static const char *const OK = "OK\n", *const ERR = "ERR\n";
|
||||
const char *const helpstring =
|
||||
"https://github.com/eddyem/stm32samples/tree/master/F3:F303/MLX90640multi build#" BUILD_NUMBER " @ " BUILD_DATE "\n"
|
||||
" management of single IR bolometer MLX90640\n"
|
||||
"dn - draw nth image in ASCII\n"
|
||||
"gn - get nth image 'as is' - float array of 768x4 bytes\n"
|
||||
"l - list active sensors IDs\n"
|
||||
"mn - show temperature map of nth image\n"
|
||||
"tn - show nth image aquisition time\n"
|
||||
"B - reinit BME280\n"
|
||||
"E - get environment parameters (temperature etc)\n"
|
||||
"G - get MLX state\n"
|
||||
"R - reset device\n"
|
||||
"T - print current Tms\n"
|
||||
" Debugging options:\n"
|
||||
"aa - change I2C address to a (a should be non-shifted value!!!)\n"
|
||||
"c - continue MLX\n"
|
||||
"i0..4 - setup I2C with speed 10k, 100k, 400k, 1M or 2M (experimental!)\n"
|
||||
"p - pause MLX\n"
|
||||
"s - stop MLX (and start from zero @ 'c')\n"
|
||||
"A - get ADC values\n"
|
||||
"C - \"cartoon\" mode on/off (show each new image) - USB only!!!\n"
|
||||
"Dn - dump MLX parameters for sensor number n\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"
|
||||
"M - get MCU temperature and Vdd value\n"
|
||||
"O - set output of DAC (0..4095)\n"
|
||||
"Px - set PWM output (0..100%) or get current value\n"
|
||||
"Us - send string 's' to other interface\n"
|
||||
;
|
||||
|
||||
TRUE_INLINE const char *setupI2C(char *buf){
|
||||
static const char * const 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);
|
||||
}
|
||||
sendfun->S("I2CSPEED="); sendfun->S(speeds[i2c_curspeed]); N();
|
||||
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{
|
||||
sendfun->S("Wrong number"); N();
|
||||
return ERR;
|
||||
}
|
||||
}else{
|
||||
sendfun->S("Need address"); N();
|
||||
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_SENSORS) 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;
|
||||
sendfun->S("I2CADDR="); sendfun->S(uhex2str(addr)); N();
|
||||
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;
|
||||
sendfun->S(b); N();
|
||||
}else hexdump16(sendfun->S, 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){
|
||||
sendfun->S("byte "); sendfun->S(u2str(i));
|
||||
sendfun->S(" :"); sendfun->S(uhex2str(locBuffer[i])); N();
|
||||
}
|
||||
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); sendfun->P(' ');
|
||||
}
|
||||
N();
|
||||
}
|
||||
}
|
||||
// dump MLX parameters
|
||||
TRUE_INLINE void dumpparams(const char *buf){
|
||||
int N = getsensnum(buf);
|
||||
if(N < 0){ sendfun->S(ERR); return; }
|
||||
MLX90640_params *params = mlx_getparams(N);
|
||||
if(!params){ sendfun->S(ERR); return; }
|
||||
N(); sendfun->S(Sensno); sendfun->S(i2str(N));
|
||||
sendfun->S("\nkVdd="); printi(params->kVdd);
|
||||
sendfun->S("\nvdd25="); printi(params->vdd25);
|
||||
sendfun->S("\nKvPTAT="); printfl(params->KvPTAT, 4);
|
||||
sendfun->S("\nKtPTAT="); printfl(params->KtPTAT, 4);
|
||||
sendfun->S("\nvPTAT25="); printi(params->vPTAT25);
|
||||
sendfun->S("\nalphaPTAT="); printfl(params->alphaPTAT, 2);
|
||||
sendfun->S("\ngainEE="); printi(params->gainEE);
|
||||
sendfun->S("\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); sendfun->P(' ');
|
||||
}
|
||||
N();
|
||||
}
|
||||
sendfun->S("K_talpha:\n");
|
||||
dumpfarr(params->kta);
|
||||
sendfun->S("Kv: ");
|
||||
for(int i = 0; i < 4; ++i){
|
||||
printfl(params->kv[i], 2); sendfun->P(' ');
|
||||
}
|
||||
sendfun->S("\ncpOffset=");
|
||||
printi(params->cpOffset[0]); sendfun->S(", "); printi(params->cpOffset[1]);
|
||||
sendfun->S("\ncpKta="); printfl(params->cpKta, 2);
|
||||
sendfun->S("\ncpKv="); printfl(params->cpKv, 2);
|
||||
sendfun->S("\ntgc="); printfl(params->tgc, 2);
|
||||
sendfun->S("\ncpALpha="); printfl(params->cpAlpha[0], 2);
|
||||
sendfun->S(", "); printfl(params->cpAlpha[1], 2);
|
||||
sendfun->S("\nKsTa="); printfl(params->KsTa, 2);
|
||||
sendfun->S("\nAlpha:\n");
|
||||
dumpfarr(params->alpha);
|
||||
sendfun->S("\nCT3="); printfl(params->CT[1], 2);
|
||||
sendfun->S("\nCT4="); printfl(params->CT[2], 2);
|
||||
for(int i = 0; i < 4; ++i){
|
||||
sendfun->S("\nKsTo"); sendfun->P('0'+i); sendfun->P('=');
|
||||
printfl(params->KsTo[i], 2);
|
||||
sendfun->S("\nalphacorr"); sendfun->P('0'+i); sendfun->P('=');
|
||||
printfl(params->alphacorr[i], 2);
|
||||
}
|
||||
N();
|
||||
}
|
||||
// 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();
|
||||
sendfun->S("MLXSTATE=");
|
||||
sendfun->S(states[s]); N();
|
||||
}
|
||||
|
||||
// `draw`==1 - draw, ==0 - show T map, 2 - send raw float array with prefix 'TIMAGEX=y\nIMAGEX=' 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){
|
||||
//sendfun->S(Sensno); sendfun->S(u2str(sensno)); N();
|
||||
sendfun->S(Timage); sendfun->P('0'+sensno); sendfun->P('='); sendfun->S(u2str(T)); N();
|
||||
switch(draw){
|
||||
case 0:
|
||||
dumpIma(img);
|
||||
break;
|
||||
case 1:
|
||||
drawIma(img);
|
||||
break;
|
||||
case 2:
|
||||
sendfun->S(Image); sendfun->P('0'+sensno); sendfun->P('=');
|
||||
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;
|
||||
sendfun->B(d, portion);
|
||||
_2send -= portion;
|
||||
d += portion;
|
||||
}
|
||||
sendfun->S("ENDIMAGE"); N();
|
||||
break;
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
return ERR;
|
||||
}
|
||||
|
||||
TRUE_INLINE void listactive(){
|
||||
int N = mlx_nactive();
|
||||
if(!N){ sendfun->S("No active sensors found!\n"); return; }
|
||||
uint8_t *ids = mlx_activeids();
|
||||
sendfun->S("Found "); sendfun->P('0'+N);
|
||||
sendfun->S(" active sensors:"); N();
|
||||
for(int i = 0; i < N_SENSORS; ++i)
|
||||
if(ids[i]){
|
||||
sendfun->S("SENSID");
|
||||
sendfun->S(u2str(i)); sendfun->P('=');
|
||||
sendfun->S(uhex2str(ids[i] >> 1));
|
||||
N();
|
||||
}
|
||||
}
|
||||
|
||||
static void getimt(const char *buf){
|
||||
int sensno = getsensnum(buf);
|
||||
if(sensno > -1){
|
||||
sendfun->S(Timage); sendfun->P('0'+sensno); sendfun->P('='); sendfun->S(u2str(mlx_lastimT(sensno))); N();
|
||||
}else sendfun->S(ERR);
|
||||
}
|
||||
|
||||
TRUE_INLINE void getenv(){
|
||||
bme280_t env;
|
||||
if(!get_environment(&env)) sendfun->S("BADENVIRONMENT\n");
|
||||
sendfun->S("TEMPERATURE="); sendfun->S(float2str(env.T, 2));
|
||||
sendfun->S("\nSKYTEMPERATURE="); sendfun->S(float2str(env.Tsky, 2));
|
||||
sendfun->S("\nPRESSURE_HPA="); sendfun->S(float2str(env.P/100.f, 2));
|
||||
sendfun->S("\nPRESSURE_MM="); sendfun->S(float2str(env.P * 0.00750062f, 2));
|
||||
sendfun->S("\nHUMIDITY="); sendfun->S(float2str(env.H, 2));
|
||||
sendfun->S("\nTEMP_DEW="); sendfun->S(float2str(env.Tdew, 1));
|
||||
sendfun->S("\nT_MEASUREMENT="); sendfun->S(u2str(env.Tmeas));
|
||||
N();
|
||||
}
|
||||
|
||||
TRUE_INLINE const char *DAC_chval(const char *buf){
|
||||
uint32_t D;
|
||||
const char *nxt = getnum(buf, &D);
|
||||
if(!nxt || nxt == buf || D > 4095) return ERR;
|
||||
DAC1->DHR12R1 = D;
|
||||
return OK;
|
||||
}
|
||||
|
||||
|
||||
TRUE_INLINE void getADC(){
|
||||
sendfun->S("AIN0="); sendfun->S(u2str(getADCval(ADC_AIN0)));
|
||||
sendfun->S("\nAIN1="); sendfun->S(u2str(getADCval(ADC_AIN1)));
|
||||
sendfun->S("\nAIN5="); sendfun->S(u2str(getADCval(ADC_AIN5)));
|
||||
N();
|
||||
}
|
||||
|
||||
TRUE_INLINE void getMCUvals(){
|
||||
sendfun->S("MCUTEMP="); sendfun->S(float2str(getMCUtemp(), 2));
|
||||
sendfun->S("\nMCUVDD="); sendfun->S(float2str(getVdd(), 2));
|
||||
N();
|
||||
}
|
||||
|
||||
TRUE_INLINE const char* setpwm(const char *buf){
|
||||
uint32_t D;
|
||||
if(!buf || !*buf){
|
||||
sendfun->S("PWM1="); sendfun->S(u2str(TIM3->CCR1));
|
||||
sendfun->S("\nPWM2="); sendfun->S(u2str(TIM3->CCR2));
|
||||
sendfun->S("\nPWM3="); sendfun->S(u2str(TIM3->CCR3));
|
||||
sendfun->S("\nPWM4="); sendfun->S(u2str(TIM3->CCR4));
|
||||
N();
|
||||
return NULL;
|
||||
}
|
||||
const char *nxt = getnum(buf, &D);
|
||||
if(!nxt || nxt == buf || !setPWM(PWM_CH_HEATER, D)) return ERR;
|
||||
return OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief parse_cmd - user string parser
|
||||
* @param buf - user data
|
||||
* @param isusb - ==1 to send answer over usb, else send over USART1
|
||||
* @return answer OK/ERR or NULL
|
||||
*/
|
||||
const char *parse_cmd(char *buf, int sendto){
|
||||
if(!buf || !*buf) return NULL;
|
||||
chsendfun(sendto);
|
||||
if(buf[1]){
|
||||
switch(*buf++){ // "long" commands
|
||||
case 'a':
|
||||
return chhwaddr(buf);
|
||||
case 'd':
|
||||
return drawimg(buf, 1);
|
||||
case 'g':
|
||||
return drawimg(buf, 2);
|
||||
case 'i':
|
||||
return setupI2C(buf);
|
||||
case 'm':
|
||||
return drawimg(buf, 0);
|
||||
case 't':
|
||||
getimt(buf); return NULL;
|
||||
case 'D':
|
||||
dumpparams(buf);
|
||||
return NULL;
|
||||
break;
|
||||
case 'I':
|
||||
buf = omit_spaces(buf);
|
||||
switch(*buf++){
|
||||
case 'a':
|
||||
return chaddr(buf);
|
||||
case 'r':
|
||||
return rdI2C(buf);
|
||||
case 'w':
|
||||
return wrI2C(buf);
|
||||
case 's':
|
||||
i2c_init_scan_mode();
|
||||
return OK;
|
||||
default:
|
||||
return ERR;
|
||||
}
|
||||
break;
|
||||
case 'O':
|
||||
return DAC_chval(buf);
|
||||
case 'P':
|
||||
return setpwm(buf);
|
||||
case 'U':
|
||||
if(sendto == SEND_USB) chsendfun(SEND_USART);
|
||||
else chsendfun(SEND_USB);
|
||||
if(sendfun->S(buf) && N()) return OK;
|
||||
return ERR;
|
||||
default:
|
||||
return ERR;
|
||||
}
|
||||
}
|
||||
switch(*buf){ // "short" (one letter) commands
|
||||
case 'A':
|
||||
getADC();
|
||||
break;
|
||||
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 'B':
|
||||
if(bme_init()) return OK;
|
||||
return ERR;
|
||||
case 'C':
|
||||
if(sendto != SEND_USB) return ERR;
|
||||
cartoon = !cartoon; return OK;
|
||||
case 'E':
|
||||
getenv();
|
||||
break;
|
||||
case 'G':
|
||||
getst();
|
||||
break;
|
||||
case 'M':
|
||||
getMCUvals();
|
||||
break;
|
||||
case 'P':
|
||||
return setpwm(NULL);
|
||||
case 'R':
|
||||
NVIC_SystemReset();
|
||||
break;
|
||||
case 'T':
|
||||
sendfun->S("T="); sendfun->S(u2str(Tms)); N();
|
||||
break;
|
||||
case '?': // help
|
||||
case 'h':
|
||||
case 'H':
|
||||
sendfun->S(helpstring);
|
||||
break;
|
||||
default:
|
||||
return ERR;
|
||||
break;
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// dump image as temperature matrix
|
||||
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);
|
||||
sendfun->P(' ');
|
||||
}
|
||||
N();
|
||||
}
|
||||
}
|
||||
|
||||
#define GRAY_LEVELS (16)
|
||||
// 16-level character set ordered by fill percentage (provided by user)
|
||||
static const char *const CHARS_16 = " .':;+*oxX#&%B$@";
|
||||
// draw image in ASCII-art
|
||||
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;
|
||||
sendfun->S("RANGE="); sendfun->S(float2str(range, 3));
|
||||
sendfun->S("\nMIN="); sendfun->S(float2str(min_val, 3));
|
||||
sendfun->S("\nMAX="); sendfun->S(float2str(max_val, 3)); N();
|
||||
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);
|
||||
sendfun->P(CHARS_16[index]);
|
||||
}
|
||||
N();
|
||||
}
|
||||
N();
|
||||
}
|
||||
|
||||
@@ -1,30 +0,0 @@
|
||||
/*
|
||||
* This file is part of the ir-allsky project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
extern const char *const Timage;
|
||||
|
||||
#define SEND_USB (1)
|
||||
#define SEND_USART (0)
|
||||
|
||||
extern uint8_t cartoon;
|
||||
void chsendfun(int sendto);
|
||||
const char *parse_cmd(char *buf, int sendto);
|
||||
void dumpIma(const fp_t im[MLX_PIXNO]);
|
||||
void drawIma(const fp_t im[MLX_PIXNO]);
|
||||
@@ -1,2 +1,2 @@
|
||||
#define BUILD_NUMBER "45"
|
||||
#define BUILD_DATE "2026-05-01"
|
||||
#define BUILD_NUMBER "65"
|
||||
#define BUILD_DATE "2026-05-06"
|
||||
|
||||
Reference in New Issue
Block a user