The Technology Partnership / Mbed 2 deprecated MS5607_test_serial

spit out pressure samples and FFT bins

Dependencies:   MS5607 mbed-dsp mbed

main.cpp@0:ec4e8ecd0db7, 2018-06-07 (annotated)

Committer:
AlexStokoe
Date:
Thu Jun 07 16:55:30 2018 +0000
Revision:
0:ec4e8ecd0db7
change to compensated pressure values;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AlexStokoe 0:ec4e8ecd0db7 1 #include "mbed.h"
AlexStokoe 0:ec4e8ecd0db7 2 #include "MS5607SPI.h"
AlexStokoe 0:ec4e8ecd0db7 3 #include "arm_math.h"
AlexStokoe 0:ec4e8ecd0db7 4
AlexStokoe 0:ec4e8ecd0db7 5 #define N_SAMPLES 1024
AlexStokoe 0:ec4e8ecd0db7 6 #define PEAK_SEARCH_START 1 //first bin to include in peak search
AlexStokoe 0:ec4e8ecd0db7 7 #define PEAK_SEARCH_END 100 //last bin to include in peak search
AlexStokoe 0:ec4e8ecd0db7 8 #define SUM_AROUND_PEAK 2 //+/- this many bins
AlexStokoe 0:ec4e8ecd0db7 9
AlexStokoe 0:ec4e8ecd0db7 10 #define REGRESSION_SLOPE 53.77986f //from Excel fit to test data
AlexStokoe 0:ec4e8ecd0db7 11 #define REGRESSION_INTERCEPT -80.07f
AlexStokoe 0:ec4e8ecd0db7 12
AlexStokoe 0:ec4e8ecd0db7 13 Serial pc(USBTX, USBRX);
AlexStokoe 0:ec4e8ecd0db7 14
AlexStokoe 0:ec4e8ecd0db7 15 DigitalOut myled(LED1);
AlexStokoe 0:ec4e8ecd0db7 16 PwmOut drive(p26);
AlexStokoe 0:ec4e8ecd0db7 17 AnalogIn motorVolt(p19);
AlexStokoe 0:ec4e8ecd0db7 18 AnalogIn motorAmp(p20);
AlexStokoe 0:ec4e8ecd0db7 19 DigitalOut ps(p9);
AlexStokoe 0:ec4e8ecd0db7 20
AlexStokoe 0:ec4e8ecd0db7 21 DigitalOut trig(p21);
AlexStokoe 0:ec4e8ecd0db7 22
AlexStokoe 0:ec4e8ecd0db7 23 Timer sample_timer;
AlexStokoe 0:ec4e8ecd0db7 24 Timer iter_timer;
AlexStokoe 0:ec4e8ecd0db7 25
AlexStokoe 0:ec4e8ecd0db7 26 MS5607SPI pressure_sensor(p5, p6, p7, p8); // mosi, miso, sclk, cs
AlexStokoe 0:ec4e8ecd0db7 27
AlexStokoe 0:ec4e8ecd0db7 28 arm_rfft_fast_instance_f32 rfft;
AlexStokoe 0:ec4e8ecd0db7 29
AlexStokoe 0:ec4e8ecd0db7 30 float p_data[N_SAMPLES];
AlexStokoe 0:ec4e8ecd0db7 31 float p_data_raw[N_SAMPLES];
AlexStokoe 0:ec4e8ecd0db7 32 int p_data_rawi[N_SAMPLES];
AlexStokoe 0:ec4e8ecd0db7 33 int t_data[N_SAMPLES];
AlexStokoe 0:ec4e8ecd0db7 34 float f_data[N_SAMPLES];
AlexStokoe 0:ec4e8ecd0db7 35 float mag_data[N_SAMPLES / 2 + 1];
AlexStokoe 0:ec4e8ecd0db7 36
AlexStokoe 0:ec4e8ecd0db7 37 unsigned int tData[N_SAMPLES];
AlexStokoe 0:ec4e8ecd0db7 38
AlexStokoe 0:ec4e8ecd0db7 39 unsigned int coefficients[7];
AlexStokoe 0:ec4e8ecd0db7 40 char str[32];
AlexStokoe 0:ec4e8ecd0db7 41 float motorDuty;
AlexStokoe 0:ec4e8ecd0db7 42 float motorCurr;
AlexStokoe 0:ec4e8ecd0db7 43 float sample_rate;
AlexStokoe 0:ec4e8ecd0db7 44
AlexStokoe 0:ec4e8ecd0db7 45 float blockMag;
AlexStokoe 0:ec4e8ecd0db7 46
AlexStokoe 0:ec4e8ecd0db7 47 //run flow block functionality and print some of sample out to console for analysis
AlexStokoe 0:ec4e8ecd0db7 48 float flowBlock(float duty, int dataPrint) {
AlexStokoe 0:ec4e8ecd0db7 49 if (dataPrint){
AlexStokoe 0:ec4e8ecd0db7 50 pc.printf("STARTING FLOW BLOCK TEST- %3.0f%% Duty Cycle\n", duty*100);
AlexStokoe 0:ec4e8ecd0db7 51 }
AlexStokoe 0:ec4e8ecd0db7 52
AlexStokoe 0:ec4e8ecd0db7 53 //turn motor on hard and settle to required duty cycle (voltage)
AlexStokoe 0:ec4e8ecd0db7 54 drive.write(1.0f);
AlexStokoe 0:ec4e8ecd0db7 55 wait_ms(100);
AlexStokoe 0:ec4e8ecd0db7 56 drive.write(duty);
AlexStokoe 0:ec4e8ecd0db7 57
AlexStokoe 0:ec4e8ecd0db7 58 int iter_counter = 0; //Iteration counter
AlexStokoe 0:ec4e8ecd0db7 59 iter_timer.reset();
AlexStokoe 0:ec4e8ecd0db7 60 iter_timer.start();
AlexStokoe 0:ec4e8ecd0db7 61 float temperature_raw = pressure_sensor.getRawTemperature();
AlexStokoe 0:ec4e8ecd0db7 62
AlexStokoe 0:ec4e8ecd0db7 63 int a = 0;
AlexStokoe 0:ec4e8ecd0db7 64 while (a<2000) {
AlexStokoe 0:ec4e8ecd0db7 65 pressure_sensor.getRawPressure();
AlexStokoe 0:ec4e8ecd0db7 66 a++;
AlexStokoe 0:ec4e8ecd0db7 67 }
AlexStokoe 0:ec4e8ecd0db7 68
AlexStokoe 0:ec4e8ecd0db7 69 sample_timer.reset();
AlexStokoe 0:ec4e8ecd0db7 70 sample_timer.start();
AlexStokoe 0:ec4e8ecd0db7 71
AlexStokoe 0:ec4e8ecd0db7 72 //Capture raw pressure samples
AlexStokoe 0:ec4e8ecd0db7 73 for(int i = 0; i < N_SAMPLES; i++) {
AlexStokoe 0:ec4e8ecd0db7 74 trig = 1;
AlexStokoe 0:ec4e8ecd0db7 75 p_data_raw[i] = pressure_sensor.calculatePressure(pressure_sensor.getRawPressure(), temperature_raw);
AlexStokoe 0:ec4e8ecd0db7 76 trig = 0;
AlexStokoe 0:ec4e8ecd0db7 77 p_data[i] = p_data_raw[i];
AlexStokoe 0:ec4e8ecd0db7 78 }
AlexStokoe 0:ec4e8ecd0db7 79
AlexStokoe 0:ec4e8ecd0db7 80 sample_timer.stop();
AlexStokoe 0:ec4e8ecd0db7 81 motorCurr = 0;
AlexStokoe 0:ec4e8ecd0db7 82 for(int x=0; x<10; x++){
AlexStokoe 0:ec4e8ecd0db7 83 motorCurr = motorCurr + motorAmp.read();
AlexStokoe 0:ec4e8ecd0db7 84 }
AlexStokoe 0:ec4e8ecd0db7 85 //turn off motor
AlexStokoe 0:ec4e8ecd0db7 86 drive.write(0.0);
AlexStokoe 0:ec4e8ecd0db7 87
AlexStokoe 0:ec4e8ecd0db7 88
AlexStokoe 0:ec4e8ecd0db7 89
AlexStokoe 0:ec4e8ecd0db7 90 //http://www.keil.com/pack/doc/CMSIS/DSP/html/group__RealFFT.html
AlexStokoe 0:ec4e8ecd0db7 91 //Compute the RFFT
AlexStokoe 0:ec4e8ecd0db7 92 //
AlexStokoe 0:ec4e8ecd0db7 93 //p_data is trashed in the process
AlexStokoe 0:ec4e8ecd0db7 94 //Result is packaged as [real_0, real_N/2, real_1, imag_1, real_2, imag_2 ... real_N/2-1, imag_N/2-1]
AlexStokoe 0:ec4e8ecd0db7 95 arm_rfft_fast_init_f32(&rfft, N_SAMPLES);
AlexStokoe 0:ec4e8ecd0db7 96 arm_rfft_fast_f32(&rfft, p_data, f_data, 0);
AlexStokoe 0:ec4e8ecd0db7 97
AlexStokoe 0:ec4e8ecd0db7 98 //http://www.keil.com/pack/doc/CMSIS/DSP/html/group__cmplx__mag.html
AlexStokoe 0:ec4e8ecd0db7 99 //Convert to magntiude, skip over the DC and fundamental terms
AlexStokoe 0:ec4e8ecd0db7 100
AlexStokoe 0:ec4e8ecd0db7 101 arm_cmplx_mag_f32(&f_data[2], &mag_data[1], (N_SAMPLES / 2) - 1);
AlexStokoe 0:ec4e8ecd0db7 102 //Fill in the first and last terms from the first entry in f_data
AlexStokoe 0:ec4e8ecd0db7 103 mag_data[0] = f_data[0];
AlexStokoe 0:ec4e8ecd0db7 104 mag_data[N_SAMPLES / 2] = f_data[1];
AlexStokoe 0:ec4e8ecd0db7 105
AlexStokoe 0:ec4e8ecd0db7 106
AlexStokoe 0:ec4e8ecd0db7 107 //Find peak in spectrum
AlexStokoe 0:ec4e8ecd0db7 108 float max_mag = -1;
AlexStokoe 0:ec4e8ecd0db7 109 int max_mag_i = -1;
AlexStokoe 0:ec4e8ecd0db7 110 if (dataPrint) {
AlexStokoe 0:ec4e8ecd0db7 111
AlexStokoe 0:ec4e8ecd0db7 112 for (int x=0; x < N_SAMPLES; x++){
AlexStokoe 0:ec4e8ecd0db7 113 pc.printf("%f, %f\n", p_data_raw[x], mag_data[x]);
AlexStokoe 0:ec4e8ecd0db7 114 }
AlexStokoe 0:ec4e8ecd0db7 115
AlexStokoe 0:ec4e8ecd0db7 116 }
AlexStokoe 0:ec4e8ecd0db7 117
AlexStokoe 0:ec4e8ecd0db7 118 for(int i = PEAK_SEARCH_START + SUM_AROUND_PEAK; i < PEAK_SEARCH_END; i++) {
AlexStokoe 0:ec4e8ecd0db7 119 if (dataPrint) {
AlexStokoe 0:ec4e8ecd0db7 120 //pc.printf("%10f \n", mag_data[i]);
AlexStokoe 0:ec4e8ecd0db7 121 }
AlexStokoe 0:ec4e8ecd0db7 122 if(mag_data[i] > max_mag) {
AlexStokoe 0:ec4e8ecd0db7 123 max_mag_i = i;
AlexStokoe 0:ec4e8ecd0db7 124 max_mag = mag_data[i];
AlexStokoe 0:ec4e8ecd0db7 125 }
AlexStokoe 0:ec4e8ecd0db7 126 }
AlexStokoe 0:ec4e8ecd0db7 127 //pc.printf("max mag=%f\n", max_mag);
AlexStokoe 0:ec4e8ecd0db7 128
AlexStokoe 0:ec4e8ecd0db7 129 //Sum surrounding
AlexStokoe 0:ec4e8ecd0db7 130 float sum = 0.f;
AlexStokoe 0:ec4e8ecd0db7 131 for(int i = max_mag_i - SUM_AROUND_PEAK; i < (max_mag_i + SUM_AROUND_PEAK + 1); i++) {
AlexStokoe 0:ec4e8ecd0db7 132 sum += mag_data[i];
AlexStokoe 0:ec4e8ecd0db7 133 //printf("%10f ", sum);
AlexStokoe 0:ec4e8ecd0db7 134 }
AlexStokoe 0:ec4e8ecd0db7 135
AlexStokoe 0:ec4e8ecd0db7 136
AlexStokoe 0:ec4e8ecd0db7 137
AlexStokoe 0:ec4e8ecd0db7 138 //Scale by N_SAMPLES to recover pressure in Pa
AlexStokoe 0:ec4e8ecd0db7 139 sum /= N_SAMPLES;
AlexStokoe 0:ec4e8ecd0db7 140
AlexStokoe 0:ec4e8ecd0db7 141 //Apply inverse of logarithmic regression from test data
AlexStokoe 0:ec4e8ecd0db7 142 float t90 = exp((sum + 80.07) / 53.77986);
AlexStokoe 0:ec4e8ecd0db7 143
AlexStokoe 0:ec4e8ecd0db7 144
AlexStokoe 0:ec4e8ecd0db7 145 //Output iteration data
AlexStokoe 0:ec4e8ecd0db7 146 iter_counter++;
AlexStokoe 0:ec4e8ecd0db7 147 sample_rate = 1000000 / (sample_timer.read_us() / N_SAMPLES);
AlexStokoe 0:ec4e8ecd0db7 148 if (dataPrint) {
AlexStokoe 0:ec4e8ecd0db7 149 pc.printf(" titer fsamp avpress fmax sum_peak t90\n");
AlexStokoe 0:ec4e8ecd0db7 150 pc.printf("%8.2f, %8.2f, %8.1f, %8.1f, %3.5e, %8.1f\n", iter_timer.read(), sample_rate, f_data[0] / N_SAMPLES, (max_mag_i * sample_rate / N_SAMPLES), sum, t90);
AlexStokoe 0:ec4e8ecd0db7 151 //pc.printf("$SAM\n");
AlexStokoe 0:ec4e8ecd0db7 152 //pc.printf("$%d\n", sample_timer.read_us());
AlexStokoe 0:ec4e8ecd0db7 153 }
AlexStokoe 0:ec4e8ecd0db7 154
AlexStokoe 0:ec4e8ecd0db7 155 return sum;
AlexStokoe 0:ec4e8ecd0db7 156
AlexStokoe 0:ec4e8ecd0db7 157 }
AlexStokoe 0:ec4e8ecd0db7 158 //run flow block functionality and print some of sample out to console for analysis
AlexStokoe 0:ec4e8ecd0db7 159 void flowBlockRaw(float duty) {
AlexStokoe 0:ec4e8ecd0db7 160
AlexStokoe 0:ec4e8ecd0db7 161 pc.printf("START\n");
AlexStokoe 0:ec4e8ecd0db7 162
AlexStokoe 0:ec4e8ecd0db7 163 //turn motor on hard and settle to required duty cycle (voltage)
AlexStokoe 0:ec4e8ecd0db7 164 drive.write(1.0f);
AlexStokoe 0:ec4e8ecd0db7 165 wait_ms(100);
AlexStokoe 0:ec4e8ecd0db7 166 drive.write(duty);
AlexStokoe 0:ec4e8ecd0db7 167
AlexStokoe 0:ec4e8ecd0db7 168 int iter_counter = 0; //Iteration counter
AlexStokoe 0:ec4e8ecd0db7 169 iter_timer.reset();
AlexStokoe 0:ec4e8ecd0db7 170 iter_timer.start();
AlexStokoe 0:ec4e8ecd0db7 171 float temperature_raw = pressure_sensor.getRawTemperature();
AlexStokoe 0:ec4e8ecd0db7 172
AlexStokoe 0:ec4e8ecd0db7 173 int a = 0;
AlexStokoe 0:ec4e8ecd0db7 174 while (a<2000) {
AlexStokoe 0:ec4e8ecd0db7 175 pressure_sensor.getRawPressure();
AlexStokoe 0:ec4e8ecd0db7 176 a++;
AlexStokoe 0:ec4e8ecd0db7 177 }
AlexStokoe 0:ec4e8ecd0db7 178
AlexStokoe 0:ec4e8ecd0db7 179 sample_timer.reset();
AlexStokoe 0:ec4e8ecd0db7 180 sample_timer.start();
AlexStokoe 0:ec4e8ecd0db7 181
AlexStokoe 0:ec4e8ecd0db7 182 //Capture raw pressure samples
AlexStokoe 0:ec4e8ecd0db7 183 for(int i = 0; i < N_SAMPLES; i++) {
AlexStokoe 0:ec4e8ecd0db7 184 t_data[i] = sample_timer.read_us();
AlexStokoe 0:ec4e8ecd0db7 185 p_data_rawi[i] = pressure_sensor.getRawPressure();
AlexStokoe 0:ec4e8ecd0db7 186 }
AlexStokoe 0:ec4e8ecd0db7 187
AlexStokoe 0:ec4e8ecd0db7 188 sample_timer.stop();
AlexStokoe 0:ec4e8ecd0db7 189
AlexStokoe 0:ec4e8ecd0db7 190 //print to console
AlexStokoe 0:ec4e8ecd0db7 191 for(int i = 0; i < N_SAMPLES; i++) {
AlexStokoe 0:ec4e8ecd0db7 192 pc.printf("%d, %u\n", t_data[i], p_data_rawi[i]);
AlexStokoe 0:ec4e8ecd0db7 193 p_data[i] = (float)p_data_rawi[i];
AlexStokoe 0:ec4e8ecd0db7 194 }
AlexStokoe 0:ec4e8ecd0db7 195
AlexStokoe 0:ec4e8ecd0db7 196 pc.printf("STOP\n");
AlexStokoe 0:ec4e8ecd0db7 197 drive.write(0.0);
AlexStokoe 0:ec4e8ecd0db7 198
AlexStokoe 0:ec4e8ecd0db7 199 }
AlexStokoe 0:ec4e8ecd0db7 200
AlexStokoe 0:ec4e8ecd0db7 201 int main() {
AlexStokoe 0:ec4e8ecd0db7 202 pc.baud(115200);
AlexStokoe 0:ec4e8ecd0db7 203 pc.printf("MS5607 TEST\n");
AlexStokoe 0:ec4e8ecd0db7 204 drive.period_ms(10);
AlexStokoe 0:ec4e8ecd0db7 205 motorDuty = 1.0f;
AlexStokoe 0:ec4e8ecd0db7 206 drive.write(motorDuty);
AlexStokoe 0:ec4e8ecd0db7 207 ps = 0;
AlexStokoe 0:ec4e8ecd0db7 208
AlexStokoe 0:ec4e8ecd0db7 209 pressure_sensor.reset();
AlexStokoe 0:ec4e8ecd0db7 210 //pc.printf("CRC check %d\n", pressure_sensor.checkCRC());
AlexStokoe 0:ec4e8ecd0db7 211 //pressure_sensor.printCoefficients(str);
AlexStokoe 0:ec4e8ecd0db7 212 //pc.printf(str);
AlexStokoe 0:ec4e8ecd0db7 213
AlexStokoe 0:ec4e8ecd0db7 214
AlexStokoe 0:ec4e8ecd0db7 215 motorDuty = 1.0f;
AlexStokoe 0:ec4e8ecd0db7 216
AlexStokoe 0:ec4e8ecd0db7 217 for (int i = 0; i<10; i++){
AlexStokoe 0:ec4e8ecd0db7 218
AlexStokoe 0:ec4e8ecd0db7 219 //pc.printf("%3.0f, %f \n", motorDuty*100, flowBlock(motorDuty, 1));
AlexStokoe 0:ec4e8ecd0db7 220 //motorDuty = motorDuty + 0.01;
AlexStokoe 0:ec4e8ecd0db7 221 flowBlock(motorDuty, 1);
AlexStokoe 0:ec4e8ecd0db7 222
AlexStokoe 0:ec4e8ecd0db7 223
AlexStokoe 0:ec4e8ecd0db7 224
AlexStokoe 0:ec4e8ecd0db7 225 }
AlexStokoe 0:ec4e8ecd0db7 226 pc.printf("END\n");
AlexStokoe 0:ec4e8ecd0db7 227
AlexStokoe 0:ec4e8ecd0db7 228 }