Nikolay Sergeev / NUCLEO_BLUENRG

Программа считывает показания датчиков и управляет сервомашинками.

Dependencies:   mbed-src

Fork of NUCLEO_BLUENRG by Ostap Ostapsky

tag_math.c@1:fb307cfca15c, 2014-08-25 (annotated)

Committer:
Sergeev
Date:
Mon Aug 25 09:45:34 2014 +0000
Revision:
1:fb307cfca15c
Parent:
0:aa1e012ec210 
Final

Who changed what in which revision?

UserRevisionLine numberNew 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 }