Mercurial > public > ostc4

comparison Small_CPU/Src/pressure.c @ 382:14fd5f35cb50 MotionDetection

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
merge default
author Ideenmodellierer
date Thu, 10 Oct 2019 22:26:03 +0200
parents 1f24022345d1
children cb3870f79e9d
comparison
equal deleted inserted replaced
381:695434a6dcf6 382:14fd5f35cb50
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 (1030.0f) /* It is 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 */