Программа считывает показания датчиков и управляет сервомашинками.
Fork of NUCLEO_BLUENRG by
tag_math.c@0:aa1e012ec210, 2014-08-16 (annotated)
- Committer:
- ostapsky
- Date:
- Sat Aug 16 11:00:04 2014 +0000
- Revision:
- 0:aa1e012ec210
CLIENT mode first revision
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ostapsky | 0:aa1e012ec210 | 1 | #include <math.h> |
ostapsky | 0:aa1e012ec210 | 2 | #include "stm32f4xx_hal.h" |
ostapsky | 0:aa1e012ec210 | 3 | |
ostapsky | 0:aa1e012ec210 | 4 | /* Conversion algorithm for target temperature */ |
ostapsky | 0:aa1e012ec210 | 5 | double calcTmpTarget(uint8_t* rawData) |
ostapsky | 0:aa1e012ec210 | 6 | { |
ostapsky | 0:aa1e012ec210 | 7 | uint16_t rawT = (rawData[1] << 8) | rawData[0]; |
ostapsky | 0:aa1e012ec210 | 8 | double m_tmpAmb = ((rawData[3] << 8) | rawData[2] )/128.0; |
ostapsky | 0:aa1e012ec210 | 9 | |
ostapsky | 0:aa1e012ec210 | 10 | //-- calculate target temperature |
ostapsky | 0:aa1e012ec210 | 11 | double Vobj2 = (double)(uint16_t)rawT; |
ostapsky | 0:aa1e012ec210 | 12 | Vobj2 *= 0.00000015625; |
ostapsky | 0:aa1e012ec210 | 13 | |
ostapsky | 0:aa1e012ec210 | 14 | double Tdie2 = m_tmpAmb + 273.15; |
ostapsky | 0:aa1e012ec210 | 15 | const double S0 = 6.4E-14; // Calibration factor |
ostapsky | 0:aa1e012ec210 | 16 | |
ostapsky | 0:aa1e012ec210 | 17 | const double a1 = 1.75E-3; |
ostapsky | 0:aa1e012ec210 | 18 | const double a2 = -1.678E-5; |
ostapsky | 0:aa1e012ec210 | 19 | const double b0 = -2.94E-5; |
ostapsky | 0:aa1e012ec210 | 20 | const double b1 = -5.7E-7; |
ostapsky | 0:aa1e012ec210 | 21 | const double b2 = 4.63E-9; |
ostapsky | 0:aa1e012ec210 | 22 | const double c2 = 13.4; |
ostapsky | 0:aa1e012ec210 | 23 | const double Tref = 298.15; |
ostapsky | 0:aa1e012ec210 | 24 | double S = S0*(1+a1*(Tdie2 - Tref)+a2*pow((Tdie2 - Tref),2)); |
ostapsky | 0:aa1e012ec210 | 25 | double Vos = b0 + b1*(Tdie2 - Tref) + b2*pow((Tdie2 - Tref),2); |
ostapsky | 0:aa1e012ec210 | 26 | double fObj = (Vobj2 - Vos) + c2*pow((Vobj2 - Vos),2); |
ostapsky | 0:aa1e012ec210 | 27 | double tObj = pow(pow(Tdie2,4) + (fObj/S),.25); |
ostapsky | 0:aa1e012ec210 | 28 | tObj = (tObj - 273.15); |
ostapsky | 0:aa1e012ec210 | 29 | |
ostapsky | 0:aa1e012ec210 | 30 | return m_tmpAmb;//tObj; |
ostapsky | 0:aa1e012ec210 | 31 | } |
ostapsky | 0:aa1e012ec210 | 32 | |
ostapsky | 0:aa1e012ec210 | 33 | double calcHumTmp(uint16_t rawT) |
ostapsky | 0:aa1e012ec210 | 34 | { |
ostapsky | 0:aa1e012ec210 | 35 | double v; |
ostapsky | 0:aa1e012ec210 | 36 | |
ostapsky | 0:aa1e012ec210 | 37 | //-- calculate temperature [deg C] -- |
ostapsky | 0:aa1e012ec210 | 38 | v = -46.85 + 175.72/65536 *(double)(uint16_t)rawT; |
ostapsky | 0:aa1e012ec210 | 39 | |
ostapsky | 0:aa1e012ec210 | 40 | return v; |
ostapsky | 0:aa1e012ec210 | 41 | } |
ostapsky | 0:aa1e012ec210 | 42 | |
ostapsky | 0:aa1e012ec210 | 43 | /* Conversion algorithm, humidity */ |
ostapsky | 0:aa1e012ec210 | 44 | |
ostapsky | 0:aa1e012ec210 | 45 | double calcHumRel(uint8_t* rawData) |
ostapsky | 0:aa1e012ec210 | 46 | { |
ostapsky | 0:aa1e012ec210 | 47 | double v; |
ostapsky | 0:aa1e012ec210 | 48 | |
ostapsky | 0:aa1e012ec210 | 49 | uint16_t rawH = (rawData[3] << 8) | rawData[2]; |
ostapsky | 0:aa1e012ec210 | 50 | |
ostapsky | 0:aa1e012ec210 | 51 | rawH &= ~0x0003; // clear bits [1..0] (status bits) |
ostapsky | 0:aa1e012ec210 | 52 | //-- calculate relative humidity [%RH] -- |
ostapsky | 0:aa1e012ec210 | 53 | v = -6.0 + 125.0/65536 * (double)rawH; // RH= -6 + 125 * SRH/2^16 |
ostapsky | 0:aa1e012ec210 | 54 | |
ostapsky | 0:aa1e012ec210 | 55 | return v; |
ostapsky | 0:aa1e012ec210 | 56 | } |