Kalman + GPS / Mbed 2 deprecated Precise_Altimeter

3 формулки

Dependencies:   X_NUCLEO_IKS01A1 mbed

main.cpp@0:9bcc849e42db, 2016-03-23 (annotated)

Committer:
Bullet_95
Date:
Wed Mar 23 14:05:23 2016 +0000
Revision:
0:9bcc849e42db
3 ??????? ?? ????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Bullet_95 0:9bcc849e42db 1 /* Includes */
Bullet_95 0:9bcc849e42db 2
Bullet_95 0:9bcc849e42db 3 #include "mbed.h"
Bullet_95 0:9bcc849e42db 4 #include "x_nucleo_iks01a1.h"
Bullet_95 0:9bcc849e42db 5 #include "lsm6ds0_class.h"
Bullet_95 0:9bcc849e42db 6 #include <stdio.h>
Bullet_95 0:9bcc849e42db 7 #include <stdlib.h>
Bullet_95 0:9bcc849e42db 8 #include <string.h>
Bullet_95 0:9bcc849e42db 9
Bullet_95 0:9bcc849e42db 10
Bullet_95 0:9bcc849e42db 11 //------------------------------------
Bullet_95 0:9bcc849e42db 12 // Hyperterminal configuration
Bullet_95 0:9bcc849e42db 13 // 9600 bauds, 8-bit data, no parity
Bullet_95 0:9bcc849e42db 14 //------------------------------------
Bullet_95 0:9bcc849e42db 15
Bullet_95 0:9bcc849e42db 16 //Serial pc(PC_4, PC_5); //TX, RX UART !!!!!!!!
Bullet_95 0:9bcc849e42db 17 Serial pc(SERIAL_TX, SERIAL_RX); //PC
Bullet_95 0:9bcc849e42db 18
Bullet_95 0:9bcc849e42db 19
Bullet_95 0:9bcc849e42db 20 /* Instantiate the expansion board */
Bullet_95 0:9bcc849e42db 21 static X_NUCLEO_IKS01A1 *mems_expansion_board = X_NUCLEO_IKS01A1::Instance(D14, D15);
Bullet_95 0:9bcc849e42db 22
Bullet_95 0:9bcc849e42db 23 static HumiditySensor *humidity_sensor = mems_expansion_board->ht_sensor;
Bullet_95 0:9bcc849e42db 24 static PressureSensor *pressure_sensor = mems_expansion_board->pt_sensor;
Bullet_95 0:9bcc849e42db 25 static TempSensor *temp_sensor1 = mems_expansion_board->ht_sensor;
Bullet_95 0:9bcc849e42db 26 static TempSensor *temp_sensor2 = mems_expansion_board->pt_sensor;
Bullet_95 0:9bcc849e42db 27
Bullet_95 0:9bcc849e42db 28
Bullet_95 0:9bcc849e42db 29 void From_Pressure_mb_To_Altitude_US_Std_Atmosphere_1976_ft(float* Pressure_mb, float* Altitude_ft);
Bullet_95 0:9bcc849e42db 30 void From_Temp_and_pressure_to_altitude_in_feer(float* Pressure_mb, float* temp_in_fahrenheit, float* Altitude_ft);
Bullet_95 0:9bcc849e42db 31 void From_ft_To_m(float* ft, float* m);
Bullet_95 0:9bcc849e42db 32 void altitude_from_press_and_presscurrent(float* Pressure_mb,float* Altitude_m);
Bullet_95 0:9bcc849e42db 33
Bullet_95 0:9bcc849e42db 34 /* Helper function for printing floats & doubles */
Bullet_95 0:9bcc849e42db 35 static char *printDouble(char* str, double v, int decimalDigits=2)
Bullet_95 0:9bcc849e42db 36 {
Bullet_95 0:9bcc849e42db 37 int i = 1;
Bullet_95 0:9bcc849e42db 38 int intPart, fractPart;
Bullet_95 0:9bcc849e42db 39 int len;
Bullet_95 0:9bcc849e42db 40 char *ptr;
Bullet_95 0:9bcc849e42db 41
Bullet_95 0:9bcc849e42db 42 /* prepare decimal digits multiplicator */
Bullet_95 0:9bcc849e42db 43 for (; decimalDigits!=0; i*=10, decimalDigits--);
Bullet_95 0:9bcc849e42db 44
Bullet_95 0:9bcc849e42db 45 /* calculate integer & fractinal parts */
Bullet_95 0:9bcc849e42db 46 intPart = (int)v;
Bullet_95 0:9bcc849e42db 47 fractPart = (int)((v-(double)(int)v)*i);
Bullet_95 0:9bcc849e42db 48
Bullet_95 0:9bcc849e42db 49 /* fill in integer part */
Bullet_95 0:9bcc849e42db 50 sprintf(str, "%i.", intPart);
Bullet_95 0:9bcc849e42db 51
Bullet_95 0:9bcc849e42db 52 /* prepare fill in of fractional part */
Bullet_95 0:9bcc849e42db 53 len = strlen(str);
Bullet_95 0:9bcc849e42db 54 ptr = &str[len];
Bullet_95 0:9bcc849e42db 55
Bullet_95 0:9bcc849e42db 56 /* fill in leading fractional zeros */
Bullet_95 0:9bcc849e42db 57 for (i/=10; i>1; i/=10, ptr++) {
Bullet_95 0:9bcc849e42db 58 if(fractPart >= i) break;
Bullet_95 0:9bcc849e42db 59 *ptr = '0';
Bullet_95 0:9bcc849e42db 60 }
Bullet_95 0:9bcc849e42db 61
Bullet_95 0:9bcc849e42db 62 /* fill in (rest of) fractional part */
Bullet_95 0:9bcc849e42db 63 sprintf(ptr, "%i", fractPart);
Bullet_95 0:9bcc849e42db 64
Bullet_95 0:9bcc849e42db 65 return str;
Bullet_95 0:9bcc849e42db 66 }
Bullet_95 0:9bcc849e42db 67
Bullet_95 0:9bcc849e42db 68
Bullet_95 0:9bcc849e42db 69
Bullet_95 0:9bcc849e42db 70 /* Simple main function */
Bullet_95 0:9bcc849e42db 71 int main()
Bullet_95 0:9bcc849e42db 72 {
Bullet_95 0:9bcc849e42db 73 uint8_t id;
Bullet_95 0:9bcc849e42db 74 float value1, value2;
Bullet_95 0:9bcc849e42db 75 char buffer1[32], buffer2[32];
Bullet_95 0:9bcc849e42db 76 float Altitude_ft,Altitude_m;
Bullet_95 0:9bcc849e42db 77 float m;
Bullet_95 0:9bcc849e42db 78
Bullet_95 0:9bcc849e42db 79
Bullet_95 0:9bcc849e42db 80 pc.printf("\r\n--- Starting new run ---\r\n");
Bullet_95 0:9bcc849e42db 81
Bullet_95 0:9bcc849e42db 82 humidity_sensor->ReadID(&id);
Bullet_95 0:9bcc849e42db 83 pc.printf("HTS221 humidity & temperature = 0x%X\r\n", id);
Bullet_95 0:9bcc849e42db 84 pressure_sensor->ReadID(&id);
Bullet_95 0:9bcc849e42db 85 pc.printf("LPS25H pressure & temperature = 0x%X\r\n", id);
Bullet_95 0:9bcc849e42db 86
Bullet_95 0:9bcc849e42db 87 wait(3);
Bullet_95 0:9bcc849e42db 88
Bullet_95 0:9bcc849e42db 89 while(1) {
Bullet_95 0:9bcc849e42db 90 printf("\r\n");
Bullet_95 0:9bcc849e42db 91
Bullet_95 0:9bcc849e42db 92 temp_sensor1->GetTemperature(&value1);
Bullet_95 0:9bcc849e42db 93 humidity_sensor->GetHumidity(&value2);
Bullet_95 0:9bcc849e42db 94 pc.printf("HTS221: [temp] %7s°C, [hum] %s%%\r\n", printDouble(buffer1, value1), printDouble(buffer2, value2));
Bullet_95 0:9bcc849e42db 95
Bullet_95 0:9bcc849e42db 96 temp_sensor2->GetFahrenheit(&value1);
Bullet_95 0:9bcc849e42db 97 pressure_sensor->GetPressure(&value2);
Bullet_95 0:9bcc849e42db 98 pc.printf("LPS25H: [temp] %7s°F, [press] %smbar\r\n", printDouble(buffer1, value1), printDouble(buffer2, value2));
Bullet_95 0:9bcc849e42db 99
Bullet_95 0:9bcc849e42db 100 From_Pressure_mb_To_Altitude_US_Std_Atmosphere_1976_ft(&value2,&Altitude_ft);
Bullet_95 0:9bcc849e42db 101 From_ft_To_m(&Altitude_ft,&m);
Bullet_95 0:9bcc849e42db 102 pc.printf("LPS25H: [altitude] %7sfeet, [altitude] %smetres\r\n",printDouble(buffer1, Altitude_ft), printDouble(buffer2, m) );
Bullet_95 0:9bcc849e42db 103 temp_sensor1->GetTemperature(&value1);
Bullet_95 0:9bcc849e42db 104 From_Temp_and_pressure_to_altitude_in_feer(&value2, &value1, &Altitude_ft);
Bullet_95 0:9bcc849e42db 105 From_ft_To_m(&Altitude_ft,&m);
Bullet_95 0:9bcc849e42db 106 pc.printf("LPS25H: [altitude] %7sfeet, [altitude] %smetres\r\n",printDouble(buffer1, Altitude_ft), printDouble(buffer2, m) );
Bullet_95 0:9bcc849e42db 107 altitude_from_press_and_presscurrent(&value2,&Altitude_m);
Bullet_95 0:9bcc849e42db 108 pc.printf("LPS25H: [altitude] %smetres\r\n",printDouble(buffer2, Altitude_m) );
Bullet_95 0:9bcc849e42db 109 pc.printf("---\r\n");
Bullet_95 0:9bcc849e42db 110
Bullet_95 0:9bcc849e42db 111 wait(2);
Bullet_95 0:9bcc849e42db 112 }
Bullet_95 0:9bcc849e42db 113 }
Bullet_95 0:9bcc849e42db 114 void altitude_from_press_and_presscurrent(float* Pressure_mb,float* Altitude_m){
Bullet_95 0:9bcc849e42db 115
Bullet_95 0:9bcc849e42db 116 *Altitude_m=44330*(1-pow(*Pressure_mb/1013.25,1/5.255));
Bullet_95 0:9bcc849e42db 117
Bullet_95 0:9bcc849e42db 118 }
Bullet_95 0:9bcc849e42db 119
Bullet_95 0:9bcc849e42db 120 void From_Temp_and_pressure_to_altitude_in_feer(float* Pressure_mb, float* temp_in_celsius, float* Altitude_ft)
Bullet_95 0:9bcc849e42db 121 {
Bullet_95 0:9bcc849e42db 122 float tempf=*temp_in_celsius;
Bullet_95 0:9bcc849e42db 123 float PA=*Pressure_mb;
Bullet_95 0:9bcc849e42db 124 *Altitude_ft=1.24*PA + 118.8*tempf-1782;
Bullet_95 0:9bcc849e42db 125
Bullet_95 0:9bcc849e42db 126 }
Bullet_95 0:9bcc849e42db 127
Bullet_95 0:9bcc849e42db 128 void From_Pressure_mb_To_Altitude_US_Std_Atmosphere_1976_ft(float*
Bullet_95 0:9bcc849e42db 129 Pressure_mb, float* Altitude_ft)
Bullet_95 0:9bcc849e42db 130 {
Bullet_95 0:9bcc849e42db 131 //=(1-(Pressure/1013.25)^0.190284)*145366.45
Bullet_95 0:9bcc849e42db 132 *Altitude_ft = (1-pow(*Pressure_mb/1013.25,0.190284))*145366.45;
Bullet_95 0:9bcc849e42db 133 }
Bullet_95 0:9bcc849e42db 134
Bullet_95 0:9bcc849e42db 135 void From_ft_To_m(float* ft, float* m)
Bullet_95 0:9bcc849e42db 136 {
Bullet_95 0:9bcc849e42db 137 //=feet/3.280839895
Bullet_95 0:9bcc849e42db 138 *m = *ft/3.280839895;
Bullet_95 0:9bcc849e42db 139 }