Mercurial > public > ostc4

comparison Small_CPU/Src/pressure.c @ 38:5f11787b4f42

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
include in ostc4 repository
author heinrichsweikamp
date Sat, 28 Apr 2018 11:52:34 +0200
parents
children f11f0bf6ef2d
comparison
equal deleted inserted replaced
37:ccc45c0e1ea2 38:5f11787b4f42
1 /**
2 ******************************************************************************
3 * @file pressure.c
4 * @author heinrichs weikamp gmbh
5 * @date 2014
6 * @version V0.0.2
7 * @since 20-Oct-2016
8 * @brief
9 *
10 @verbatim
11 ==============================================================================
12 ##### How to use #####
13 ==============================================================================
14 V0.0.2 18-Oct-2016 pressure_calculation_AN520_004_mod_MS5803_30BA__09_2015
15
16 @endverbatim
17 ******************************************************************************
18 * @attention
19 *
20 * <h2><center>&copy; COPYRIGHT(c) 2016 heinrichs weikamp</center></h2>
21 *
22 ******************************************************************************
23 */
24
25
26
27 /* surface time
28 the last 30 minutes will be saved once per minute in a endless loop
29 at the beginning of a dive the oldest value will be used
30 */
31
32
33 #include "pressure.h"
34 #include "i2c.h"
35 #include "rtc.h"
36
37 #define CMD_RESET 0x1E // ADC reset command
38 #define CMD_ADC_READ 0x00 // ADC read command
39 #define CMD_ADC_CONV 0x40 // ADC conversion command
40 #define CMD_ADC_D1 0x00 // ADC D1 conversion
41 #define CMD_ADC_D2 0x10 // ADC D2 conversion
42 #define CMD_ADC_256 0x00 // ADC OSR=256
43 #define CMD_ADC_512 0x02 // ADC OSR=512
44 #define CMD_ADC_1024 0x04 // ADC OSR=1024
45 #define CMD_ADC_2048 0x06 // ADC OSR=2056
46 #define CMD_ADC_4096 0x08 // ADC OSR=4096
47 #define CMD_PROM_RD 0xA0 // Prom read command
48
49
50 //uint16_t get_ci(uint8_t cmd);
51 //uint8_t get_ci_crc(void);
52 uint16_t get_ci_by_coef_num(uint8_t coef_num);
53 void pressure_calculation_new(void);
54 void pressure_calculation_old(void);
55 void pressure_calculation_AN520_004_mod_MS5803_30BA__09_2015(void);
56
57 uint8_t crc4(uint16_t n_prom[]);
58
59 HAL_StatusTypeDef pressure_sensor_get_data(void);
60 uint32_t get_adc(void);
61 uint8_t pressureSensorInitSuccess = 0;
62
63 //void test_calculation(void);
64
65 uint16_t C[8] = { 1 };
66 uint32_t D1 = 1;
67 uint32_t D2 = 1;
68 uint8_t n_crc;
69
70 int64_t C5_x_2p8 = 1;
71 int64_t C2_x_2p16 = 1;
72 int64_t C1_x_2p15 = 1;
73
74 /*
75 short C2plus10000 = -1;
76 short C3plus200 = -1;
77 short C4minus250 = -1;
78 short UT1 = -1;
79 short C6plus100 = -1;
80 */
81
82 float ambient_temperature = 0;
83 float ambient_pressure_mbar = 0;
84 float surface_pressure_mbar = 1000;
85 float surface_ring_mbar[31] = { 0 };
86
87 uint8_t secondCounterSurfaceRing = 0;
88
89 float get_temperature(void)
90 {
91 return ambient_temperature;
92 }
93
94 //float test = 1000;
95
96 float get_pressure_mbar(void)
97 {
98 // return test;
99 return ambient_pressure_mbar;
100 }
101
102
103 float get_surface_mbar(void)
104 {
105 return surface_pressure_mbar;
106 }
107
108
109 void init_surface_ring(void)
110 {
111 surface_ring_mbar[0] = 0;
112 for(int i=1; i<31; i++)
113 surface_ring_mbar[i] = ambient_pressure_mbar;
114 surface_pressure_mbar = ambient_pressure_mbar;
115 }
116
117
118 /* the ring has one place with 0
119 * after that comes the oldest value
120 * the new pressure is written in this hole
121 * the oldest value is read and then the new hole
122 */
123 void update_surface_pressure(uint8_t call_rhythm_seconds)
124 {
125 secondCounterSurfaceRing += call_rhythm_seconds;
126
127 if(secondCounterSurfaceRing < 60)
128 return;
129
130 secondCounterSurfaceRing = 0;
131
132 int hole;
133 for(hole=30;hole>0;hole--)
134 if(surface_ring_mbar[hole] == 0) { break; }
135
136 surface_ring_mbar[hole] = ambient_pressure_mbar;
137
138 hole++;
139 if(hole > 30)
140 hole = 0;
141 surface_pressure_mbar = surface_ring_mbar[hole];
142 surface_ring_mbar[hole] = 0;
143 }
144
145
146 float demo_modify_temperature_helper(float bottom_mbar_diff_to_surface)
147 {
148 const float temperature_surface = 31.0;
149 const float temperature_bottom = 14.0;
150
151 const float temperature_difference = temperature_bottom - temperature_surface;
152
153 // range 0.0 - 1.0
154 float position_now = (ambient_pressure_mbar - surface_pressure_mbar) / bottom_mbar_diff_to_surface;
155
156 if(position_now <= 0)
157 return temperature_surface;
158
159 if(position_now >= 1)
160 return temperature_bottom;
161
162 return temperature_surface + (temperature_difference * position_now);
163 }
164
165
166 uint32_t demo_modify_temperature_and_pressure(int32_t divetime_in_seconds, uint8_t subseconds, float ceiling_mbar)
167 {
168
169 const float descent_rate = 4000/60;
170 const float ascent_rate = 1000/60;
171 const uint32_t seconds_descend = (1 * 60) + 30;
172 const uint32_t turbo_seconds_at_bottom_start = (0 * 60) + 0;
173 const uint32_t seconds_descend_and_bottomtime = seconds_descend + turbo_seconds_at_bottom_start + (2 * 60) + 0;
174 uint32_t time_elapsed_in_seconds;
175 static float ambient_pressure_mbar_memory = 0;
176 static uint32_t time_last_call = 0;
177
178 if(divetime_in_seconds <= seconds_descend)
179 {
180 ambient_pressure_mbar = (divetime_in_seconds * descent_rate) + ((float)(subseconds) * descent_rate) + surface_pressure_mbar;
181 ambient_temperature = demo_modify_temperature_helper(descent_rate * seconds_descend);
182
183 time_last_call = divetime_in_seconds;
184 return 0;
185 }
186 else
187 if(divetime_in_seconds <= seconds_descend + turbo_seconds_at_bottom_start)
188 {
189 ambient_pressure_mbar = (seconds_descend * descent_rate) + surface_pressure_mbar;
190 ambient_temperature = demo_modify_temperature_helper(descent_rate * seconds_descend);
191 ambient_pressure_mbar_memory = ambient_pressure_mbar;
192 time_last_call = divetime_in_seconds;
193 return turbo_seconds_at_bottom_start;
194 }
195 else
196 if(divetime_in_seconds <= seconds_descend_and_bottomtime)
197 {
198 ambient_pressure_mbar = (seconds_descend * descent_rate) + surface_pressure_mbar;
199 ambient_temperature = demo_modify_temperature_helper(descent_rate * seconds_descend);
200 ambient_pressure_mbar_memory = ambient_pressure_mbar;
201 time_last_call = divetime_in_seconds;
202 return 0;
203 }
204 else
205 {
206 time_elapsed_in_seconds = divetime_in_seconds - time_last_call;
207 ambient_pressure_mbar = ambient_pressure_mbar_memory - time_elapsed_in_seconds * ascent_rate;
208
209 if(ambient_pressure_mbar < surface_pressure_mbar)
210 ambient_pressure_mbar = surface_pressure_mbar;
211 else if(ambient_pressure_mbar < ceiling_mbar)
212 ambient_pressure_mbar = ceiling_mbar;
213
214 ambient_temperature = demo_modify_temperature_helper(descent_rate * seconds_descend);
215 ambient_pressure_mbar_memory = ambient_pressure_mbar;
216 time_last_call = divetime_in_seconds;
217 return 0;
218 }
219 }
220
221
222 /* called just once on power on */
223 void init_pressure_DRx(void)
224 {
225 uint8_t resetCommand[1] = {0x1E};
226
227 I2C_Master_Transmit( DEVICE_PRESSURE, resetCommand, 1);
228 HAL_Delay(3);
229
230 C[1] = get_ci_by_coef_num(0x02);
231 C[2] = get_ci_by_coef_num(0x04);
232 C[3] = get_ci_by_coef_num(0x06);
233 C[4] = get_ci_by_coef_num(0x08);
234 C[5] = get_ci_by_coef_num(0x0A);
235 C[6] = get_ci_by_coef_num(0x0C);
236
237 C5_x_2p8 = C[5] * 256;
238 C2_x_2p16 = C[2] * 65536;
239 C1_x_2p15 = C[1] * 32768;
240 pressure_update();
241 }
242
243 uint8_t is_init_pressure_done(void)
244 {
245 return pressureSensorInitSuccess;
246 }
247
248 uint8_t init_pressure(void)
249 {
250 uint8_t buffer[1];
251 buffer[0] = 0x1e;
252 uint8_t retValue = 0xFF;
253
254
255 retValue = I2C_Master_Transmit( DEVICE_PRESSURE, buffer, 1);
256 if(retValue != HAL_OK)
257 {
258 return (HAL_StatusTypeDef)retValue;
259 }
260 HAL_Delay(3);
261
262 for(uint8_t i=0;i<8;i++)
263 {
264 C[i] = get_ci_by_coef_num(i);
265 }
266 n_crc = crc4(C); // no evaluation at the moment hw 151026
267
268 C5_x_2p8 = C[5] * 256;
269 C2_x_2p16 = C[2] * 65536;
270 C1_x_2p15 = C[1] * 32768;
271
272 if(I2C1_Status() == HAL_OK)
273 {
274 pressureSensorInitSuccess = 1;
275 }
276 return pressure_update();
277 }
278
279
280 uint32_t get_adc(void)
281 {
282 uint8_t buffer[1];
283 uint8_t resivebuf[4];
284 uint32_t answer = 0;
285 //
286 buffer[0] = 0x00; // Get ADC
287 I2C_Master_Transmit( DEVICE_PRESSURE, buffer, 1);
288 I2C_Master_Receive( DEVICE_PRESSURE, resivebuf, 4);
289 resivebuf[3] = 0;
290 answer = 256*256 *(uint32_t)resivebuf[0] + 256 * (uint32_t)resivebuf[1] + (uint32_t)resivebuf[2];
291
292 return answer;
293 }
294
295
296 uint16_t get_ci_by_coef_num(uint8_t coef_num)
297 {
298 uint8_t resivebuf[2];
299
300 uint8_t cmd = CMD_PROM_RD+coef_num*2;
301 I2C_Master_Transmit( DEVICE_PRESSURE, &cmd, 1);
302 I2C_Master_Receive( DEVICE_PRESSURE, resivebuf, 2);
303 return (256*(uint16_t)resivebuf[0]) + (uint16_t)resivebuf[1];
304 }
305
306
307
308 uint8_t pressure_update(void)
309 {
310 HAL_StatusTypeDef statusReturn = HAL_TIMEOUT;
311
312 statusReturn = pressure_sensor_get_data();
313 pressure_calculation();
314 return (uint8_t)statusReturn;
315 }
316
317
318 uint32_t pressure_sensor_get_one_value(uint8_t cmd, HAL_StatusTypeDef *statusReturn)
319 {
320 uint8_t command = CMD_ADC_CONV + cmd;
321 HAL_StatusTypeDef statusReturnTemp = HAL_TIMEOUT;
322
323 statusReturnTemp = I2C_Master_Transmit( DEVICE_PRESSURE, &command, 1);
324
325 if(statusReturn)
326 {
327 *statusReturn = statusReturnTemp;
328 }
329
330 switch (cmd & 0x0f) // wait necessary conversion time
331 {
332 case CMD_ADC_256 : HAL_Delay(1); break;
333 case CMD_ADC_512 : HAL_Delay(3); break;
334 case CMD_ADC_1024: HAL_Delay(4); break;
335 case CMD_ADC_2048: HAL_Delay(6); break;
336 case CMD_ADC_4096: HAL_Delay(10); break;
337 }
338 return get_adc();
339 }
340
341
342 HAL_StatusTypeDef pressure_sensor_get_data(void)
343 {
344 HAL_StatusTypeDef statusReturn1 = HAL_TIMEOUT;
345 HAL_StatusTypeDef statusReturn2 = HAL_TIMEOUT;
346
347 D2 = pressure_sensor_get_one_value(CMD_ADC_D2 + CMD_ADC_4096, &statusReturn1);
348 D1 = pressure_sensor_get_one_value(CMD_ADC_D1 + CMD_ADC_4096, &statusReturn2);
349
350 if(statusReturn2 > statusReturn1) // if anything is not HAL_OK (0x00) or worse
351 return statusReturn2;
352 else
353 return statusReturn1;
354 }
355
356
357 void pressure_sensor_get_pressure_raw(void)
358 {
359 D1 = pressure_sensor_get_one_value(CMD_ADC_D1 + CMD_ADC_4096, 0);
360 }
361
362
363 void pressure_sensor_get_temperature_raw(void)
364 {
365 D2 = pressure_sensor_get_one_value(CMD_ADC_D2 + CMD_ADC_4096, 0);
366 }
367
368
369 void pressure_calculation(void)
370 {
371 if(I2C1_Status() != HAL_OK)
372 return;
373
374 pressure_calculation_AN520_004_mod_MS5803_30BA__09_2015();
375 return;
376
377 // before October 2016: pressure_calculation_old();
378
379 // pressure_calculation_new();
380 }
381
382 void pressure_calculation_AN520_004_mod_MS5803_30BA__09_2015(void)
383 {
384 uint32_t local_D1; // ADC value of the pressure conversion
385 uint32_t local_D2; // ADC value of the temperature conversion
386 int32_t local_Px10; // compensated pressure value
387 int32_t local_Tx100; // compensated temperature value
388 int64_t local_dT; // int32_t, difference between actual and measured temperature
389 int64_t local_OFF; // offset at actual temperature
390 int64_t local_SENS; // sensitivity at actual temperature
391
392 int64_t T2;
393 int64_t OFF2;
394 int64_t SENS2;
395
396 local_D1 = D1;
397 local_D2 = D2;
398
399 local_dT = ((int64_t)local_D2) - ((int64_t)C[5]) * 256; //pow(2,8);
400 local_OFF = ((int64_t)C[2]) * 65536 + local_dT * ((int64_t)C[4]) / 128; // pow(2,16), pow(2,7)
401 local_SENS = ((int64_t)C[1]) * 32768 + local_dT * ((int64_t)C[3]) / 256; // pow(2,15), pow(2,8)
402
403 local_Tx100 = (int32_t)(2000 + (local_dT * ((int64_t)C[6])) / 8388608);// pow(2,23)
404
405
406 if(local_Tx100 < 2000) // low temperature
407 {
408 T2 = 3 * local_dT;
409 T2 *= local_dT;
410 T2 /= 8589934592;
411
412 OFF2 = ((int64_t)local_Tx100) - 2000;
413 OFF2 *= OFF2;
414 OFF2 *= 3;
415 OFF2 /= 2;
416
417 SENS2 = ((int64_t)local_Tx100) - 2000;
418 SENS2 *= SENS2;
419 SENS2 *= 5;
420 SENS2 /= 8;
421
422 local_Tx100 -= (int32_t)T2;
423 local_OFF -= OFF2;
424 local_SENS -= SENS2;
425 }
426 else
427 {
428 T2 = 7 * local_dT;
429 T2 *= local_dT;
430 T2 /= 137438953472;
431
432 OFF2 = ((int64_t)local_Tx100) - 2000;
433 OFF2 *= OFF2;
434 OFF2 /= 16;
435
436 local_Tx100 -= (int32_t)T2;
437 local_OFF -= OFF2;
438 }
439
440 local_Px10 = (int32_t)(
441 (((int64_t)((local_D1 * local_SENS) / 2097152)) - local_OFF)
442 / 8192 );// )) / 10; // pow(2,21), pow(2,13)
443
444 ambient_temperature = ((float)local_Tx100) / 100;
445 ambient_pressure_mbar = ((float)local_Px10) / 10;
446 }
447
448
449 void pressure_calculation_new(void)
450 {
451 #define POW2_8 (256)
452 #define POW2_17 (131072)
453 #define POW2_6 (64)
454 #define POW2_16 (65536)
455 #define POW2_7 (128)
456 #define POW2_23 (8388608)
457 #define POW2_21 (2097152)
458 #define POW2_15 (32768)
459 #define POW2_13 (8192)
460 #define POW2_37 (137438953472)
461 #define POW2_4 (16)
462 #define POW2_33 (8589934592)
463 #define POW2_3 (8)
464
465 int32_t P; // compensated pressure value
466 int32_t T; // compensated temperature value
467 int32_t dT; // difference between actual and measured temperature
468 int64_t OFF; // offset at actual temperature
469 int64_t SENS;
470
471 int32_t T2;
472 int64_t OFF2;
473 int64_t SENS2;
474
475 dT = ((int32_t)D2) - ((int32_t)C[5]) * POW2_8;
476 OFF = ((int64_t)C[2]) * POW2_16 + ((int64_t)dT) * ((int64_t)C[4]) / POW2_7;
477 SENS = ((int64_t)C[1]) * POW2_15 + ((int64_t)dT) * ((int64_t)C[3]) / POW2_8;
478
479 T = 2000 + (dT * ((int32_t)C[6])) / POW2_23;
480
481
482 if(T < 2000) // low temperature
483 {
484 T2 = 3 * dT * dT;
485 T2 /= POW2_33;
486 OFF2 = ((int64_t)T) - 2000;
487 OFF2 *= OFF2;
488 OFF2 *= 3;
489 OFF2 /= 2;
490 SENS2 = ((int64_t)T) - 2000;
491 SENS2 *= SENS2;
492 SENS2 *= 5;
493 SENS2 /= POW2_3;
494 }
495 else // high temperature
496 {
497 T2 = 7 * dT * dT;
498 T2 /= POW2_37;
499 OFF2 = ((int64_t)T) - 2000;
500 OFF2 *= OFF2;
501 OFF2 /= POW2_4;
502 SENS2 = 0;
503 }
504
505 T = T - T2;
506 OFF = OFF - OFF2;
507 SENS = SENS - SENS2;
508
509 P = (int32_t)(((((int64_t)D1) * SENS) / POW2_21 - OFF) / POW2_13);
510
511 ambient_temperature = ((float)T) / 100;
512 ambient_pressure_mbar = ((float)P) / 10;
513 }
514
515
516 void pressure_calculation_old(void) {
517 //
518 double ambient_temperature_centigrad = 0;
519 double ambient_pressure_decimbar = 0;
520
521 // static for debug
522 static int64_t dt = 0;
523 static int64_t temp = 0;
524 static int64_t ms_off = 0;
525 static int64_t sens = 0;
526 //
527 static int64_t ms_off2 = 0;
528 static int64_t sens2 = 0;
529 static int64_t t2 = 0;
530
531 /* info
532 uint16_t C[8] = { 1 };
533 uint32_t D1 = 1;
534 uint32_t D2 = 1;
535 uint8_t n_crc;
536 */
537 if((D2 == 0) || (D1 == 0))
538 return;
539 //
540
541 // dT = D2 - C[5] * POW2_8;
542 // T = 2000 + (dT * C[6]) / POW2_23;
543 dt = (int64_t)D2 - C5_x_2p8;
544 //temp ; // in 10 milliGrad Celcius
545 ambient_temperature_centigrad = 2000 + dt * C[6] / 8388608;
546
547
548 if(ambient_temperature_centigrad < 2000) // low temperature
549 {
550 t2 = 3 * dt;
551 t2 *= dt;
552 t2 /= 8589934592;
553 ms_off2 = ambient_temperature_centigrad - 2000;
554 ms_off2 *= ms_off2;
555 sens2 = ms_off2;
556 ms_off2 *= 3;
557 ms_off2 /= 2;
558 sens2 *= 5;
559 sens2 /= 8;
560 }
561 else // high temperature
562 {
563 t2 = 7 * dt;
564 t2 *= dt;
565 t2 /= 137438953472;
566 ms_off2 = ambient_temperature_centigrad - 2000;
567 ms_off2 *= ms_off2;
568 ms_off2 /= 16;
569 sens2 = 0;
570 }
571
572
573 //
574
575 // pressure
576 // OFF = C[2] * POW2_16 + dT * C[4] / POW2_7;
577 // SENS = C[1] * POW2_15 + dT * C[3] / POW2_8;
578 ms_off = C[4] * dt;
579 ms_off /= 128;
580 ms_off += C2_x_2p16;
581 //
582 sens = C[3] * dt;
583 sens /= 256;
584 sens += C1_x_2p15;
585
586 // 2nd order correction
587 ambient_temperature_centigrad -= t2;
588 ms_off -= ms_off2;
589 sens -= sens2;
590
591 ambient_temperature = ambient_temperature_centigrad / 100;
592 // P = (D1 * SENS / POW2_21 - OFF) / POW2_13;
593 temp = D1 * sens;
594 temp /= 2097152;
595 temp -= ms_off;
596 temp /= 8192;
597 ambient_pressure_decimbar = temp; // to float/double
598 ambient_pressure_mbar = ambient_pressure_decimbar / 10;
599 }
600
601
602 /* taken from AN520 by meas-spec.com dated 9. Aug. 2011
603 * short and int are both 16bit according to AVR/GCC google results
604 */
605 uint8_t crc4(uint16_t n_prom[])
606 {
607 uint16_t cnt; // simple counter
608 uint16_t n_rem; // crc reminder
609 uint16_t crc_read; // original value of the crc
610 uint8_t n_bit;
611 n_rem = 0x00;
612 crc_read=n_prom[7]; //save read CRC
613 n_prom[7]=(0xFF00 & (n_prom[7])); //CRC byte is replaced by 0
614 for (cnt = 0; cnt < 16; cnt++) // operation is performed on bytes
615 { // choose LSB or MSB
616 if (cnt%2==1) n_rem ^= (uint16_t) ((n_prom[cnt>>1]) & 0x00FF);
617 else n_rem ^= (uint16_t) (n_prom[cnt>>1]>>8);
618 for (n_bit = 8; n_bit > 0; n_bit--)
619 {
620 if (n_rem & (0x8000))
621 {
622 n_rem = (n_rem << 1) ^ 0x3000;
623 }
624 else
625 {
626 n_rem = (n_rem << 1);
627 }
628 }
629 }
630 n_rem= (0x000F & (n_rem >> 12)); // // final 4-bit reminder is CRC code
631 n_prom[7]=crc_read; // restore the crc_read to its original place
632 return (n_rem ^ 0x00);
633 }
634 /*
635 void test_calculation(void)
636 {
637 C1 = 29112;
638 C2 = 26814;
639 C3 = 19125;
640 C4 = 17865;
641 C5 = 32057;
642 C6 = 31305;
643
644 C2_x_2p16 = C2 * 65536;
645 C1_x_2p15 = C1 * 32768;
646
647 D1 = 4944364;
648 D2 = 8198974;
649 pressure_calculation() ;
650 };
651 */
652