Mercurial > public > ostc4

comparison Small_CPU/Src/pressure.c @ 345:4093ac18b25c FlightMode_Improvment

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Added function to evaluate the state of the surface pressure history memory Activation of the computer during or short after landing caused a switch into divemode which could then not be left. To solve this the history memory is evaluated for significant pressure changes as the would not be expected in "normal" condition. In case of such a event (unstable) the pressure limit to enter dive mode is set to an absolute value (instead of relativ in normal mode)
author ideenmodellierer
date Thu, 03 Oct 2019 21:26:46 +0200
parents 37f45300bc2e
children ba53ab77f059
comparison
equal deleted inserted replaced
344:3f09b283d993 345:4093ac18b25c
44 #define CMD_ADC_1024 0x04 // ADC OSR=1024 44 #define CMD_ADC_1024 0x04 // ADC OSR=1024
45 #define CMD_ADC_2048 0x06 // ADC OSR=2056 45 #define CMD_ADC_2048 0x06 // ADC OSR=2056
46 #define CMD_ADC_4096 0x08 // ADC OSR=4096 46 #define CMD_ADC_4096 0x08 // ADC OSR=4096
47 #define CMD_PROM_RD 0xA0 // Prom read command 47 #define CMD_PROM_RD 0xA0 // Prom read command
48 48
49 #define PRESSURE_SURFACE_MAX_MBAR (1070.0f) /* It is very unlikely that pressure at surface is greater than this value => clip to it */ 49 /* remove comment to use a predefined profile for pressure changes instead of real world data */
50 #define PRESSURE_HISTORY_SIZE (8u) 50 /* #define SIMULATE_PRESSURE */
51 #define PRESSURE_JUMP_VALID_MBAR (500.0f) /* values are measure several times a second => jumps > 5m very unlikely */ 51
52 52 #define PRESSURE_SURFACE_MAX_MBAR (1070.0f) /* It is very unlikely that pressure at surface is greater than this value => clip to it */
53 #define PRESSURE_SURFACE_QUE (30u) /* history buffer [minutes] for past pressure measurements */ 53 #define PRESSURE_HISTORY_SIZE (8u)
54 #define PRESSURE_JUMP_VALID_MBAR (500.0f) /* values are measure several times a second => jumps > 5m very unlikely */
55
56 #define PRESSURE_SURFACE_QUE (30u) /* history buffer [minutes] for past pressure measurements */
57 #define PRESSURE_SURFACE_EVA_WINDOW (15u) /* Number of entries evaluated during instability test. Used to avoid detection while dive enters water */
58 #define PRESSURE_SURFACE_STABLE_LIMIT (10u) /* Define pressure as stable if delta (mBar) is below this value */
59 #define PRESSURE_SURFACE_DETECT_STABLE_CNT (5u) /* Event count to detect stable condition */
60 #define PRESSURE_SURFACE_UNSTABLE_LIMIT (50u) /* Define pressure as not stable if delta (mBar) is larger than this value */
61 #define PRESSURE_SURFACE_DETECT_UNSTABLE_CNT (3u) /* Event count to detect unstable condition */
62
54 63
55 static uint16_t get_ci_by_coef_num(uint8_t coef_num); 64 static uint16_t get_ci_by_coef_num(uint8_t coef_num);
56 //void pressure_calculation_new(void); 65 //void pressure_calculation_new(void);
57 //void pressure_calculation_old(void); 66 //void pressure_calculation_old(void);
58 static void pressure_calculation_AN520_004_mod_MS5803_30BA__09_2015(void); 67 static void pressure_calculation_AN520_004_mod_MS5803_30BA__09_2015(void);
84 static float ambient_temperature = 0; 93 static float ambient_temperature = 0;
85 static float ambient_pressure_mbar = 1000.0; 94 static float ambient_pressure_mbar = 1000.0;
86 static float surface_pressure_mbar = 1000.0; 95 static float surface_pressure_mbar = 1000.0;
87 static float surface_ring_mbar[PRESSURE_SURFACE_QUE] = { 0 }; 96 static float surface_ring_mbar[PRESSURE_SURFACE_QUE] = { 0 };
88 97
98 static uint8_t surface_pressure_writeIndex = 0;
99 static float surface_pressure_stable_value = 0;
100 static uint8_t surface_pressure_stable = 0;
101
89 static float pressure_history_mbar[PRESSURE_HISTORY_SIZE]; 102 static float pressure_history_mbar[PRESSURE_HISTORY_SIZE];
90 103
91 static uint8_t secondCounterSurfaceRing = 0; 104 static uint8_t secondCounterSurfaceRing = 0;
92 static uint8_t avgCount = 0; 105 static uint8_t avgCount = 0;
93 static float runningAvg = 0; 106 static float runningAvg = 0;
117 runningAvg = 0; 130 runningAvg = 0;
118 131
119 for(int i=0; i<PRESSURE_SURFACE_QUE; i++) 132 for(int i=0; i<PRESSURE_SURFACE_QUE; i++)
120 surface_ring_mbar[i] = ambient_pressure_mbar; 133 surface_ring_mbar[i] = ambient_pressure_mbar;
121 surface_pressure_mbar = ambient_pressure_mbar; 134 surface_pressure_mbar = ambient_pressure_mbar;
135 surface_pressure_writeIndex = 0; /* index of the oldest value in the ring buffer */
122 } 136 }
123 } 137 }
124 138
125 void init_pressure_history(void) 139 void init_pressure_history(void)
126 { 140 {
128 { 142 {
129 pressure_history_mbar[i] = 1000.0; 143 pressure_history_mbar[i] = 1000.0;
130 } 144 }
131 } 145 }
132 146
133 147 uint8_t is_surface_pressure_stable(void)
148 {
149 return surface_pressure_stable;
150 }
151
152 float set_last_surface_pressure_stable(void)
153 {
154 surface_pressure_mbar = surface_pressure_stable_value;
155 return surface_pressure_stable_value;
156 }
157
158 /* iterate backward through the history memory and evaluate the changes pressure changes during the last 30 minutes */
159 void evaluate_surface_pressure()
160 {
161 uint8_t index;
162 float lastvalue;
163 uint8_t stablecnt = 0;
164 uint8_t unstablecnt = 0;
165 uint8_t EvaluationWindow = PRESSURE_SURFACE_QUE - PRESSURE_SURFACE_EVA_WINDOW; /* do not use the latest 15 values to avoid unstable condition due to something like fin handling */
166 uint8_t EvaluatedValues = 0;
167
168 lastvalue = surface_ring_mbar[surface_pressure_writeIndex];
169 surface_pressure_stable_value = surface_ring_mbar[surface_pressure_writeIndex]; /* default: if no stable value is found return the oldest value */
170 index = surface_pressure_writeIndex;
171 surface_pressure_stable = 1;
172
173 if(index == 0)
174 {
175 index = PRESSURE_SURFACE_QUE - 1;
176 }
177 else
178 {
179 index = index - 1;
180 }
181 do
182 {
183 if((EvaluatedValues < EvaluationWindow) &&
184 (fabs(surface_pressure_stable_value - surface_ring_mbar[index]) > PRESSURE_SURFACE_UNSTABLE_LIMIT)) /* unusual change during last 30 minutes */
185 {
186 unstablecnt++;
187 if(unstablecnt > PRESSURE_SURFACE_DETECT_UNSTABLE_CNT)
188 {
189 surface_pressure_stable = 0;
190 }
191 }
192 /* search for a value which does not change for several iterations */
193 if (fabs(lastvalue - surface_ring_mbar[index]) < PRESSURE_SURFACE_STABLE_LIMIT)
194 {
195 stablecnt++;
196 }
197 else
198 {
199 stablecnt = 0;
200 }
201 if ((stablecnt >= PRESSURE_SURFACE_DETECT_STABLE_CNT) && (surface_pressure_stable == 0)&&(surface_pressure_stable_value == surface_ring_mbar[surface_pressure_writeIndex])) /* pressure is unstable => search for new stable value */
202 {
203 surface_pressure_stable_value = surface_ring_mbar[index];
204 unstablecnt = 0;
205 }
206
207 lastvalue = surface_ring_mbar[index];
208
209 if(index == 0)
210 {
211 index = PRESSURE_SURFACE_QUE - 1;
212 }
213 else
214 {
215 index = index - 1;
216 }
217 EvaluatedValues++;
218 } while (index != surface_pressure_writeIndex);
219 }
134 void update_surface_pressure(uint8_t call_rhythm_seconds) 220 void update_surface_pressure(uint8_t call_rhythm_seconds)
135 { 221 {
136 static uint8_t writeIndex = 0; /* Reinitialization will reset all entries to the same value => no need to reinit write index */
137 222
138 223
139 if(is_init_pressure_done()) 224 if(is_init_pressure_done())
140 { 225 {
141 runningAvg = (runningAvg * avgCount + ambient_pressure_mbar) / (avgCount +1); 226 runningAvg = (runningAvg * avgCount + ambient_pressure_mbar) / (avgCount +1);
144 229
145 if(secondCounterSurfaceRing >= 60) 230 if(secondCounterSurfaceRing >= 60)
146 { 231 {
147 if(runningAvg < PRESSURE_SURFACE_MAX_MBAR) 232 if(runningAvg < PRESSURE_SURFACE_MAX_MBAR)
148 { 233 {
149 surface_ring_mbar[writeIndex] = runningAvg; 234 surface_ring_mbar[surface_pressure_writeIndex] = runningAvg;
150 } 235 }
151 else 236 else
152 { 237 {
153 surface_ring_mbar[writeIndex] = PRESSURE_SURFACE_MAX_MBAR; 238 surface_ring_mbar[surface_pressure_writeIndex] = PRESSURE_SURFACE_MAX_MBAR;
154 } 239 }
155 writeIndex++; /* the write index is now pointing to the oldest value in the buffer which will be overwritten next time */ 240 surface_pressure_writeIndex++; /* the write index is now pointing to the oldest value in the buffer which will be overwritten next time */
156 241
157 if(writeIndex == PRESSURE_SURFACE_QUE) 242 if(surface_pressure_writeIndex == PRESSURE_SURFACE_QUE)
158 { 243 {
159 writeIndex = 0; 244 surface_pressure_writeIndex = 0;
160 } 245 }
161 246
162 surface_pressure_mbar = surface_ring_mbar[writeIndex]; /* 30 minutes old measurement */ 247 surface_pressure_mbar = surface_ring_mbar[surface_pressure_writeIndex]; /* 30 minutes old measurement */
163 248
164 secondCounterSurfaceRing = 0; 249 secondCounterSurfaceRing = 0;
165 avgCount = 1; /* use the current value as starting point but restart the weight decrement of the measurements */ 250 avgCount = 1; /* use the current value as starting point but restart the weight decrement of the measurements */
166 } 251 }
252 evaluate_surface_pressure();
167 } 253 }
168 } 254 }
169 255
170 #ifdef DEMOMODE 256 #ifdef DEMOMODE
171 float demo_modify_temperature_helper(float bottom_mbar_diff_to_surface) 257 float demo_modify_temperature_helper(float bottom_mbar_diff_to_surface)
423 } 509 }
424 return statusReturn; 510 return statusReturn;
425 } 511 }
426 512
427 513
514 #ifdef SIMULATE_PRESSURE
515 void pressure_simulation()
516 {
517 static uint32_t tickstart = 0;
518 static float pressure_sim_mbar = 0;
519 static uint32_t passedSecond = 0;
520 static uint32_t secondtick = 0;
521
522 uint32_t lasttick = 0;
523
524
525
526 if( tickstart == 0)
527 {
528 tickstart = HAL_GetTick(); /* init time stamp */
529 secondtick = tickstart;
530 pressure_sim_mbar = 1000;
531 }
532
533 lasttick = HAL_GetTick();
534 if(time_elapsed_ms(secondtick,lasttick) > 1000) /* one second passed since last tick */
535 {
536 secondtick = lasttick;
537 passedSecond++;
538
539 #ifdef DIVE_AFTER_LANDING
540 if(passedSecond < 10) pressure_sim_mbar = 1000.0; /* stay stable for 10 seconds */
541 else if(passedSecond < 300) pressure_sim_mbar -= 1.0; /* decrease pressure in 5 minutes target 770mbar => delta 330 */
542 else if(passedSecond < 900) pressure_sim_mbar += 0.0; /*stay stable 10 minutes*/
543 else if(passedSecond < 1500) pressure_sim_mbar += 0.5; /* return to 1 bar in 10 Minutes*/
544 else if(passedSecond < 1800) pressure_sim_mbar += 0.0; /* 5 minutes break */
545 else if(passedSecond < 2000) pressure_sim_mbar += 10.0; /* start dive */
546 else if(passedSecond < 2300) pressure_sim_mbar += 0.0; /* stay on depth */
547 else if(passedSecond < 2500) pressure_sim_mbar -= 10.0; /* return to surface */
548 else pressure_sim_mbar = 1000.0; /* final state */
549 #else /* short dive */
550 if(passedSecond < 10) pressure_sim_mbar = 1000.0; /* stay stable for 10 seconds */
551 else if(passedSecond < 180) pressure_sim_mbar += 10.0; /* Start dive */
552 else if(passedSecond < 300) pressure_sim_mbar += 0.0; /*stay on depth*/
553 else if(passedSecond < 460) pressure_sim_mbar -= 10.0; /* return to surface */
554 else if(passedSecond < 600) pressure_sim_mbar += 0.0; /* stay */
555 else if(passedSecond < 610) pressure_sim_mbar = 1000.0; /* get ready for second dive */
556 else if(passedSecond < 780) pressure_sim_mbar += 10.0; /* Start dive */
557 else if(passedSecond < 900) pressure_sim_mbar += 0.0; /*stay on depth*/
558 else if(passedSecond < 1060) pressure_sim_mbar -= 10.0; /* return to surface */
559 else if(passedSecond < 1200) pressure_sim_mbar += 0.0; /* stay */
560 else pressure_sim_mbar = 1000.0; /* final state */
561 #endif
562 }
563
564
565 ambient_pressure_mbar = pressure_sim_mbar;
566 ambient_temperature = 25.0;
567 return;
568 }
569
570 #endif
571
428 void pressure_calculation(void) 572 void pressure_calculation(void)
429 { 573 {
430 if(global.I2C_SystemStatus != HAL_OK) 574 if(global.I2C_SystemStatus != HAL_OK)
431 return; 575 return;
432 576
577 #ifdef SIMULATE_PRESSURE
578 pressure_simulation();
579 #else
433 pressure_calculation_AN520_004_mod_MS5803_30BA__09_2015(); 580 pressure_calculation_AN520_004_mod_MS5803_30BA__09_2015();
581 #endif
434 } 582 }
435 583
436 static uint8_t pressure_plausible(float pressurevalue) 584 static uint8_t pressure_plausible(float pressurevalue)
437 { 585 {
438 static uint8_t pressurewriteindex = 0; 586 static uint8_t pressurewriteindex = 0;
550 avgCnt++; /* by the measurement range of the sensor which is focused on under water pressure measurement */ 698 avgCnt++; /* by the measurement range of the sensor which is focused on under water pressure measurement */
551 } 699 }
552 ambient_pressure_mbar = runningAvg; 700 ambient_pressure_mbar = runningAvg;
553 } 701 }
554 } 702 }
555
556
557 /*
558 void pressure_calculation_new(void)
559 {
560 #define POW2_8 (256)
561 #define POW2_17 (131072)
562 #define POW2_6 (64)
563 #define POW2_16 (65536)
564 #define POW2_7 (128)
565 #define POW2_23 (8388608)
566 #define POW2_21 (2097152)
567 #define POW2_15 (32768)
568 #define POW2_13 (8192)
569 #define POW2_37 (137438953472)
570 #define POW2_4 (16)
571 #define POW2_33 (8589934592)
572 #define POW2_3 (8)
573
574 int32_t P; // compensated pressure value
575 int32_t T; // compensated temperature value
576 int32_t dT; // difference between actual and measured temperature
577 int64_t OFF; // offset at actual temperature
578 int64_t SENS;
579
580 int32_t T2;
581 int64_t OFF2;
582 int64_t SENS2;
583
584 dT = ((int32_t)D2) - ((int32_t)C[5]) * POW2_8;
585 OFF = ((int64_t)C[2]) * POW2_16 + ((int64_t)dT) * ((int64_t)C[4]) / POW2_7;
586 SENS = ((int64_t)C[1]) * POW2_15 + ((int64_t)dT) * ((int64_t)C[3]) / POW2_8;
587
588 T = 2000 + (dT * ((int32_t)C[6])) / POW2_23;
589
590
591 if(T < 2000) // low temperature
592 {
593 T2 = 3 * dT * dT;
594 T2 /= POW2_33;
595 OFF2 = ((int64_t)T) - 2000;
596 OFF2 *= OFF2;
597 OFF2 *= 3;
598 OFF2 /= 2;
599 SENS2 = ((int64_t)T) - 2000;
600 SENS2 *= SENS2;
601 SENS2 *= 5;
602 SENS2 /= POW2_3;
603 }
604 else // high temperature
605 {
606 T2 = 7 * dT * dT;
607 T2 /= POW2_37;
608 OFF2 = ((int64_t)T) - 2000;
609 OFF2 *= OFF2;
610 OFF2 /= POW2_4;
611 SENS2 = 0;
612 }
613
614 T = T - T2;
615 OFF = OFF - OFF2;
616 SENS = SENS - SENS2;
617
618 P = (int32_t)(((((int64_t)D1) * SENS) / POW2_21 - OFF) / POW2_13);
619
620 ambient_temperature = ((float)T) / 100;
621 ambient_pressure_mbar = ((float)P) / 10;
622 }
623 */
624
625 /*
626 void pressure_calculation_old(void) {
627 //
628 double ambient_temperature_centigrad = 0;
629 double ambient_pressure_decimbar = 0;
630
631 // static for debug
632 static int64_t dt = 0;
633 static int64_t temp = 0;
634 static int64_t ms_off = 0;
635 static int64_t sens = 0;
636 //
637 static int64_t ms_off2 = 0;
638 static int64_t sens2 = 0;
639 static int64_t t2 = 0;
640
641 if((D2 == 0) || (D1 == 0))
642 return;
643 //
644
645 // dT = D2 - C[5] * POW2_8;
646 // T = 2000 + (dT * C[6]) / POW2_23;
647 dt = (int64_t)D2 - C5_x_2p8;
648 //temp ; // in 10 milliGrad Celcius
649 ambient_temperature_centigrad = 2000 + dt * C[6] / 8388608;
650
651
652 if(ambient_temperature_centigrad < 2000) // low temperature
653 {
654 t2 = 3 * dt;
655 t2 *= dt;
656 t2 /= 8589934592;
657 ms_off2 = ambient_temperature_centigrad - 2000;
658 ms_off2 *= ms_off2;
659 sens2 = ms_off2;
660 ms_off2 *= 3;
661 ms_off2 /= 2;
662 sens2 *= 5;
663 sens2 /= 8;
664 }
665 else // high temperature
666 {
667 t2 = 7 * dt;
668 t2 *= dt;
669 t2 /= 137438953472;
670 ms_off2 = ambient_temperature_centigrad - 2000;
671 ms_off2 *= ms_off2;
672 ms_off2 /= 16;
673 sens2 = 0;
674 }
675
676
677 //
678
679 // pressure
680 // OFF = C[2] * POW2_16 + dT * C[4] / POW2_7;
681 // SENS = C[1] * POW2_15 + dT * C[3] / POW2_8;
682 ms_off = C[4] * dt;
683 ms_off /= 128;
684 ms_off += C2_x_2p16;
685 //
686 sens = C[3] * dt;
687 sens /= 256;
688 sens += C1_x_2p15;
689
690 // 2nd order correction
691 ambient_temperature_centigrad -= t2;
692 ms_off -= ms_off2;
693 sens -= sens2;
694
695 ambient_temperature = ambient_temperature_centigrad / 100;
696 // P = (D1 * SENS / POW2_21 - OFF) / POW2_13;
697 temp = D1 * sens;
698 temp /= 2097152;
699 temp -= ms_off;
700 temp /= 8192;
701 ambient_pressure_decimbar = temp; // to float/double
702 ambient_pressure_mbar = ambient_pressure_decimbar / 10;
703 }
704 */
705 703
706 704
707 /* taken from AN520 by meas-spec.com dated 9. Aug. 2011 705 /* taken from AN520 by meas-spec.com dated 9. Aug. 2011
708 * short and int are both 16bit according to AVR/GCC google results 706 * short and int are both 16bit according to AVR/GCC google results
709 */ 707 */