diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/drivers/ao_mpu6000.c | 18 | ||||
| -rw-r--r-- | src/drivers/ao_mpu6000.h | 37 | ||||
| -rw-r--r-- | src/test/Makefile | 4 | ||||
| -rw-r--r-- | src/test/ao_micropeak_test.c | 40 | ||||
| -rwxr-xr-x | src/test/plotmicro | 4 | ||||
| -rw-r--r-- | src/util/atmosphere.5c | 153 |
6 files changed, 247 insertions, 9 deletions
diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c index 49596705..6d47482c 100644 --- a/src/drivers/ao_mpu6000.c +++ b/src/drivers/ao_mpu6000.c @@ -199,6 +199,24 @@ ao_mpu6000_setup(void) ao_delay(AO_MS_TO_TICKS(200)); ao_mpu6000_sample(&test_mode); +#if TRIDGE + // read the product ID rev c has 1/2 the sensitivity of rev d + _mpu6000_product_id = _register_read(MPUREG_PRODUCT_ID); + //Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id); + + if ((_mpu6000_product_id == MPU6000ES_REV_C4) || (_mpu6000_product_id == MPU6000ES_REV_C5) || + (_mpu6000_product_id == MPU6000_REV_C4) || (_mpu6000_product_id == MPU6000_REV_C5)) { + // Accel scale 8g (4096 LSB/g) + // Rev C has different scaling than rev D + register_write(MPUREG_ACCEL_CONFIG,1<<3); + } else { + // Accel scale 8g (4096 LSB/g) + register_write(MPUREG_ACCEL_CONFIG,2<<3); + } + hal.scheduler->delay(1); + +#endif + /* Configure accelerometer to +/-16G */ ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG, (0 << MPU600_ACCEL_CONFIG_XA_ST) | diff --git a/src/drivers/ao_mpu6000.h b/src/drivers/ao_mpu6000.h index 6aada9a9..f01e9e83 100644 --- a/src/drivers/ao_mpu6000.h +++ b/src/drivers/ao_mpu6000.h @@ -21,6 +21,27 @@ #define MPU6000_ADDR_WRITE 0xd0 #define MPU6000_ADDR_READ 0xd1 +/* From Tridge */ +#define MPUREG_XG_OFFS_TC 0x00 +#define MPUREG_YG_OFFS_TC 0x01 +#define MPUREG_ZG_OFFS_TC 0x02 +#define MPUREG_X_FINE_GAIN 0x03 +#define MPUREG_Y_FINE_GAIN 0x04 +#define MPUREG_Z_FINE_GAIN 0x05 +#define MPUREG_XA_OFFS_H 0x06 // X axis accelerometer offset (high byte) +#define MPUREG_XA_OFFS_L 0x07 // X axis accelerometer offset (low byte) +#define MPUREG_YA_OFFS_H 0x08 // Y axis accelerometer offset (high byte) +#define MPUREG_YA_OFFS_L 0x09 // Y axis accelerometer offset (low byte) +#define MPUREG_ZA_OFFS_H 0x0A // Z axis accelerometer offset (high byte) +#define MPUREG_ZA_OFFS_L 0x0B // Z axis accelerometer offset (low byte) +#define MPUREG_PRODUCT_ID 0x0C // Product ID Register +#define MPUREG_XG_OFFS_USRH 0x13 // X axis gyro offset (high byte) +#define MPUREG_XG_OFFS_USRL 0x14 // X axis gyro offset (low byte) +#define MPUREG_YG_OFFS_USRH 0x15 // Y axis gyro offset (high byte) +#define MPUREG_YG_OFFS_USRL 0x16 // Y axis gyro offset (low byte) +#define MPUREG_ZG_OFFS_USRH 0x17 // Z axis gyro offset (high byte) +#define MPUREG_ZG_OFFS_USRL 0x18 // Z axis gyro offset (low byte) + #define MPU6000_SMPRT_DIV 0x19 #define MPU6000_CONFIG 0x1a @@ -163,4 +184,20 @@ extern struct ao_mpu6000_sample ao_mpu6000_current; void ao_mpu6000_init(void); +/* Product ID Description for MPU6000 + * high 4 bits low 4 bits + * Product Name Product Revision + */ +#define MPU6000ES_REV_C4 0x14 /* 0001 0100 */ +#define MPU6000ES_REV_C5 0x15 /* 0001 0101 */ +#define MPU6000ES_REV_D6 0x16 /* 0001 0110 */ +#define MPU6000ES_REV_D7 0x17 /* 0001 0111 */ +#define MPU6000ES_REV_D8 0x18 /* 0001 1000 */ +#define MPU6000_REV_C4 0x54 /* 0101 0100 */ +#define MPU6000_REV_C5 0x55 /* 0101 0101 */ +#define MPU6000_REV_D6 0x56 /* 0101 0110 */ +#define MPU6000_REV_D7 0x57 /* 0101 0111 */ +#define MPU6000_REV_D8 0x58 /* 0101 1000 */ +#define MPU6000_REV_D9 0x59 /* 0101 1001 */ + #endif /* _AO_MPU6000_H_ */ diff --git a/src/test/Makefile b/src/test/Makefile index 87bd70fe..1c2d771e 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -62,5 +62,5 @@ ao_aprs_data.wav: ao_aprs_test check: ao_fec_test ao_flight_test ao_flight_test_baro run-tests ./ao_fec_test && ./run-tests -ao_micropeak_test: ao_micropeak_test.c ao_microflight.c - cc $(CFLAGS) -o $@ ao_micropeak_test.c -lm
\ No newline at end of file +ao_micropeak_test: ao_micropeak_test.c ao_microflight.c ao_kalman.h + cc $(CFLAGS) -o $@ ao_micropeak_test.c -lm diff --git a/src/test/ao_micropeak_test.c b/src/test/ao_micropeak_test.c index 04686402..5961bd93 100644 --- a/src/test/ao_micropeak_test.c +++ b/src/test/ao_micropeak_test.c @@ -67,10 +67,11 @@ ao_micro_report(void) { if (running) { alt_t ground = ao_pa_to_altitude(pa_ground); - printf ("%6.2f %10d %10d %10d\n", now / 100.0, + printf ("%6.3f %10d %10d %10d %10d %10d\n", now / 100.0, ao_pa_to_altitude(pa) - ground, ao_pa_to_altitude(ao_pa) - ground, - ao_pa_to_altitude(pa_min) - ground); + ao_pa_to_altitude(pa_min) - ground, + ao_pa_speed, ao_pa_accel); } } @@ -92,14 +93,24 @@ ao_pa_get(void) double time; double pressure; static double last_time; + static double last_pressure; static int been_here; static int start_samples; + static int is_mp; + static int use_saved; if (been_here && start_samples < 100) { start_samples++; return; } ao_micro_report(); + if (use_saved) { + pa = last_pressure; + now = last_time; + use_saved = 0; +// printf ("use saved %d %d\n", now, pa); + return; + } for (;;) { if (!fgets(line, sizeof (line), emulator_in)) exit(0); @@ -119,15 +130,32 @@ ao_pa_get(void) } } continue; + } else if (!strcmp(toks[0], "Time")) { + time_id = 0; + pa_id = 1; + is_mp = 1; + continue; } time = strtod(toks[time_id],NULL); pressure = strtod(toks[pa_id],NULL); - if (been_here && time - last_time < 0.1) + time *= 100; + if (been_here && time - last_time < 0.096 * 100) continue; - been_here = 1; + if (is_mp && been_here) { + double avg_pressure = (pressure + last_pressure) / 2.0; + double avg_time = (time + last_time) / 2.0; + + now = avg_time; + pa = avg_pressure; +// printf ("new %d %d\n", now, pa); + use_saved = 1; + } else { + now = floor (time + 0.5); + pa = pressure; + } + last_pressure = pressure; last_time = time; - now = floor (time * 100 + 0.5); - pa = pressure; + been_here = 1; break; } } diff --git a/src/test/plotmicro b/src/test/plotmicro index cdfcc581..bb8f4d1d 100755 --- a/src/test/plotmicro +++ b/src/test/plotmicro @@ -3,12 +3,14 @@ for i in "$@"; do gnuplot -p << EOF & set title "$i" set ylabel "height (m)" +set y2label "accel (m/s²)" set xlabel "time (s)" set xtics border out nomirror set ytics border out nomirror set y2tics border out nomirror plot "$i" using 1:2 with lines lt 2 axes x1y1 title "raw height",\ "$i" using 1:3 with lines lt 4 axes x1y1 title "kalman height",\ - "$i" using 1:4 with lines lt 1 axes x1y1 title "max height" + "$i" using 1:4 with lines lt 1 axes x1y1 title "max height",\ + "$i" using 1:6 with lines lt 3 axes x1y2 title "pa accel" EOF done diff --git a/src/util/atmosphere.5c b/src/util/atmosphere.5c new file mode 100644 index 00000000..9b5107f0 --- /dev/null +++ b/src/util/atmosphere.5c @@ -0,0 +1,153 @@ +#!/usr/bin/nickle -f +/* + * Pressure Sensor Model, version 1.1 + * + * written by Holly Grimes + * + * Uses the International Standard Atmosphere as described in + * "A Quick Derivation relating altitude to air pressure" (version 1.03) + * from the Portland State Aerospace Society, except that the atmosphere + * is divided into layers with each layer having a different lapse rate. + * + * Lapse rate data for each layer was obtained from Wikipedia on Sept. 1, 2007 + * at site <http://en.wikipedia.org/wiki/International_Standard_Atmosphere + * + * Height measurements use the local tangent plane. The postive z-direction is up. + * + * All measurements are given in SI units (Kelvin, Pascal, meter, meters/second^2). + * The lapse rate is given in Kelvin/meter, the gas constant for air is given + * in Joules/(kilogram-Kelvin). + */ + +const real GRAVITATIONAL_ACCELERATION = -9.80665; +const real AIR_GAS_CONSTANT = 287.053; +const int NUMBER_OF_LAYERS = 7; +const real MAXIMUM_ALTITUDE = 84852; +const real MINIMUM_PRESSURE = 0.3734; +const real LAYER0_BASE_TEMPERATURE = 288.15; +const real LAYER0_BASE_PRESSURE = 101325; + +/* lapse rate and base altitude for each layer in the atmosphere */ +const real[NUMBER_OF_LAYERS] lapse_rate = { + -0.0065, 0.0, 0.001, 0.0028, 0.0, -0.0028, -0.002 +}; +const int[NUMBER_OF_LAYERS] base_altitude = { + 0, 11000, 20000, 32000, 47000, 51000, 71000 +}; + + +/* outputs atmospheric pressure associated with the given altitude. altitudes + are measured with respect to the mean sea level */ +real altitude_to_pressure(real altitude) { + + real base_temperature = LAYER0_BASE_TEMPERATURE; + real base_pressure = LAYER0_BASE_PRESSURE; + + real pressure; + real base; /* base for function to determine pressure */ + real exponent; /* exponent for function to determine pressure */ + int layer_number; /* identifies layer in the atmosphere */ + int delta_z; /* difference between two altitudes */ + + if (altitude > MAXIMUM_ALTITUDE) /* FIX ME: use sensor data to improve model */ + return 0; + + /* calculate the base temperature and pressure for the atmospheric layer + associated with the inputted altitude */ + for(layer_number = 0; layer_number < NUMBER_OF_LAYERS - 1 && altitude > base_altitude[layer_number + 1]; layer_number++) { + delta_z = base_altitude[layer_number + 1] - base_altitude[layer_number]; + if (lapse_rate[layer_number] == 0.0) { + exponent = GRAVITATIONAL_ACCELERATION * delta_z + / AIR_GAS_CONSTANT / base_temperature; + base_pressure *= exp(exponent); + } + else { + base = (lapse_rate[layer_number] * delta_z / base_temperature) + 1.0; + exponent = GRAVITATIONAL_ACCELERATION / + (AIR_GAS_CONSTANT * lapse_rate[layer_number]); + base_pressure *= pow(base, exponent); + } + base_temperature += delta_z * lapse_rate[layer_number]; + } + + /* calculate the pressure at the inputted altitude */ + delta_z = altitude - base_altitude[layer_number]; + if (lapse_rate[layer_number] == 0.0) { + exponent = GRAVITATIONAL_ACCELERATION * delta_z + / AIR_GAS_CONSTANT / base_temperature; + pressure = base_pressure * exp(exponent); + } + else { + base = (lapse_rate[layer_number] * delta_z / base_temperature) + 1.0; + exponent = GRAVITATIONAL_ACCELERATION / + (AIR_GAS_CONSTANT * lapse_rate[layer_number]); + pressure = base_pressure * pow(base, exponent); + } + + return pressure; +} + + +/* outputs the altitude associated with the given pressure. the altitude + returned is measured with respect to the mean sea level */ +real pressure_to_altitude(real pressure) { + + real next_base_temperature = LAYER0_BASE_TEMPERATURE; + real next_base_pressure = LAYER0_BASE_PRESSURE; + + real altitude; + real base_pressure; + real base_temperature; + real base; /* base for function to determine base pressure of next layer */ + real exponent; /* exponent for function to determine base pressure + of next layer */ + real coefficient; + int layer_number; /* identifies layer in the atmosphere */ + int delta_z; /* difference between two altitudes */ + + if (pressure < 0) /* illegal pressure */ + return -1; + if (pressure < MINIMUM_PRESSURE) /* FIX ME: use sensor data to improve model */ + return MAXIMUM_ALTITUDE; + + /* calculate the base temperature and pressure for the atmospheric layer + associated with the inputted pressure. */ + layer_number = -1; + do { + layer_number++; + base_pressure = next_base_pressure; + base_temperature = next_base_temperature; + delta_z = base_altitude[layer_number + 1] - base_altitude[layer_number]; + if (lapse_rate[layer_number] == 0.0) { + exponent = GRAVITATIONAL_ACCELERATION * delta_z + / AIR_GAS_CONSTANT / base_temperature; + next_base_pressure *= exp(exponent); + } + else { + base = (lapse_rate[layer_number] * delta_z / base_temperature) + 1.0; + exponent = GRAVITATIONAL_ACCELERATION / + (AIR_GAS_CONSTANT * lapse_rate[layer_number]); + next_base_pressure *= pow(base, exponent); + } + next_base_temperature += delta_z * lapse_rate[layer_number]; + } + while(layer_number < NUMBER_OF_LAYERS - 1 && pressure < next_base_pressure); + + /* calculate the altitude associated with the inputted pressure */ + if (lapse_rate[layer_number] == 0.0) { + coefficient = (AIR_GAS_CONSTANT / GRAVITATIONAL_ACCELERATION) + * base_temperature; + altitude = base_altitude[layer_number] + + coefficient * log(pressure / base_pressure); + } + else { + base = pressure / base_pressure; + exponent = AIR_GAS_CONSTANT * lapse_rate[layer_number] + / GRAVITATIONAL_ACCELERATION; + coefficient = base_temperature / lapse_rate[layer_number]; + altitude = base_altitude[layer_number] + + coefficient * (pow(base, exponent) - 1); + } + + return altitude; +} |