From 5ad437856758bfe7d25b82a4e2a2d033f9661501 Mon Sep 17 00:00:00 2001 From: Frank Severinsen Date: Tue, 18 Aug 2020 19:16:13 +0200 Subject: [PATCH] WIP, Current status DAC output --- conf_general.h | 8 +-- hwconf/hw_uxv_sr.c | 137 +++++++++++++++++++++++++++++---------------- hwconf/hw_uxv_sr.h | 7 ++- 3 files changed, 100 insertions(+), 52 deletions(-) diff --git a/conf_general.h b/conf_general.h index 5be19748..dbf1e23b 100644 --- a/conf_general.h +++ b/conf_general.h @@ -140,8 +140,8 @@ //#define HW_SOURCE "hw_unity.c" //#define HW_HEADER "hw_unity.h" -//#define HW_SOURCE "hw_uxv_sr.c" -//#define HW_HEADER "hw_uxv_sr.h" +#define HW_SOURCE "hw_uxv_sr.c" +#define HW_HEADER "hw_uxv_sr.h" //#define HW_DUAL_CONFIG_PARALLEL //#define HW_SOURCE "hw_stormcore_100d.c" @@ -153,8 +153,8 @@ //#define HW_SOURCE "hw_stormcore_100s.c" //#define HW_HEADER "hw_stormcore_100s.h" -#define HW_SOURCE "hw_140_300.c" -#define HW_HEADER "hw_140_300.h" +//#define HW_SOURCE "hw_140_300.c" +//#define HW_HEADER "hw_140_300.h" #endif #ifndef HW_SOURCE diff --git a/hwconf/hw_uxv_sr.c b/hwconf/hw_uxv_sr.c index 25795cfb..7365b800 100644 --- a/hwconf/hw_uxv_sr.c +++ b/hwconf/hw_uxv_sr.c @@ -22,15 +22,22 @@ #include "stm32f4xx_conf.h" #include "utils.h" #include "drv8323s.h" +#include "comm_can.h" +#include "mc_interface.h" + +// Threads +THD_FUNCTION(dac_thread, arg); +static THD_WORKING_AREA(dac_thread_wa, 512); +static bool dac_thread_running = false; // Variables static volatile bool i2c_running = false; // I2C configuration static const I2CConfig i2cfg = { - OPMODE_I2C, - 100000, - STD_DUTY_CYCLE + OPMODE_I2C, + 100000, + STD_DUTY_CYCLE }; void hw_init_gpio(void) { @@ -42,51 +49,51 @@ void hw_init_gpio(void) { // LEDs palSetPadMode(GPIOB, 0, - PAL_MODE_OUTPUT_PUSHPULL | - PAL_STM32_OSPEED_HIGHEST); + PAL_MODE_OUTPUT_PUSHPULL | + PAL_STM32_OSPEED_HIGHEST); palSetPadMode(GPIOB, 1, - PAL_MODE_OUTPUT_PUSHPULL | - PAL_STM32_OSPEED_HIGHEST); + PAL_MODE_OUTPUT_PUSHPULL | + PAL_STM32_OSPEED_HIGHEST); // ENABLE_GATE palSetPadMode(GPIOB, 5, - PAL_MODE_OUTPUT_PUSHPULL | - PAL_STM32_OSPEED_HIGHEST); + PAL_MODE_OUTPUT_PUSHPULL | + PAL_STM32_OSPEED_HIGHEST); // Disable BMI160 palSetPadMode(GPIOA, 15, - PAL_MODE_OUTPUT_PUSHPULL | - PAL_STM32_OSPEED_HIGHEST); + PAL_MODE_OUTPUT_PUSHPULL | + PAL_STM32_OSPEED_HIGHEST); palSetPad(GPIOA, 15); // Disable DCCAL palSetPadMode(GPIOD, 2, - PAL_MODE_OUTPUT_PUSHPULL | - PAL_STM32_OSPEED_HIGHEST); + PAL_MODE_OUTPUT_PUSHPULL | + PAL_STM32_OSPEED_HIGHEST); palClearPad(GPIOD, 2); ENABLE_GATE(); // GPIOA Configuration: Channel 1 to 3 as alternate function push-pull palSetPadMode(GPIOA, 8, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) | - PAL_STM32_OSPEED_HIGHEST | - PAL_STM32_PUDR_FLOATING); + PAL_STM32_OSPEED_HIGHEST | + PAL_STM32_PUDR_FLOATING); palSetPadMode(GPIOA, 9, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) | - PAL_STM32_OSPEED_HIGHEST | - PAL_STM32_PUDR_FLOATING); + PAL_STM32_OSPEED_HIGHEST | + PAL_STM32_PUDR_FLOATING); palSetPadMode(GPIOA, 10, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) | - PAL_STM32_OSPEED_HIGHEST | - PAL_STM32_PUDR_FLOATING); + PAL_STM32_OSPEED_HIGHEST | + PAL_STM32_PUDR_FLOATING); palSetPadMode(GPIOB, 13, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) | - PAL_STM32_OSPEED_HIGHEST | - PAL_STM32_PUDR_FLOATING); + PAL_STM32_OSPEED_HIGHEST | + PAL_STM32_PUDR_FLOATING); palSetPadMode(GPIOB, 14, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) | - PAL_STM32_OSPEED_HIGHEST | - PAL_STM32_PUDR_FLOATING); + PAL_STM32_OSPEED_HIGHEST | + PAL_STM32_PUDR_FLOATING); palSetPadMode(GPIOB, 15, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) | - PAL_STM32_OSPEED_HIGHEST | - PAL_STM32_PUDR_FLOATING); + PAL_STM32_OSPEED_HIGHEST | + PAL_STM32_PUDR_FLOATING); // Hall sensors palSetPadMode(HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1, PAL_MODE_INPUT_PULLUP); @@ -148,6 +155,12 @@ void hw_setup_adc_channels(void) { ADC_InjectedChannelConfig(ADC1, ADC_Channel_10, 3, ADC_SampleTime_15Cycles); ADC_InjectedChannelConfig(ADC2, ADC_Channel_11, 3, ADC_SampleTime_15Cycles); ADC_InjectedChannelConfig(ADC3, ADC_Channel_12, 3, ADC_SampleTime_15Cycles); + + + if (!dac_thread_running) { + chThdCreateStatic(dac_thread_wa, sizeof(dac_thread_wa), NORMALPRIO, dac_thread, NULL); + dac_thread_running = true; + } } void hw_start_i2c(void) { @@ -155,15 +168,15 @@ void hw_start_i2c(void) { if (!i2c_running) { palSetPadMode(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN, - PAL_MODE_ALTERNATE(HW_I2C_GPIO_AF) | - PAL_STM32_OTYPE_OPENDRAIN | - PAL_STM32_OSPEED_MID1 | - PAL_STM32_PUDR_PULLUP); + PAL_MODE_ALTERNATE(HW_I2C_GPIO_AF) | + PAL_STM32_OTYPE_OPENDRAIN | + PAL_STM32_OSPEED_MID1 | + PAL_STM32_PUDR_PULLUP); palSetPadMode(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN, - PAL_MODE_ALTERNATE(HW_I2C_GPIO_AF) | - PAL_STM32_OTYPE_OPENDRAIN | - PAL_STM32_OSPEED_MID1 | - PAL_STM32_PUDR_PULLUP); + PAL_MODE_ALTERNATE(HW_I2C_GPIO_AF) | + PAL_STM32_OTYPE_OPENDRAIN | + PAL_STM32_OSPEED_MID1 | + PAL_STM32_PUDR_PULLUP); i2cStart(&HW_I2C_DEV, &i2cfg); i2c_running = true; @@ -195,14 +208,14 @@ void hw_try_restore_i2c(void) { i2cAcquireBus(&HW_I2C_DEV); palSetPadMode(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN, - PAL_STM32_OTYPE_OPENDRAIN | - PAL_STM32_OSPEED_MID1 | - PAL_STM32_PUDR_PULLUP); + PAL_STM32_OTYPE_OPENDRAIN | + PAL_STM32_OSPEED_MID1 | + PAL_STM32_PUDR_PULLUP); palSetPadMode(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN, - PAL_STM32_OTYPE_OPENDRAIN | - PAL_STM32_OSPEED_MID1 | - PAL_STM32_PUDR_PULLUP); + PAL_STM32_OTYPE_OPENDRAIN | + PAL_STM32_OSPEED_MID1 | + PAL_STM32_PUDR_PULLUP); palSetPad(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN); palSetPad(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN); @@ -226,16 +239,16 @@ void hw_try_restore_i2c(void) { palSetPad(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN); palSetPadMode(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN, - PAL_MODE_ALTERNATE(HW_I2C_GPIO_AF) | - PAL_STM32_OTYPE_OPENDRAIN | - PAL_STM32_OSPEED_MID1 | - PAL_STM32_PUDR_PULLUP); + PAL_MODE_ALTERNATE(HW_I2C_GPIO_AF) | + PAL_STM32_OTYPE_OPENDRAIN | + PAL_STM32_OSPEED_MID1 | + PAL_STM32_PUDR_PULLUP); palSetPadMode(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN, - PAL_MODE_ALTERNATE(HW_I2C_GPIO_AF) | - PAL_STM32_OTYPE_OPENDRAIN | - PAL_STM32_OSPEED_MID1 | - PAL_STM32_PUDR_PULLUP); + PAL_MODE_ALTERNATE(HW_I2C_GPIO_AF) | + PAL_STM32_OTYPE_OPENDRAIN | + PAL_STM32_OSPEED_MID1 | + PAL_STM32_PUDR_PULLUP); HW_I2C_DEV.state = I2C_STOP; i2cStart(&HW_I2C_DEV, &i2cfg); @@ -243,3 +256,33 @@ void hw_try_restore_i2c(void) { i2cReleaseBus(&HW_I2C_DEV); } } + +THD_FUNCTION(dac_thread, arg) { + (void)arg; + + chRegSetThreadName("DAC"); + + RCC_APB1PeriphClockCmd(RCC_APB1Periph_DAC, ENABLE); + DAC->CR |= DAC_CR_EN2; + + const float current_scaling_factor = MAX_CURRENT_SUM / 4096; + + float current_sum = 0; + + for(;;) { + current_sum = mc_interface_get_tot_current_in_filtered(); + + for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) { + can_status_msg_4 *msg4 = comm_can_get_status_msg_4_index(i); + if (msg4->id >= 0 && UTILS_AGE_S(msg4->rx_time) < 0.1) { + current_sum += msg4->current_in; + } + } + int scaled_current = (int)(round(current_sum/current_scaling_factor)); + + DAC->DHR12R2 = scaled_current; + + + chThdSleepMilliseconds(100); + } +} \ No newline at end of file diff --git a/hwconf/hw_uxv_sr.h b/hwconf/hw_uxv_sr.h index 907c2214..cf6a409b 100644 --- a/hwconf/hw_uxv_sr.h +++ b/hwconf/hw_uxv_sr.h @@ -103,7 +103,8 @@ #define NTC_TEMP(adc_ind) (1.0 / ((logf(NTC_RES(ADC_Value[adc_ind]) / 10000.0) / 3380.0) + (1.0 / 298.15)) - 273.15) #define NTC_RES_MOTOR(adc_val) (10000.0 / ((4095.0 / (float)adc_val) - 1.0)) // Motor temp sensor on low side -#define NTC_TEMP_MOTOR(beta) (1.0 / ((logf(NTC_RES_MOTOR(ADC_Value[ADC_IND_TEMP_MOTOR]) / 10000.0) / beta) + (1.0 / 298.15)) - 273.15) +//#define NTC_TEMP_MOTOR(beta) (1.0 / ((logf(NTC_RES_MOTOR(ADC_Value[ADC_IND_TEMP_MOTOR]) / 10000.0) / beta) + (1.0 / 298.15)) - 273.15) +#define NTC_TEMP_MOTOR(beta) (10000.0 / ((4095.0 / (float)0.5) - 1.0)) // Voltage on ADC channel #define ADC_VOLTS(ch) ((float)ADC_Value[ch] / 4096.0 * V_REG) @@ -202,6 +203,10 @@ //APP settings #define APPCONF_UAVCAN_ESC_INDEX (HW_DEFAULT_ID - 1) #define APPCONF_APP_TO_USE APP_PPM +#define APPCONF_SEND_CAN_STATUS_RATE_HZ 20 +#define APPCONF_CAN_BAUD_RATE CAN_BAUD_1M +#define APPCONF_SEND_CAN_STATUS CAN_STATUS_1_2_3_4 +#define MAX_CURRENT_SUM 1000 // Measurement macros #define ADC_V_L1 ADC_Value[ADC_IND_SENS1]