diff --git a/F0-nolib/usbcan_relay/adc.c b/F0-nolib/usbcan_relay/adc.c new file mode 100644 index 0000000..95c1c3a --- /dev/null +++ b/F0-nolib/usbcan_relay/adc.c @@ -0,0 +1,105 @@ +/* + * This file is part of the canrelay project. + * Copyright 2021 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 "adc.h" + +/** + * @brief ADC_array - array for ADC channels with median filtering: + * 0 & 1 - external channels (5 & 12V) + * 2 - internal Tsens + * 3 - Vref + */ +static uint16_t ADC_array[NUMBER_OF_ADC_CHANNELS*9]; + +void adc_setup(){ + uint16_t ctr = 0; // 0xfff0 - more than 1.3ms + ADC1->CR &= ~ADC_CR_ADEN; + DMA1_Channel1->CCR &= ~DMA_CCR_EN; + RCC->APB2ENR |= RCC_APB2ENR_ADC1EN; // Enable the peripheral clock of the ADC + RCC->CR2 |= RCC_CR2_HSI14ON; // Start HSI14 RC oscillator + while ((RCC->CR2 & RCC_CR2_HSI14RDY) == 0 && ++ctr < 0xfff0){}; // Wait HSI14 is ready + // calibration + if(ADC1->CR & ADC_CR_ADEN){ // Ensure that ADEN = 0 + ADC1->CR &= (uint32_t)(~ADC_CR_ADEN); // Clear ADEN + } + ADC1->CR |= ADC_CR_ADCAL; // Launch the calibration by setting ADCAL + ctr = 0; // ADC calibration time is 5.9us + while(ADC1->CR & ADC_CR_ADCAL && ++ctr < 0xfff0); // Wait until ADCAL=0 + // enable ADC + ctr = 0; + do{ + ADC1->CR |= ADC_CR_ADEN; + }while((ADC1->ISR & ADC_ISR_ADRDY) == 0 && ++ctr < 0xfff0); + // configure ADC + ADC1->CFGR1 |= ADC_CFGR1_CONT; // Select the continuous mode + // channels 6,7,16 and 17 + ADC1->CHSELR = ADC_CHSELR_CHSEL6 | ADC_CHSELR_CHSEL7 | ADC_CHSELR_CHSEL16 | ADC_CHSELR_CHSEL17; + ADC1->SMPR |= ADC_SMPR_SMP; // Select a sampling mode of 111 i.e. 239.5 ADC clk to be greater than 17.1us + ADC->CCR |= ADC_CCR_TSEN | ADC_CCR_VREFEN; // Wake-up the VREFINT and Temperature sensor + // configure DMA for ADC + RCC->AHBENR |= RCC_AHBENR_DMA1EN; // Enable the peripheral clock on DMA + ADC1->CFGR1 |= ADC_CFGR1_DMAEN | ADC_CFGR1_DMACFG; // Enable DMA transfer on ADC and circular mode + DMA1_Channel1->CPAR = (uint32_t) (&(ADC1->DR)); // Configure the peripheral data register address + DMA1_Channel1->CMAR = (uint32_t)(ADC_array); // Configure the memory address + DMA1_Channel1->CNDTR = NUMBER_OF_ADC_CHANNELS * 9; // Configure the number of DMA tranfer to be performs on DMA channel 1 + DMA1_Channel1->CCR |= DMA_CCR_MINC | DMA_CCR_MSIZE_0 | DMA_CCR_PSIZE_0 | DMA_CCR_CIRC; // Configure increment, size, interrupts and circular mode + DMA1_Channel1->CCR |= DMA_CCR_EN; // Enable DMA Channel 1 + ADC1->CR |= ADC_CR_ADSTART; // start the ADC conversions +} + +/** + * @brief getADCval - calculate median value for `nch` channel + * @param nch - number of channel + * @return + */ +uint16_t getADCval(int nch){ + int i, addr = nch; + 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; } + uint16_t p[9]; + for(i = 0; i < 9; ++i, addr += NUMBER_OF_ADC_CHANNELS) // first we should prepare array for optmed + p[i] = ADC_array[addr]; + PIX_SORT(p[1], p[2]) ; PIX_SORT(p[4], p[5]) ; PIX_SORT(p[7], p[8]) ; + PIX_SORT(p[0], p[1]) ; PIX_SORT(p[3], p[4]) ; PIX_SORT(p[6], p[7]) ; + PIX_SORT(p[1], p[2]) ; PIX_SORT(p[4], p[5]) ; PIX_SORT(p[7], p[8]) ; + PIX_SORT(p[0], p[3]) ; PIX_SORT(p[5], p[8]) ; PIX_SORT(p[4], p[7]) ; + PIX_SORT(p[3], p[6]) ; PIX_SORT(p[1], p[4]) ; PIX_SORT(p[2], p[5]) ; + PIX_SORT(p[4], p[7]) ; PIX_SORT(p[4], p[2]) ; PIX_SORT(p[6], p[4]) ; + PIX_SORT(p[4], p[2]) ; + return p[4]; +#undef PIX_SORT +#undef PIX_SWAP +} + +// return MCU temperature (degrees of celsius * 10) +int32_t getMCUtemp(){ + int32_t ADval = getADCval(2); + int32_t temperature = (int32_t) *TEMP30_CAL_ADDR - ADval; + temperature *= (int32_t)(1100 - 300); + temperature /= (int32_t)(*TEMP30_CAL_ADDR - *TEMP110_CAL_ADDR); + temperature += 300; + return(temperature); +} + +// return Vdd * 100 (V) +uint32_t getVdd(){ + uint32_t vdd = ((uint32_t) *VREFINT_CAL_ADDR) * (uint32_t)330; // 3.3V + vdd /= getADCval(3); + return vdd; +} diff --git a/F0-nolib/usbcan_relay/adc.h b/F0-nolib/usbcan_relay/adc.h new file mode 100644 index 0000000..54c6ba4 --- /dev/null +++ b/F0-nolib/usbcan_relay/adc.h @@ -0,0 +1,32 @@ +/* + * This file is part of the canrelay project. + * Copyright 2021 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 . + */ + +#ifndef ADC_H +#define ADC_H +#include "stm32f0.h" + +// 2 external & 2 internal +#define NUMBER_OF_ADC_CHANNELS (4) + +void adc_setup(); +int32_t getMCUtemp(); +uint32_t getVdd(); +uint16_t getADCval(int nch); +int16_t getNTC(int nch); + +#endif // ADC_H diff --git a/F0-nolib/usbcan_relay/buttons.c b/F0-nolib/usbcan_relay/buttons.c index 44ea7a5..24a032f 100644 --- a/F0-nolib/usbcan_relay/buttons.c +++ b/F0-nolib/usbcan_relay/buttons.c @@ -52,10 +52,10 @@ void process_keys(){ break; } }else{ // released - if(e == EVT_PRESS || e == EVT_RELEASE){ // released + if(e == EVT_PRESS || e == EVT_HOLD){ // released if(k->counter > PRESSTHRESHOLD) k->counter = PRESSTHRESHOLD; else if((k->counter -= d) < 0){ - k->event = EVT_NONE; // button released + k->event = EVT_RELEASE; // button released } } } diff --git a/F0-nolib/usbcan_relay/can.c b/F0-nolib/usbcan_relay/can.c index d9bb268..a51641c 100644 --- a/F0-nolib/usbcan_relay/can.c +++ b/F0-nolib/usbcan_relay/can.c @@ -17,6 +17,7 @@ */ #include "can.h" +#include "canproto.h" #include "hardware.h" #include "proto.h" @@ -120,8 +121,9 @@ void CAN_setup(uint16_t speed){ /* Enable the peripheral clock CAN */ RCC->APB1ENR |= RCC_APB1ENR_CANEN; // Configure CAN - while((CAN->MSR & CAN_MSR_INAK)!=CAN_MSR_INAK) - { + CAN->MCR |= CAN_MCR_INRQ; // Enter CAN init mode to write the configuration + while((CAN->MSR & CAN_MSR_INAK) != CAN_MSR_INAK){ + IWDG->KR = IWDG_REFRESH; if(--tmout == 0) break; } CAN->MCR &=~ CAN_MCR_SLEEP; @@ -129,7 +131,10 @@ void CAN_setup(uint16_t speed){ CAN->BTR = 2 << 20 | 3 << 16 | (6000/speed - 1); // speed CAN->MCR &=~ CAN_MCR_INRQ; tmout = 16000000; - while((CAN->MSR & CAN_MSR_INAK)==CAN_MSR_INAK) if(--tmout == 0) break; + while((CAN->MSR & CAN_MSR_INAK) == CAN_MSR_INAK){ // Wait the init mode leaving + IWDG->KR = IWDG_REFRESH; + if(--tmout == 0) break; + } // accept self ID at filter 0, ALL other at filters 1 and 2 CAN->FMR = CAN_FMR_FINIT; CAN->FA1R = CAN_FA1R_FACT0 | CAN_FA1R_FACT1 | CAN_FA1R_FACT2; @@ -188,6 +193,7 @@ void can_proc(){ if(CAN->ESR & CAN_ESR_EPVF) SEND("Passive error limit"); if(CAN->ESR & CAN_ESR_EWGF) SEND("Error counter limit"); NL(); + IWDG->KR = IWDG_REFRESH; // request abort for all mailboxes CAN->TSR |= CAN_TSR_ABRQ0 | CAN_TSR_ABRQ1 | CAN_TSR_ABRQ2; // reset CAN bus @@ -271,7 +277,6 @@ static void can_process_fifo(uint8_t fifo_num){ // read all while(*RFxR & CAN_RF0R_FMP0){ // amount of messages pending // CAN_RDTxR: (16-31) - timestamp, (8-15) - filter match index, (0-3) - data length - /* TODO: check filter match index if more than one ID can receive */ CAN_message msg; uint8_t *dat = msg.data; uint8_t len = box->RDTR & 0x0f; @@ -307,7 +312,8 @@ static void can_process_fifo(uint8_t fifo_num){ dat[0] = lb & 0xff; } } - if(CAN_messagebuf_push(&msg)) return; // error: buffer is full, try later + if(msg.ID == CANID) parseCANcommand(&msg); + else if(CAN_messagebuf_push(&msg)) return; // error: buffer is full, try later *RFxR |= CAN_RF0R_RFOM0; // release fifo for access to next message } //if(*RFxR & CAN_RF0R_FULL0) *RFxR &= ~CAN_RF0R_FULL0; diff --git a/F0-nolib/usbcan_relay/canproto.c b/F0-nolib/usbcan_relay/canproto.c new file mode 100644 index 0000000..cc0d1c5 --- /dev/null +++ b/F0-nolib/usbcan_relay/canproto.c @@ -0,0 +1,171 @@ +/* + * This file is part of the canrelay project. + * Copyright 2021 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 "adc.h" +#include "buttons.h" +#include "canproto.h" +#include "hardware.h" +#include "proto.h" + +/* + * Protocol (endian format depends on command!): + * answer will be send to `OUTPID` CAN ID, length depends on received command + * IN packet (sent to `CANID`) data bytes: + * 0 - command (any from CAN_commands), 1..7 - data + * To set value command should have CAN_CMD_SETFLAG == 1 + * OUT packet depending on command: + * CAN_CMD_PING (or any incoming packet with zero lenght) - zero length answer + * other commands have data[0] equal to in command (without CAN_CMD_SETFLAG) + * CAN_CMD_ADC: data[1]..data[6] - 12bits*4channels RAW ADC data (ADC channels 0..3) + * CAN_CMD_BTNS: data[1] = button number (0..3), data[2] - state (keyevent), data[4..7] - event time (LITTLE endian!) + * CAN_CMD_LED: data[1] = LEDs state (bits 0..3 for LEDs number 0..3) + * CAN_CMD_MCU: data[2,3] = int16_t MCUT*10, data[4,5] = uint16_t Vdd*100 (LITTLE endian!) + * CAN_CMD_PWM: data[1..3] = pwm value for each channel (0..2) + * CAN_CMD_RELAY: data[0] = (bits 0 & 1) - state of relay N] + * CAN_CMD_TMS: data[4..7] = Tms (LITTLE endian!) + */ + +// return total message length +TRUE_INLINE uint8_t ADCget(uint8_t values[8]){ + uint16_t v[NUMBER_OF_ADC_CHANNELS]; + for(int i = 0; i < NUMBER_OF_ADC_CHANNELS; ++i) v[i] = getADCval(i); + // if NUMBER_OF_ADC_CHANNELS != 4 - need for manual refactoring! + values[1] = v[0] >> 4; + values[2] = (v[0] << 4) | ((v[1] >> 8) & 0x0f); + values[3] = v[1] & 0xff; + values[4] = v[2] >> 4; + values[5] = (v[2] << 4) | ((v[3] >> 8) & 0x0f); + values[6] = v[3] & 0xff; + return 7; +} + +TRUE_INLINE void BTNSget(uint8_t values[8]){ + uint32_t T; + for(uint8_t i = 0; i < BTNSNO; ++i){ + values[1] = i; + values[2] = keystate(i, &T); + values[3] = 0; + *((uint32_t*)&values[4]) = T; + int N = 1000; + while(CAN_BUSY == can_send(values, 8, OUTPID)) if(--N == 0) break; + } +} + +TRUE_INLINE void LEDSset(uint8_t data[8]){ + for(int i = 0; i < LEDSNO; ++i){ + if(data[1] & 1<CCR1; + for(int i = 0; i < 3; ++i) + reg[i] = data[i+1]; +} + +TRUE_INLINE uint8_t PWMget(uint8_t data[8]){ + volatile uint32_t *reg = &TIM1->CCR1; + for(int i = 0; i < 3; ++i) + data[i+1] = reg[i]; + return 4; +} + +TRUE_INLINE void Rset(uint8_t data[8]){ + for(int i = 0; i < RelaysNO; ++i){ + if(data[1] & 1<length; ++i){ + printuhex(msg->data[i]); bufputchar(' '); + } + NL(); +#endif + int N = 1000; + uint8_t cmd = msg->data[0] & CANCMDMASK, setter = msg->data[0] & CAN_CMD_SETFLAG; + if(msg->length == 0 || cmd == CAN_CMD_PING){ // no commands or ping cmd -> return empty message (ping) + goto sendans; + } + outpdata[0] = cmd; + switch(cmd){ + case CAN_CMD_ADC: + len = ADCget(outpdata); + break; + case CAN_CMD_BTNS: + BTNSget(outpdata); + return; + break; + case CAN_CMD_LED: + if(setter) LEDSset(msg->data); + len = LEDSget(outpdata); + break; + case CAN_CMD_MCU: + len = MCUget(outpdata); + break; + case CAN_CMD_PWM: + if(setter) PWMset(msg->data); + len = PWMget(outpdata); + break; + case CAN_CMD_RELAY: + if(setter) Rset(msg->data); + len = Rget(outpdata); + break; + case CAN_CMD_TMS: + len = showTms(outpdata); + break; + default: // wrong command -> + outpdata[0] = CAN_CMD_ERRCMD; + len = 1; + } +sendans: + while(CAN_BUSY == can_send(outpdata, len, OUTPID)) if(--N == 0) break; +} diff --git a/F0-nolib/usbcan_relay/canproto.h b/F0-nolib/usbcan_relay/canproto.h new file mode 100644 index 0000000..2de8898 --- /dev/null +++ b/F0-nolib/usbcan_relay/canproto.h @@ -0,0 +1,47 @@ +/* + * This file is part of the canrelay project. + * Copyright 2021 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 +#ifndef CANPROTO_H__ +#define CANPROTO_H__ + +#include "stm32f0.h" +#include "can.h" + +// command without setter flag +#define CANCMDMASK (0x7f) + +typedef enum{ + CAN_CMD_PING, // ping (without setter) + CAN_CMD_RELAY, // relay get/set + CAN_CMD_PWM, // PWM get/set + CAN_CMD_ADC, // get ADC (without setter) + CAN_CMD_MCU, // MCU T and Vdd + CAN_CMD_LED, // LEDs get/set + CAN_CMD_BTNS, // get Buttons state (without setter) + CAN_CMD_TMS, // get time from start (in ms) + CAN_CMD_ERRCMD, // wrong command + CAN_CMD_SETFLAG = 0x80 // command is setter +} CAN_commands; + +// output messages identifier +#define OUTPID (CANID + 0x100) + +void parseCANcommand(CAN_message *msg); + +#endif // CANPROTO_H__ diff --git a/F0-nolib/usbcan_relay/hardware.c b/F0-nolib/usbcan_relay/hardware.c index 6d48316..fd2c7ae 100644 --- a/F0-nolib/usbcan_relay/hardware.c +++ b/F0-nolib/usbcan_relay/hardware.c @@ -27,6 +27,10 @@ const uint32_t LEDpins[LEDSNO] = {1<<12, 1<<13, 1<<14, 1<<15}; // Buttons: PA2..PA5, pullup GPIO_TypeDef *BTNports[BTNSNO] = {GPIOA, GPIOA, GPIOA, GPIOA}; const uint32_t BTNpins[BTNSNO] = {1<<2, 1<<3, 1<<4, 1<<5}; +// relays: PA0/1 +GPIO_TypeDef *R_ports[RelaysNO] = {GPIOA, GPIOA}; +const uint32_t R_pins[RelaysNO] = {1<<0, 1<<1}; + void gpio_setup(void){ RCC->AHBENR |= RCC_AHBENR_GPIOAEN | RCC_AHBENR_GPIOBEN; @@ -37,8 +41,8 @@ void gpio_setup(void){ GPIOB->PUPDR = GPIO_PUPDR0_PU | GPIO_PUPDR1_PU | GPIO_PUPDR2_PU | GPIO_PUPDR3_PU | GPIO_PUPDR4_PU | GPIO_PUPDR5_PU | GPIO_PUPDR6_PU | GPIO_PUPDR7_PU; // relays (PA0..1) as outputs, buttons (PA2..5) as pullup inputs - // PA8..10 - PWM @ TIM1 - GPIOA->MODER = GPIO_MODER_MODER0_O | GPIO_MODER_MODER1_O | + // PA6,7 - ADC IN, PA8..10 - PWM @ TIM1 + GPIOA->MODER = GPIO_MODER_MODER0_O | GPIO_MODER_MODER1_O | GPIO_MODER_MODER6_AI | GPIO_MODER_MODER7_AI | GPIO_MODER_MODER8_AF | GPIO_MODER_MODER9_AF | GPIO_MODER_MODER10_AF; GPIOA->PUPDR = GPIO_PUPDR2_PU | GPIO_PUPDR3_PU | GPIO_PUPDR4_PU | GPIO_PUPDR5_PU; CANID = (~READ_INV_CAN_ADDR()) & CAN_INV_ID_MASK; @@ -90,6 +94,8 @@ void pause_ms(uint32_t pause){ while(Tms < Tnxt) nop(); } + +#ifdef EBUG void Jump2Boot(){ __disable_irq(); IWDG->KR = IWDG_REFRESH; @@ -123,6 +129,20 @@ void Jump2Boot(){ // set main stack pointer __set_MSP(*((uint32_t *)addr)); IWDG->KR = IWDG_REFRESH; - // jump to bootloader + // due to "empty check" mechanism, STM32F042 can jump to bootloader only with empty first 4 bytes of user code + while ((FLASH->SR & FLASH_SR_BSY) != 0){} + FLASH->SR = FLASH_SR_EOP | FLASH_SR_PGERR | FLASH_SR_WRPRTERR; + if ((FLASH->CR & FLASH_CR_LOCK) != 0){ + FLASH->KEYR = FLASH_KEY1; + FLASH->KEYR = FLASH_KEY2; + } + FLASH->CR |= FLASH_CR_PER; + FLASH->AR = 0x08000000; // erase zero's page + FLASH->CR |= FLASH_CR_STRT; + while(!(FLASH->SR & FLASH_SR_EOP)); + FLASH->SR |= FLASH_SR_EOP; + FLASH->CR &= ~FLASH_CR_PER; + // jump to bootloader (don't work :( ) SysMemBootJump(); } +#endif diff --git a/F0-nolib/usbcan_relay/hardware.h b/F0-nolib/usbcan_relay/hardware.h index 700bde2..00f5dd3 100644 --- a/F0-nolib/usbcan_relay/hardware.h +++ b/F0-nolib/usbcan_relay/hardware.h @@ -48,6 +48,14 @@ extern const uint32_t LEDpins[LEDSNO]; #define LED_toggle(x) do{pin_toggle(LEDports[x], LEDpins[x]);}while(0) #define LED_on(x) do{pin_clear(LEDports[x], LEDpins[x]);}while(0) #define LED_off(x) do{pin_set(LEDports[x], LEDpins[x]);}while(0) +#define LED_chk(x) ((LEDports[x]->IDR & LEDpins[x]) ? 0 : 1) + +#define RelaysNO (2) +extern GPIO_TypeDef *R_ports[RelaysNO]; +extern const uint32_t R_pins[RelaysNO]; +#define Relay_ON(x) do{pin_set(R_ports[x], R_pins[x]);}while(0) +#define Relay_OFF(x) do{pin_clear(R_ports[x], R_pins[x]);}while(0) +#define Relay_chk(x) (pin_read(R_ports[x], R_pins[x])) // Buttons amount #define BTNSNO (4) @@ -66,10 +74,12 @@ extern uint16_t CANID; // self CAN ID (read @ init) extern volatile uint32_t Tms; -void gpio_setup(void); +void gpio_setup(); void iwdg_setup(); void tim1_setup(); void pause_ms(uint32_t pause); +#ifdef EBUG void Jump2Boot(); +#endif #endif // __HARDWARE_H__ diff --git a/F0-nolib/usbcan_relay/main.c b/F0-nolib/usbcan_relay/main.c index 361debd..4d6343e 100644 --- a/F0-nolib/usbcan_relay/main.c +++ b/F0-nolib/usbcan_relay/main.c @@ -16,6 +16,7 @@ * along with this program. If not, see . */ +#include "adc.h" #include "buttons.h" #include "can.h" #include "hardware.h" @@ -70,6 +71,7 @@ int main(void){ sysreset(); SysTick_Config(6000, 1); gpio_setup(); + adc_setup(); tim1_setup(); USB_setup(); CAN_setup(100); diff --git a/F0-nolib/usbcan_relay/proto.c b/F0-nolib/usbcan_relay/proto.c index fcd0fee..9e5ee19 100644 --- a/F0-nolib/usbcan_relay/proto.c +++ b/F0-nolib/usbcan_relay/proto.c @@ -16,6 +16,7 @@ * along with this program. If not, see . */ +#include "adc.h" #include "buttons.h" #include "can.h" #include "hardware.h" @@ -179,7 +180,7 @@ static CAN_message *parseCANmsg(char *txt){ TRUE_INLINE void sendCANcommand(char *txt){ CAN_message *msg = parseCANmsg(txt); if(!msg) return; - uint32_t N = 1000000; + uint32_t N = 1000; while(CAN_BUSY == can_send(msg->data, msg->length, msg->ID)){ if(--N == 0) break; } @@ -428,7 +429,7 @@ TRUE_INLINE void getPWM(){ volatile uint32_t *reg = &TIM1->CCR1; for(int n = 0; n < 3; ++n){ SEND("PWM"); - bufputchar(n); + bufputchar('0' + n); bufputchar('='); printu(*reg++); bufputchar('\n'); @@ -455,6 +456,35 @@ TRUE_INLINE void changePWM(char *str){ SEND("OK, changed"); } +TRUE_INLINE void printADC(){ // show all 4 channels ADC + for(int i = 0; i < NUMBER_OF_ADC_CHANNELS; ++i){ + SEND("ADC"); bufputchar('0' + i); bufputchar('='); + printu(getADCval(i)); bufputchar('\n'); + } + sendbuf(); +} + +TRUE_INLINE void printVT(){ // show T and Vdd + int32_t t = getMCUtemp(); + SEND("T="); + if(t < 0){ bufputchar('-'); t = -t; } + printu(t); SEND("/10degC\nVDD="); + printu(getVdd()); SEND("/100V"); +} + +// set or check relay(N) state +TRUE_INLINE void relayX(uint8_t N, const char *txt){ + if(N >= RelaysNO) return; + txt = omit_spaces(txt); + uint32_t sr; + char *b = getnum(txt, &sr); + if(b && b != txt && sr < 2){ + if(sr) Relay_ON(N); else Relay_OFF(N); + } + SEND("Relay"); bufputchar('0'+N); bufputchar('='); + bufputchar('0' + Relay_chk(N)); +} + /** * @brief cmd_parser - command parsing * @param txt - buffer with commands & data @@ -466,6 +496,14 @@ void cmd_parser(char *txt){ * parse long commands here */ switch(_1st){ + case '0': + relayX(0, txt + 1); + goto eof; + break; + case '1': + relayX(1, txt + 1); + goto eof; + break; case 'a': addIGN(txt + 1); goto eof; @@ -502,23 +540,32 @@ void cmd_parser(char *txt){ } if(txt[1] != '\n') *txt = '?'; // help for wrong message length switch(_1st){ + case 'A': + printADC(); + return; + break; case 'b': getBtnState(); break; case 'd': IgnSz = 0; break; +#ifdef EBUG case 'D': SEND("Go into DFU mode\n"); sendbuf(); Jump2Boot(); break; +#endif case 'I': SEND("CAN ID: "); printuhex(CANID); break; case 'l': list_filters(); break; + case 'm': + printVT(); + break; case 'p': print_ign_buf(); break; @@ -543,15 +590,21 @@ void cmd_parser(char *txt){ break; default: // help SEND( + "'0' - turn relay0 on(1) or off(0)\n" + "'1' - turn relay1 on(1) or off(0)\n" "'a' - add ID to ignore list (max 10 IDs)\n" + "'A' - get ADC values @ all 4 channels\n" "'b' - get buttons' state\n" "'C' - reinit CAN with given baudrate\n" "'d' - delete ignore list\n" + #ifdef EBUG "'D' - activate DFU mode\n" + #endif "'f' - add/delete filter, format: bank# FIFO# mode(M/I) num0 [num1 [num2 [num3]]]\n" "'F' - send/clear flood message: F ID byte0 ... byteN\n" "'I' - read CAN ID\n" "'l' - list all active filters\n" + "'m' - get MCU temp & Vdd\n" "'o' - turn nth LED OFF\n" "'O' - turn nth LED ON\n" "'p' - print ignore buffer\n" diff --git a/F0-nolib/usbcan_relay/usbcan.bin b/F0-nolib/usbcan_relay/usbcan.bin index 00e5a24..84068e3 100755 Binary files a/F0-nolib/usbcan_relay/usbcan.bin and b/F0-nolib/usbcan_relay/usbcan.bin differ