Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : temperature calibration library
17 : */
18 :
19 : #include "AP_TempCalibration_config.h"
20 :
21 : #if AP_TEMPCALIBRATION_ENABLED
22 :
23 : #include "AP_TempCalibration.h"
24 : #include <stdio.h>
25 : #include <AP_Baro/AP_Baro.h>
26 :
27 : extern const AP_HAL::HAL& hal;
28 :
29 : #define TCAL_DEBUG 0
30 :
31 : #if TCAL_DEBUG
32 : # define debug(fmt, args ...) do {printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
33 : #else
34 : # define debug(fmt, args ...)
35 : #endif
36 :
37 : #ifndef HAL_TCAL_BARO_EXP_DEFAULT
38 : #define HAL_TCAL_BARO_EXP_DEFAULT 0.0f
39 : #endif
40 :
41 : // table of user settable and learned parameters
42 : const AP_Param::GroupInfo AP_TempCalibration::var_info[] = {
43 :
44 : // @Param: _ENABLED
45 : // @DisplayName: Temperature calibration enable
46 : // @Description: Enable temperature calibration. Set to 0 to disable. Set to 1 to use learned values. Set to 2 to learn new values and use the values
47 : // @Values: 0:Disabled,1:Enabled,2:EnableAndLearn
48 : // @User: Advanced
49 : AP_GROUPINFO_FLAGS("_ENABLED", 1, AP_TempCalibration, enabled, TC_DISABLED, AP_PARAM_FLAG_ENABLE),
50 :
51 : // @Param: _TEMP_MIN
52 : // @DisplayName: Temperature calibration min learned temperature
53 : // @Description: Minimum learned temperature. This is automatically set by the learning process
54 : // @Units: degC
55 : // @ReadOnly: True
56 : // @Volatile: True
57 : // @User: Advanced
58 : AP_GROUPINFO("_TEMP_MIN", 2, AP_TempCalibration, temp_min, 0),
59 :
60 : // 3 was used by a duplicated temp_min entry (do not use in the future!)
61 :
62 : // @Param: _TEMP_MAX
63 : // @DisplayName: Temperature calibration max learned temperature
64 : // @Description: Maximum learned temperature. This is automatically set by the learning process
65 : // @Units: degC
66 : // @ReadOnly: True
67 : // @Volatile: True
68 : // @User: Advanced
69 : AP_GROUPINFO("_TEMP_MAX", 4, AP_TempCalibration, temp_max, 0),
70 :
71 : // @Param: _BARO_EXP
72 : // @DisplayName: Temperature Calibration barometer exponent
73 : // @Description: Learned exponent for barometer temperature correction
74 : // @ReadOnly: True
75 : // @Volatile: True
76 : // @User: Advanced
77 : AP_GROUPINFO("_BARO_EXP", 5, AP_TempCalibration, baro_exponent, HAL_TCAL_BARO_EXP_DEFAULT),
78 :
79 : AP_GROUPEND
80 : };
81 :
82 : /*
83 : calculate the correction given an exponent and a temperature
84 :
85 : This one parameter correction is deliberately chosen to be very
86 : robust for extrapolation. It fits the characteristics of the
87 : ICM-20789 barometer nicely.
88 : */
89 0 : float AP_TempCalibration::calculate_correction(float temp, float exponent) const
90 : {
91 0 : return powf(MAX(temp - Tzero, 0), exponent);
92 : }
93 :
94 :
95 : /*
96 : setup for learning
97 : */
98 0 : void AP_TempCalibration::setup_learning(void)
99 : {
100 0 : learn_temp_start = AP::baro().get_temperature();
101 0 : learn_temp_step = 0.25;
102 0 : learn_count = 200;
103 0 : learn_i = 0;
104 0 : delete [] learn_values;
105 0 : learn_values = NEW_NOTHROW float[learn_count];
106 0 : if (learn_values == nullptr) {
107 0 : return;
108 : }
109 : }
110 :
111 : /*
112 : calculate the sum of squares range of pressure values we get with
113 : the current data. This is the function we try to minimise in the
114 : calibration
115 : */
116 0 : float AP_TempCalibration::calculate_p_range(float baro_factor) const
117 : {
118 0 : float sum = 0;
119 0 : float P0 = learn_values[0] + calculate_correction(learn_temp_start, baro_factor);
120 0 : for (uint16_t i=0; i<learn_i; i++) {
121 0 : if (is_zero(learn_values[i])) {
122 : // gap in the data
123 0 : continue;
124 : }
125 0 : float temp = learn_temp_start + learn_temp_step*i;
126 0 : float correction = calculate_correction(temp, baro_factor);
127 0 : float P = learn_values[i] + correction;
128 0 : sum += sq(P - P0);
129 : }
130 0 : return sum / learn_i;
131 : }
132 :
133 : /*
134 : calculate a calibration value
135 :
136 : This fits a simple single value power function to the baro data to
137 : find the calibration exponent.
138 : */
139 0 : void AP_TempCalibration::calculate_calibration(void)
140 : {
141 0 : float current_err = calculate_p_range(baro_exponent);
142 0 : float test_exponent = baro_exponent + learn_delta;
143 0 : float test_err = calculate_p_range(test_exponent);
144 0 : if (test_err >= current_err) {
145 0 : test_exponent = baro_exponent - learn_delta;
146 0 : test_err = calculate_p_range(test_exponent);
147 : }
148 0 : if (test_exponent <= exp_limit_max &&
149 0 : test_exponent >= exp_limit_min &&
150 : test_err < current_err) {
151 : // move to new value
152 : debug("CAL: %.2f\n", test_exponent);
153 0 : if (!is_equal(test_exponent, baro_exponent.get())) {
154 0 : baro_exponent.set_and_save(test_exponent);
155 : }
156 0 : temp_min.set_and_save_ifchanged(learn_temp_start);
157 0 : temp_max.set_and_save_ifchanged(learn_temp_start + learn_i*learn_temp_step);
158 : }
159 0 : }
160 :
161 : /*
162 : update calibration learning
163 : */
164 0 : void AP_TempCalibration::learn_calibration(void)
165 : {
166 : // just for first baro now
167 0 : const AP_Baro &baro = AP::baro();
168 0 : if (!baro.healthy(0) ||
169 0 : hal.util->get_soft_armed() ||
170 0 : baro.get_temperature(0) < Tzero) {
171 0 : return;
172 : }
173 :
174 : // if we have any movement then we reset learning
175 0 : if (learn_values == nullptr ||
176 0 : !AP::ins().is_still()) {
177 : debug("learn reset\n");
178 0 : setup_learning();
179 0 : if (learn_values == nullptr) {
180 0 : return;
181 : }
182 : }
183 0 : float temp = baro.get_temperature(0);
184 0 : float P = baro.get_pressure(0);
185 0 : uint16_t idx = (temp - learn_temp_start) / learn_temp_step;
186 0 : if (idx >= learn_count) {
187 : // could change learn_temp_step here
188 0 : return;
189 : }
190 0 : if (is_zero(learn_values[idx])) {
191 0 : learn_values[idx] = P;
192 : debug("learning %u %.2f at %.2f\n", idx, learn_values[idx], temp);
193 : } else {
194 : // filter in new value
195 0 : learn_values[idx] = 0.9 * learn_values[idx] + 0.1 * P;
196 : }
197 0 : learn_i = MAX(learn_i, idx);
198 :
199 0 : uint32_t now = AP_HAL::millis();
200 0 : if (now - last_learn_ms > 100 &&
201 0 : idx*learn_temp_step > min_learn_temp_range &&
202 0 : temp - learn_temp_start > temp_max - temp_min) {
203 0 : last_learn_ms = now;
204 : // run estimation and update parameters
205 0 : calculate_calibration();
206 : }
207 : }
208 :
209 : /*
210 : apply learned calibration for current temperature
211 : */
212 0 : void AP_TempCalibration::apply_calibration(void)
213 : {
214 0 : AP_Baro &baro = AP::baro();
215 : // just for first baro now
216 0 : if (!baro.healthy(0)) {
217 0 : return;
218 : }
219 0 : float temp = baro.get_temperature(0);
220 0 : float correction = calculate_correction(temp, baro_exponent);
221 0 : baro.set_pressure_correction(0, correction);
222 : }
223 :
224 : /*
225 : called at 10Hz from the main thread. This is called both when armed
226 : and disarmed. It only does learning while disarmed, but needs to
227 : supply the corrections to the sensor libraries at all times
228 : */
229 759507 : void AP_TempCalibration::update(void)
230 : {
231 759507 : switch (enabled.get()) {
232 759507 : case TC_DISABLED:
233 759507 : break;
234 0 : case TC_ENABLE_LEARN:
235 0 : learn_calibration();
236 : FALLTHROUGH;
237 0 : case TC_ENABLE_USE:
238 0 : apply_calibration();
239 0 : break;
240 : }
241 759507 : }
242 :
243 : #endif // AP_TEMPCALIBRATION_ENABLED
|
