wideband/firmware/heater_thread.cpp at 2103e476a59255724c2f08ff2be57c0ad9fd24a9 · xtitoris/wideband · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
#include "ch.h"
#include "hal.h"
#include "pwm.h"

#include "heater_control.h"
#include "port.h"
#include "sampling.h"

// 400khz / 1024 = 390hz PWM
static Pwm heaterPwm(HEATER_PWM_DEVICE);
static const PWMConfig heaterPwmConfig = {
    .frequency = 400'000,
    .period = 1024,
    .callback = nullptr,
    .channels = {
        {PWM_OUTPUT_ACTIVE_HIGH | PWM_COMPLEMENTARY_OUTPUT_ACTIVE_LOW, nullptr},
        {PWM_OUTPUT_ACTIVE_HIGH | PWM_COMPLEMENTARY_OUTPUT_ACTIVE_LOW, nullptr},
        {PWM_OUTPUT_ACTIVE_HIGH | PWM_COMPLEMENTARY_OUTPUT_ACTIVE_LOW, nullptr},
        {PWM_OUTPUT_ACTIVE_HIGH | PWM_COMPLEMENTARY_OUTPUT_ACTIVE_LOW, nullptr}
    },
    .cr2 = 0,
#if STM32_PWM_USE_ADVANCED
    .bdtr = 0,
#endif
    .dier = 0
};

class HeaterController : public HeaterControllerBase {
public:
    HeaterController(int ch, int pwm_ch)
        : HeaterControllerBase(ch)
        , pwm_ch(pwm_ch)
    {
    }

    void SetDuty(float duty) const override
    {
        heaterPwm.SetDuty(pwm_ch, duty);
    }

// TODO: private:
public:
    const uint8_t pwm_ch;
};

HeaterController heaterControllers[AFR_CHANNELS] =
{
    { 0, HEATER_PWM_CHANNEL_0 },

#if AFR_CHANNELS >= 2
    { 1, HEATER_PWM_CHANNEL_1 },
#endif

#if AFR_CHANNELS >= 3
    { 2, HEATER_PWM_CHANNEL_2 },
#endif

#if AFR_CHANNELS >= 4
    { 3, HEATER_PWM_CHANNEL_3 },
#endif
};

const IHeaterController& GetHeaterController(int ch)
{
    return heaterControllers[ch];
}

static THD_WORKING_AREA(waHeaterThread, 256);
static void HeaterThread(void*)
{
    chRegSetThreadName("Heater");

    // Wait for temperature sensing to stabilize so we don't
    // immediately think we overshot the target temperature
    chThdSleepMilliseconds(1000);

    struct HeaterConfig* configuration = &GetConfiguration()->heaterConfig;

    // Configure heater controllers for sensor type
    for (int i = 0; i < AFR_CHANNELS; i++)
    {
        auto& h = heaterControllers[i];

        switch (GetSensorType())
        {
            case SensorType::LSU42:
                h.Configure(730, 80, configuration);
                break;
            case SensorType::LSUADV:
                h.Configure(785, 300, configuration);
                break;
            case SensorType::LSU49:
            default:
                h.Configure(780, 300, configuration);
                break;
        }
    }

    while (true)
    {
        auto heaterAllowState = GetHeaterAllowed();

        for (int i = 0; i < AFR_CHANNELS; i++)
        {
            const auto& sampler = GetSampler(i);
            auto& heater = heaterControllers[i];

            heater.Update(sampler, heaterAllowState);
        }

        // Loop at ~20hz
        chThdSleepMilliseconds(HEATER_CONTROL_PERIOD);
    }
}

void StartHeaterControl()
{
    heaterPwm.Start(heaterPwmConfig);

    for (int i = 0; i < AFR_CHANNELS; i++)
    {
        heaterPwm.SetDuty(heaterControllers[i].pwm_ch, 0);
    }

    chThdCreateStatic(waHeaterThread, sizeof(waHeaterThread), NORMALPRIO + 1, HeaterThread, nullptr);
}

float GetHeaterDuty(int ch)
{
    return heaterPwm.GetLastDuty(heaterControllers[ch].pwm_ch);
}

HeaterState GetHeaterState(int ch)
{
    return heaterControllers[ch].GetHeaterState();
}