Compare commits

...

3 Commits

Author SHA1 Message Date
45c0625864 fixup style issues 2020-08-31 22:58:00 +02:00
03e1ccf97e Fix style 2020-08-31 22:50:39 +02:00
0fd738f37e Fix startup code bug in release mode 2020-08-30 19:40:33 +02:00
7 changed files with 100 additions and 75 deletions

View File

@ -39,27 +39,28 @@ static float IN_SECTION(.ccm.bss) filter_alpha;
/** /**
* @brief Filtered PT1000 resistance value. * @brief Filtered PT1000 resistance value.
* @note This value is not yet calibrated. Use @ref adc_pt1000_get_current_resistance to get this value with calibration. * @note This value is not yet calibrated.
* Use @ref adc_pt1000_get_current_resistance to get this value with calibration.
*/ */
static volatile float IN_SECTION(.ccm.bss) pt1000_res_raw_lf; static volatile float IN_SECTION(.ccm.bss) pt1000_res_raw_lf;
static volatile int * volatile streaming_flag_ptr = NULL; static volatile int * volatile streaming_flag_ptr;
static uint32_t IN_SECTION(.ccm.bss) filter_startup_cnt; static uint32_t IN_SECTION(.ccm.bss) filter_startup_cnt;
static volatile float IN_SECTION(.ccm.bss) adc_pt1000_raw_reading_hf; static volatile float IN_SECTION(.ccm.bss) adc_pt1000_raw_reading_hf;
static volatile uint16_t dma_sample_buffer[ADC_PT1000_DMA_AVG_SAMPLES]; static volatile uint16_t dma_sample_buffer[ADC_PT1000_DMA_AVG_SAMPLES];
static volatile uint32_t IN_SECTION(.ccm.bss) adc_watchdog_counter = 0UL; static volatile uint32_t IN_SECTION(.ccm.bss) adc_watchdog_counter;
volatile float * volatile stream_buffer = NULL; volatile float * volatile stream_buffer;
volatile uint32_t stream_count; volatile uint32_t stream_count;
volatile uint32_t stream_pos; volatile uint32_t stream_pos;
static inline void adc_pt1000_stop_sample_frequency_timer() static inline void adc_pt1000_stop_sample_frequency_timer(void)
{ {
TIM2->CR1 &= ~TIM_CR1_CEN; TIM2->CR1 &= ~TIM_CR1_CEN;
rcc_manager_disable_clock(&RCC->APB1ENR, BITMASK_TO_BITNO(RCC_APB1ENR_TIM2EN)); rcc_manager_disable_clock(&RCC->APB1ENR, BITMASK_TO_BITNO(RCC_APB1ENR_TIM2EN));
} }
static inline void adc_pt1000_setup_sample_frequency_timer() static inline void adc_pt1000_setup_sample_frequency_timer(void)
{ {
rcc_manager_enable_clock(&RCC->APB1ENR, BITMASK_TO_BITNO(RCC_APB1ENR_TIM2EN)); rcc_manager_enable_clock(&RCC->APB1ENR, BITMASK_TO_BITNO(RCC_APB1ENR_TIM2EN));
@ -77,7 +78,7 @@ static inline void adc_pt1000_setup_sample_frequency_timer()
} }
static inline void adc_pt1000_disable_adc() static inline void adc_pt1000_disable_adc(void)
{ {
ADC_PT1000_PERIPH->CR2 &= ~ADC_CR2_ADON; ADC_PT1000_PERIPH->CR2 &= ~ADC_CR2_ADON;
DMA2_Stream0->CR = 0; DMA2_Stream0->CR = 0;
@ -99,7 +100,7 @@ static inline void adc_pt1000_disable_adc()
* After that, the moving average filter is fed with the values. * After that, the moving average filter is fed with the values.
* *
*/ */
static inline void adc_pt1000_enable_dma_stream() static inline void adc_pt1000_enable_dma_stream(void)
{ {
/* Enable peripheral clock for DMA2 */ /* Enable peripheral clock for DMA2 */
rcc_manager_enable_clock(&RCC->AHB1ENR, BITMASK_TO_BITNO(RCC_AHB1ENR_DMA2EN)); rcc_manager_enable_clock(&RCC->AHB1ENR, BITMASK_TO_BITNO(RCC_AHB1ENR_DMA2EN));
@ -124,7 +125,7 @@ static inline void adc_pt1000_enable_dma_stream()
DMA_SxCR_CIRC | DMA_SxCR_TCIE | DMA_SxCR_TEIE | DMA_SxCR_EN | ((ADC_PT1000_CHANNEL & 0x7)<<25); DMA_SxCR_CIRC | DMA_SxCR_TCIE | DMA_SxCR_TEIE | DMA_SxCR_EN | ((ADC_PT1000_CHANNEL & 0x7)<<25);
} }
static inline void adc_pt1000_disable_dma_stream() static inline void adc_pt1000_disable_dma_stream(void)
{ {
/* Disable the stream */ /* Disable the stream */
DMA2_Stream0->CR = 0; DMA2_Stream0->CR = 0;
@ -136,7 +137,7 @@ static inline void adc_pt1000_disable_dma_stream()
NVIC_DisableIRQ(DMA2_Stream0_IRQn); NVIC_DisableIRQ(DMA2_Stream0_IRQn);
} }
void adc_pt1000_setup_meas() void adc_pt1000_setup_meas(void)
{ {
rcc_manager_enable_clock(&RCC->APB2ENR, BITMASK_TO_BITNO(RCC_APB2ENR_ADC3EN)); rcc_manager_enable_clock(&RCC->APB2ENR, BITMASK_TO_BITNO(RCC_APB2ENR_ADC3EN));
rcc_manager_enable_clock(&RCC->AHB1ENR, BITMASK_TO_BITNO(ADC_PT1000_PORT_RCC_MASK)); rcc_manager_enable_clock(&RCC->AHB1ENR, BITMASK_TO_BITNO(ADC_PT1000_PORT_RCC_MASK));
@ -163,7 +164,8 @@ void adc_pt1000_setup_meas()
ADC_PT1000_PERIPH->SQR3 = (ADC_PT1000_CHANNEL<<0); ADC_PT1000_PERIPH->SQR3 = (ADC_PT1000_CHANNEL<<0);
ADC_PT1000_PERIPH->CR1 = ADC_CR1_OVRIE | ADC_CR1_AWDEN | ADC_CR1_AWDIE; ADC_PT1000_PERIPH->CR1 = ADC_CR1_OVRIE | ADC_CR1_AWDEN | ADC_CR1_AWDIE;
ADC_PT1000_PERIPH->CR2 = ADC_CR2_EXTEN_0 | ADC_CR2_EXTSEL_2 | ADC_CR2_EXTSEL_1 | ADC_CR2_ADON | ADC_CR2_DMA | ADC_CR2_DDS; ADC_PT1000_PERIPH->CR2 = ADC_CR2_EXTEN_0 | ADC_CR2_EXTSEL_2 | ADC_CR2_EXTSEL_1 |
ADC_CR2_ADON | ADC_CR2_DMA | ADC_CR2_DDS;
adc_pt1000_set_moving_average_filter_param(ADC_PT1000_FILTER_WEIGHT); adc_pt1000_set_moving_average_filter_param(ADC_PT1000_FILTER_WEIGHT);
adc_pt1000_set_resistance_calibration(0, 0, false); adc_pt1000_set_resistance_calibration(0, 0, false);
@ -176,6 +178,10 @@ void adc_pt1000_setup_meas()
adc_pt1000_setup_sample_frequency_timer(); adc_pt1000_setup_sample_frequency_timer();
safety_controller_ack_flag_with_key(ERR_FLAG_MEAS_ADC_OFF, MEAS_ADC_SAFETY_FLAG_KEY); safety_controller_ack_flag_with_key(ERR_FLAG_MEAS_ADC_OFF, MEAS_ADC_SAFETY_FLAG_KEY);
streaming_flag_ptr = NULL;
adc_watchdog_counter = 0UL;
stream_buffer = NULL;
} }
void adc_pt1000_set_moving_average_filter_param(float alpha) void adc_pt1000_set_moving_average_filter_param(float alpha)
@ -191,11 +197,10 @@ void adc_pt1000_set_resistance_calibration(float offset, float sensitivity_devia
pt1000_sens_dev = sensitivity_deviation; pt1000_sens_dev = sensitivity_deviation;
calibration_active = active; calibration_active = active;
if (!calibration_active) { if (!calibration_active)
safety_controller_report_error_with_key(ERR_FLAG_UNCAL, MEAS_ADC_SAFETY_FLAG_KEY); safety_controller_report_error_with_key(ERR_FLAG_UNCAL, MEAS_ADC_SAFETY_FLAG_KEY);
} else { else
safety_controller_ack_flag_with_key(ERR_FLAG_UNCAL, MEAS_ADC_SAFETY_FLAG_KEY); safety_controller_ack_flag_with_key(ERR_FLAG_UNCAL, MEAS_ADC_SAFETY_FLAG_KEY);
}
} }
void adc_pt1000_get_resistance_calibration(float *offset, float *sensitivity_deviation, bool *active) void adc_pt1000_get_resistance_calibration(float *offset, float *sensitivity_deviation, bool *active)
@ -280,7 +285,7 @@ void adc_pt1000_convert_raw_value_array_to_resistance(float *resistance_dest, fl
resistance_dest[i] = ADC_TO_RES(raw_source[i]); resistance_dest[i] = ADC_TO_RES(raw_source[i]);
} }
void adc_pt1000_disable() void adc_pt1000_disable(void)
{ {
adc_pt1000_disable_adc(); adc_pt1000_disable_adc();
adc_pt1000_stop_sample_frequency_timer(); adc_pt1000_stop_sample_frequency_timer();
@ -300,16 +305,15 @@ static inline __attribute__((optimize("O3"))) void adc_pt1000_filter(float adc_p
{ {
if (filter_startup_cnt > 0) { if (filter_startup_cnt > 0) {
filter_startup_cnt--; filter_startup_cnt--;
if (filter_startup_cnt == 0) { if (filter_startup_cnt == 0)
safety_controller_ack_flag_with_key(ERR_FLAG_MEAS_ADC_UNSTABLE, MEAS_ADC_SAFETY_FLAG_KEY); safety_controller_ack_flag_with_key(ERR_FLAG_MEAS_ADC_UNSTABLE, MEAS_ADC_SAFETY_FLAG_KEY);
}
} }
pt1000_res_raw_lf = (1.0f-filter_alpha) * pt1000_res_raw_lf + filter_alpha * ADC_TO_RES(adc_prefiltered_value); pt1000_res_raw_lf = (1.0f-filter_alpha) * pt1000_res_raw_lf + filter_alpha * ADC_TO_RES(adc_prefiltered_value);
safety_controller_report_timing(ERR_TIMING_MEAS_ADC); safety_controller_report_timing(ERR_TIMING_MEAS_ADC);
} }
static inline __attribute__((optimize("O3"))) float adc_pt1000_dma_avg_pre_filter() static inline __attribute__((optimize("O3"))) float adc_pt1000_dma_avg_pre_filter(void)
{ {
unsigned int i; unsigned int i;
uint32_t sum = 0; uint32_t sum = 0;
@ -371,7 +375,7 @@ static void append_stream_buffer(float val)
} }
void DMA2_Stream0_IRQHandler() void DMA2_Stream0_IRQHandler(void)
{ {
uint32_t lisr; uint32_t lisr;
float adc_val; float adc_val;
@ -394,8 +398,7 @@ void DMA2_Stream0_IRQHandler()
adc_pt1000_filter(adc_val); adc_pt1000_filter(adc_val);
} }
if (lisr & DMA_LISR_TEIF0) { if (lisr & DMA_LISR_TEIF0)
adc_pt1000_disable(); adc_pt1000_disable();
}
} }

View File

@ -18,6 +18,8 @@
* ------------------------------------------------------------------------ * ------------------------------------------------------------------------
*/ */
#include <stdint.h>
/* C++ library init */ /* C++ library init */
# if defined(__cplusplus) # if defined(__cplusplus)
extern "C" { extern "C" {
@ -280,9 +282,22 @@ extern unsigned int __ld_ebss;
extern unsigned int __ld_sheap; extern unsigned int __ld_sheap;
extern unsigned int __ld_eheap; extern unsigned int __ld_eheap;
#ifdef CPACR
#undef CPACR
#endif
#define CPACR (*((volatile uint32_t *)0xE000ED88))
void Reset_Handler(void) { void Reset_Handler(void) {
/* Stack is already initialized by hardware */ /* Stack is already initialized by hardware */
/* The first thing we do here, is to initialize the FPU
* When this code is compiled optimized with hardfpu abi,
* GCC tends to generate FPU instructions for data copying
*/
CPACR |= (0xF << 20);
/* Copy .data section */ /* Copy .data section */
__init_section(&__ld_load_data, &__ld_sdata, &__ld_edata); __init_section(&__ld_load_data, &__ld_sdata, &__ld_edata);
/* Fill bss with zero */ /* Fill bss with zero */

View File

@ -47,7 +47,7 @@ static void digio_setup_pin_int(uint8_t bit_no, uint8_t in_out, uint8_t alt_func
} }
void digio_setup_default_all() void digio_setup_default_all(void)
{ {
unsigned int i; unsigned int i;
@ -90,7 +90,7 @@ int digio_get(uint8_t num)
static const uint8_t led_pins[] = {LED_PINS}; static const uint8_t led_pins[] = {LED_PINS};
void led_setup() void led_setup(void)
{ {
unsigned int i; unsigned int i;
@ -132,7 +132,7 @@ static void loudspeaker_freq_timer_init(void)
#endif #endif
} }
void loudspeaker_setup() void loudspeaker_setup(void)
{ {
rcc_manager_enable_clock(&RCC->AHB1ENR, BITMASK_TO_BITNO(LOUDSPEAKER_RCC_MASK)); rcc_manager_enable_clock(&RCC->AHB1ENR, BITMASK_TO_BITNO(LOUDSPEAKER_RCC_MASK));
@ -179,7 +179,7 @@ void loudspeaker_set(uint16_t val)
} }
} }
uint16_t loudspeaker_get() uint16_t loudspeaker_get(void)
{ {
return loudspeaker_val; return loudspeaker_val;
} }

View File

@ -109,7 +109,7 @@
* The filter weight \f$\alpha\f$ is configured for @ref ADC_PT1000_FILTER_WEIGHT * The filter weight \f$\alpha\f$ is configured for @ref ADC_PT1000_FILTER_WEIGHT
* *
*/ */
void adc_pt1000_setup_meas(); void adc_pt1000_setup_meas(void);
/** /**
* @brief Set moving average filter parameters * @brief Set moving average filter parameters
@ -180,6 +180,6 @@ void adc_pt1000_convert_raw_value_array_to_resistance(float *resistance_dest, fl
/** /**
* @brief Disable the PT1000 measurement * @brief Disable the PT1000 measurement
*/ */
void adc_pt1000_disable(); void adc_pt1000_disable(void);
#endif // __ADCMEAS_H__ #endif // __ADCMEAS_H__

View File

@ -41,7 +41,7 @@ void oven_pid_init(struct pid_controller *controller_to_copy);
void oven_pid_handle(float target_temp); void oven_pid_handle(float target_temp);
void oven_pid_stop(); void oven_pid_stop(void);
void oven_driver_apply_power_level(void); void oven_driver_apply_power_level(void);

View File

@ -1,22 +1,22 @@
/* Reflow Oven Controller /* Reflow Oven Controller
* *
* Copyright (C) 2020 Mario Hüttel <mario.huettel@gmx.net> * Copyright (C) 2020 Mario Hüttel <mario.huettel@gmx.net>
* *
* This file is part of the Reflow Oven Controller Project. * This file is part of the Reflow Oven Controller Project.
* *
* The reflow oven controller is free software: you can redistribute it and/or modify * The reflow oven controller is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as * it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation. * published by the Free Software Foundation.
* *
* The Reflow Oven Control Firmware is distributed in the hope that it will be useful, * The Reflow Oven Control Firmware is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details. * GNU General Public License for more details.
* *
* You should have received a copy of the GNU General Public License * You should have received a copy of the GNU General Public License
* along with the reflow oven controller project. * along with the reflow oven controller project.
* If not, see <http://www.gnu.org/licenses/>. * If not, see <http://www.gnu.org/licenses/>.
*/ */
#include <reflow-controller/oven-driver.h> #include <reflow-controller/oven-driver.h>
#include <reflow-controller/periph-config/oven-driver-hwcfg.h> #include <reflow-controller/periph-config/oven-driver-hwcfg.h>
@ -28,11 +28,11 @@
#include <reflow-controller/safety/safety-controller.h> #include <reflow-controller/safety/safety-controller.h>
static struct pid_controller IN_SECTION(.ccm.bss) oven_pid; static struct pid_controller IN_SECTION(.ccm.bss) oven_pid;
static bool oven_pid_running = false; static bool oven_pid_running;
static bool oven_pid_aborted = false; static bool oven_pid_aborted;
static uint8_t IN_SECTION(.ccm.bss) oven_driver_power_level = 0U; static uint8_t IN_SECTION(.ccm.bss) oven_driver_power_level;
void oven_driver_init() void oven_driver_init(void)
{ {
rcc_manager_enable_clock(&RCC->AHB1ENR, BITMASK_TO_BITNO(OVEN_CONTROLLER_PORT_RCC_MASK)); rcc_manager_enable_clock(&RCC->AHB1ENR, BITMASK_TO_BITNO(OVEN_CONTROLLER_PORT_RCC_MASK));
rcc_manager_enable_clock(&RCC->APB1ENR, BITMASK_TO_BITNO(OVEN_CONTROLLER_TIM_RCC_MASK)); rcc_manager_enable_clock(&RCC->APB1ENR, BITMASK_TO_BITNO(OVEN_CONTROLLER_TIM_RCC_MASK));
@ -50,6 +50,12 @@ void oven_driver_init()
OVEN_CONTROLLER_PWM_TIMER->PSC = 42000U - 1U; OVEN_CONTROLLER_PWM_TIMER->PSC = 42000U - 1U;
OVEN_CONTROLLER_PWM_TIMER->CR1 = TIM_CR1_CMS | TIM_CR1_CEN; OVEN_CONTROLLER_PWM_TIMER->CR1 = TIM_CR1_CMS | TIM_CR1_CEN;
/* Explicitly init global variables */
oven_pid_aborted = false;
oven_pid_running = false;
oven_driver_set_power(0U);
} }
void oven_driver_set_power(uint8_t power) void oven_driver_set_power(uint8_t power)
@ -65,7 +71,7 @@ void oven_driver_apply_power_level(void)
OVEN_CONTROLLER_PWM_TIMER->CCR3 = oven_driver_power_level * 10; OVEN_CONTROLLER_PWM_TIMER->CCR3 = oven_driver_power_level * 10;
} }
void oven_driver_disable() void oven_driver_disable(void)
{ {
OVEN_CONTROLLER_PWM_TIMER->CR1 = 0UL; OVEN_CONTROLLER_PWM_TIMER->CR1 = 0UL;
OVEN_CONTROLLER_PWM_TIMER->CR2 = 0UL; OVEN_CONTROLLER_PWM_TIMER->CR2 = 0UL;
@ -105,9 +111,10 @@ void oven_pid_handle(float target_temp)
} }
} }
void oven_pid_stop() void oven_pid_stop(void)
{ {
oven_pid_running = false; oven_pid_running = false;
oven_driver_set_power(0U);
safety_controller_enable_timing_mon(ERR_TIMING_PID, false); safety_controller_enable_timing_mon(ERR_TIMING_PID, false);
} }

View File

@ -1,27 +1,28 @@
/* Reflow Oven Controller /* Reflow Oven Controller
* *
* Copyright (C) 2020 Mario Hüttel <mario.huettel@gmx.net> * Copyright (C) 2020 Mario Hüttel <mario.huettel@gmx.net>
* *
* This file is part of the Reflow Oven Controller Project. * This file is part of the Reflow Oven Controller Project.
* *
* The reflow oven controller is free software: you can redistribute it and/or modify * The reflow oven controller is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as * it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation. * published by the Free Software Foundation.
* *
* The Reflow Oven Control Firmware is distributed in the hope that it will be useful, * The Reflow Oven Control Firmware is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details. * GNU General Public License for more details.
* *
* You should have received a copy of the GNU General Public License * You should have received a copy of the GNU General Public License
* along with the reflow oven controller project. * along with the reflow oven controller project.
* If not, see <http://www.gnu.org/licenses/>. * If not, see <http://www.gnu.org/licenses/>.
*/ */
#include <reflow-controller/pid-controller.h> #include <reflow-controller/pid-controller.h>
#include <string.h> #include <string.h>
void pid_init(struct pid_controller *pid, float k_deriv, float k_int, float k_p, float output_sat_min, float output_sat_max, float integral_max, float sample_period) void pid_init(struct pid_controller *pid, float k_deriv, float k_int, float k_p, float output_sat_min,
float output_sat_max, float integral_max, float sample_period)
{ {
if (!pid) if (!pid)
return; return;
@ -61,11 +62,10 @@ static void calculate_integral(struct pid_controller *pid, float deviation)
pid->integral = pid->integral + pid->k_int_t * (deviation + pid->last_in); pid->integral = pid->integral + pid->k_int_t * (deviation + pid->last_in);
/* Saturate integral term to spoecified maximum */ /* Saturate integral term to spoecified maximum */
if (pid->integral > pid->integral_max) { if (pid->integral > pid->integral_max)
pid->integral = pid->integral_max; pid->integral = pid->integral_max;
} else if (pid->integral < -pid->integral_max){ else if (pid->integral < -pid->integral_max)
pid->integral = - pid->integral_max; pid->integral = -pid->integral_max;
}
} }
float pid_sample(struct pid_controller *pid, float deviation) float pid_sample(struct pid_controller *pid, float deviation)