Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
meteo_stick_calib.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Gautier Hattenberger, Alexandre Bustico
3  *
4  * This file is part of paparazzi
5 
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  *
20  */
21 
31 #include <math.h>
32 
33 typedef struct {
34  float ref;
35  float value;
36  float rawValue;
38 
39 // local functions declaration
40 static bool mtostk_populate_sensor_from_buffer(Sensors_params *sp, uint32_t **eeprom_buffer);
41 static bool mtostk_populate_float_array_from_buffer(float *ar,
42  uint16_t *num_of_array, uint16_t *num_of_elem,
43  uint32_t **eeprom_buffer);
44 static bool mtostk_populate_uuid_from_buffer(char *str, uint32_t **eeprom_buffer);
45 static bool mtostk_populate_uint_from_buffer(uint32_t *sca, uint32_t **eeprom_buffer);
46 static bool mtostk_seek_array_buffer(size_t dataSize, uint32_t **eeprom_buffer);
47 static bool mtostk_seek_scalar_buffer(size_t dataSize, uint32_t **eeprom_buffer);
48 static uint32_t fletcher32(uint32_t const *_data, size_t words);
49 static float mtostk_apply_polynomial(float *coeffs, uint16_t num_coef, float value);
50 static float mtostk_apply_polynomial_temp(Sensors_params *params, float temp, float value);
51 
52 // Populate calib struct
54 {
55  uint32_t *eeprom_buffer_ptr = (uint32_t *) _eeprom_buffer;
56 
57  if (mtostk_populate_uuid_from_buffer(cp->uuid, &eeprom_buffer_ptr) == false) {
58  return false;
59  }
60 
61  for (Mtostk_sensors ms = MTOSTK_TEMP; ms < MTOSTK_NUM_SENSORS; ms++) {
62  if (mtostk_populate_sensor_from_buffer(&cp->params[ms], &eeprom_buffer_ptr) == false) {
63  return false;
64  }
65  }
66 
67  return true;
68 }
69 
70 // Get calibrated data
71 float mtostk_get_calibrated_value(Calibration_params *cp, Mtostk_sensors type, float uncal, float temp)
72 {
73  if (type >= MTOSTK_NUM_SENSORS) { return 0; } // Invalid number
74 
75  Sensors_params *params = &(cp->params[type]);
76  if (type == MTOSTK_TEMP) {
77  // sanity check
78  if (params->num_temp == 1 && params->timestamp != 0) {
79  return mtostk_apply_polynomial(params->coeffs[0], params->num_coeff, uncal);
80  } else {
81  return uncal;
82  }
83  } else {
84  return mtostk_apply_polynomial_temp(params, temp, uncal);
85  }
86 
87 }
88 
90 {
91  uint32_t storeChksum = 0, recordSize;
92  const uint32_t *initialBufPtr = *eeprom_buffer;
93 
94  mtostk_populate_uint_from_buffer(&recordSize, eeprom_buffer);
95  mtostk_populate_uint_from_buffer((uint32_t *) & (sp->timestamp), eeprom_buffer);
96 
97  mtostk_seek_scalar_buffer(sizeof(float), eeprom_buffer); // we don't need maxStdDev
98  mtostk_populate_float_array_from_buffer((float *) sp->temps, NULL, NULL, eeprom_buffer);
99  mtostk_populate_float_array_from_buffer((float *) sp->coeffs, &sp->num_temp, &sp->num_coeff, eeprom_buffer);
100  mtostk_seek_array_buffer(sizeof(CalibrationPoint), eeprom_buffer); // we don't need calibrationPoints
101 
102  const size_t chksumBufLen = *eeprom_buffer - initialBufPtr;
103  const uint32_t calcChksum = fletcher32(initialBufPtr, chksumBufLen);
104  mtostk_populate_uint_from_buffer(&storeChksum, eeprom_buffer);
105 
106  if (calcChksum != storeChksum) {
107  return false;
108  }
109 
110  return true;
111 }
112 
113 static bool mtostk_populate_uint_from_buffer(uint32_t *sca, uint32_t **eeprom_buffer)
114 {
115  if (*eeprom_buffer == NULL) {
116  return false;
117  }
118 
119  *sca = *(*eeprom_buffer)++;
120  return true;
121 }
122 
124  uint16_t *num_of_array, uint16_t *num_of_elem,
125  uint32_t **eeprom_buffer)
126 {
127  if (*eeprom_buffer == NULL) {
128  return false;
129  }
130  uint16_t noa = 0, noe = 0;
131 
132  if (num_of_array == NULL) {
133  num_of_array = &noa;
134  }
135 
136  if (num_of_elem == NULL) {
137  num_of_elem = &noe;
138  } else {
139  *num_of_elem = 0;
140  }
141 
142  float **eeprom_buffer_float = (float **) eeprom_buffer;
143 
144  _Static_assert(sizeof(uint32_t) == sizeof(mtostk_time_t),
145  "sizeof (uint32_t) differ from sizeof (mtostk_time_t)");
146  _Static_assert(sizeof(uint32_t) == sizeof(float),
147  "sizeof (uint32_t) differ from sizeof (float)");
148 
149  *num_of_array = *(*eeprom_buffer)++;
150  if (*num_of_array > MTOSTK_MAX_TEMP_ARRAY_SIZE) {
151  return false;
152  }
153 
154  for (size_t idx = 0; idx < *num_of_array; idx++) {
155  const size_t this_num_of_elem = *(*eeprom_buffer)++;
156  if (*num_of_elem == 0) {
157  *num_of_elem = this_num_of_elem;
158  } else {
159  if (*num_of_elem != this_num_of_elem) {
160  return false;
161  }
162  }
163 
164  if (*num_of_elem > MTOSTK_MAX_POLY_ARRAY_SIZE) {
165  return false;
166  }
167 
168  for (int32_t jdx = 0; jdx < *num_of_elem; jdx++) {
169  ar[(idx * MTOSTK_MAX_POLY_ARRAY_SIZE) + jdx] = *(*eeprom_buffer_float)++;
170  }
171  }
172 
173  return true;
174 }
175 
176 static bool mtostk_populate_uuid_from_buffer(char *str, uint32_t **eeprom_buffer)
177 {
178  uint8_t **eeprom_buffer_byte = (uint8_t **) eeprom_buffer;
179  if (*eeprom_buffer == NULL) {
180  return false;
181  }
182 
183  const size_t numOfArrays = *(*eeprom_buffer)++;
184  if (numOfArrays > 1) {
185  return false;
186  }
187 
188  const size_t numOfElems = *(*eeprom_buffer)++;
189  if (numOfElems > UUID_LEN) {
190  return false;
191  }
192 
193  for (uint32_t idx = 0; idx < numOfArrays * numOfElems; idx++) {
194  *str++ = *(*eeprom_buffer_byte)++;
195  }
196 
197  return true;
198 }
199 
200 static bool mtostk_seek_array_buffer(size_t dataSize, uint32_t **eeprom_buffer)
201 {
202  uint8_t **eeprom_buffer_byte = (uint8_t **) eeprom_buffer;
203 
204  if (*eeprom_buffer == NULL) {
205  return false;
206  }
207 
208  const size_t numOfArrays = *(*eeprom_buffer)++;
209  if (numOfArrays > MTOSTK_MAX_TEMP_ARRAY_SIZE) {
210  return false;
211  }
212 
213  for (size_t i = 0; i < numOfArrays; i++) {
214  const size_t numOfElems = *(*eeprom_buffer)++;
215  if (numOfElems > MTOSTK_MAX_SEEK_ARRAY_SIZE) {
216  return false;
217  }
218  *eeprom_buffer_byte += numOfElems * dataSize;
219  // *eeprom_buffer = (uint32_t *) *eeprom_buffer_byte;
220  }
221 
222  return true;
223 }
224 
225 static bool mtostk_seek_scalar_buffer(size_t dataSize, uint32_t **eeprom_buffer)
226 {
227  uint8_t **eeprom_buffer_byte = (uint8_t **) eeprom_buffer;
228  *eeprom_buffer_byte += dataSize;
229  return true;
230 }
231 
232 
233 static uint32_t fletcher32(uint32_t const *_data, size_t words)
234 {
235  words += words;
236  uint16_t const *data = (uint16_t *) _data;
237 
238  uint32_t sum1 = 0xffff, sum2 = 0xffff;
239 
240  while (words) {
241  unsigned tlen = words > 359 ? 359 : words;
242  words -= tlen;
243  do {
244  sum2 += sum1 += *data++;
245  } while (--tlen);
246  sum1 = (sum1 & 0xffff) + (sum1 >> 16);
247  sum2 = (sum2 & 0xffff) + (sum2 >> 16);
248  }
249  /* Second reduction step to reduce sums to 16 bits */
250  sum1 = (sum1 & 0xffff) + (sum1 >> 16);
251  sum2 = (sum2 & 0xffff) + (sum2 >> 16);
252  return sum2 << 16 | sum1;
253 }
254 
255 float mtostk_apply_polynomial(float *coeffs, uint16_t num_coef, float value)
256 {
257  int i;
258  float y = coeffs[0];
259  for (i = 1; i < num_coef; i++) {
260  y += coeffs[i] * powf(value, (float) i);
261  }
262  return y;
263 }
264 
265 float mtostk_apply_polynomial_temp(Sensors_params *params, float temp, float value)
266 {
267  /* if not temperature compensated, just apply polynomial if any
268  * if temperature compensated
269  * get current corrected temp
270  * if out of bound, use closest bound
271  * else
272  * find correct interval
273  * compute the two values based on bounding polynomials
274  * make a linear interpolation of these two value
275  */
276  if (params->num_temp == 0 || params->timestamp == 0) {
277  return value; // No calibration available
278  } else if (params->num_temp == 1) {
279  return mtostk_apply_polynomial(params->coeffs[0], params->num_coeff, value); // No interpolation
280  } else {
281  if (temp <= params->temps[0]) {
282  return mtostk_apply_polynomial(params->coeffs[0], params->num_coeff, value);
283  } else if (temp >= params->temps[params->num_temp - 1]) {
284  return mtostk_apply_polynomial(params->coeffs[params->num_temp - 1], params->num_coeff, value);
285  } else {
286  int i;
287  for (i = 0; i <= params->num_temp - 2; i++) {
288  const float t1 = params->temps[i];
289  const float t2 = params->temps[i + 1];
290  if (temp > t1 && temp <= t2) {
291  const float v1 = mtostk_apply_polynomial(params->coeffs[i], params->num_coeff, value);
292  const float v2 = mtostk_apply_polynomial(params->coeffs[i + 1], params->num_coeff, value);
293  // Linear interpolation
294  const float alpha = (t2 - temp) / (t2 - t1);
295  return (alpha * v1 + (1.0f - alpha) * v2);
296  }
297  }
298  return value; // This should never append
299  }
300  }
301 
302 }
303 
static uint16_t words[4]
Definition: baro_MS5534A.c:62
_Static_assert(sizeof(DshotEPeriodPacket)==sizeof(uint16_t), "DshotEPeriodPacket size error")
float t1
Definition: ins_ext_pose.c:345
static bool mtostk_populate_uuid_from_buffer(char *str, uint32_t **eeprom_buffer)
static bool mtostk_seek_scalar_buffer(size_t dataSize, uint32_t **eeprom_buffer)
static float mtostk_apply_polynomial(float *coeffs, uint16_t num_coef, float value)
static bool mtostk_populate_uint_from_buffer(uint32_t *sca, uint32_t **eeprom_buffer)
static bool mtostk_seek_array_buffer(size_t dataSize, uint32_t **eeprom_buffer)
static float mtostk_apply_polynomial_temp(Sensors_params *params, float temp, float value)
bool mtostk_populate_cal_from_buffer(Calibration_params *cp, uint8_t *_eeprom_buffer)
Initializes a Calibration_params structure from memory buffer.
static bool mtostk_populate_float_array_from_buffer(float *ar, uint16_t *num_of_array, uint16_t *num_of_elem, uint32_t **eeprom_buffer)
static uint32_t fletcher32(uint32_t const *_data, size_t words)
static bool mtostk_populate_sensor_from_buffer(Sensors_params *sp, uint32_t **eeprom_buffer)
float mtostk_get_calibrated_value(Calibration_params *cp, Mtostk_sensors type, float uncal, float temp)
get calibrated value for a sensor according to current temperature
Parser for calibration data coming from 25AA256 EEPROM of the Meteo Stick.
Mtostk_sensors
@ MTOSTK_TEMP
@ MTOSTK_NUM_SENSORS
#define MTOSTK_MAX_POLY_ARRAY_SIZE
Sensors_params params[MTOSTK_NUM_SENSORS]
float coeffs[MTOSTK_MAX_TEMP_ARRAY_SIZE][MTOSTK_MAX_POLY_ARRAY_SIZE]
#define MTOSTK_MAX_TEMP_ARRAY_SIZE
uint32_t mtostk_time_t
time in seconds since 01/01/1970
#define UUID_LEN
mtostk_time_t timestamp
#define MTOSTK_MAX_SEEK_ARRAY_SIZE
float temps[MTOSTK_MAX_TEMP_ARRAY_SIZE]
Calibration_params structure initialized with calibvration data read from meteostick eeprom.
static uint32_t idx
float alpha
Definition: textons.c:133
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98