Obs_Calibration_Vector.cpp 6.46 KB
Newer Older
Achim Morschhauser's avatar
Achim Morschhauser committed
1
2
3
4
5
6
// C++ headers
#include <string>           // String
#include <iostream>         // String streams
#include <fstream>          // File operations
#include <unordered_map>

Achim Morschhauser's avatar
Achim Morschhauser committed
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
#include <Obs_Calibration_Vector.hpp>
#include <stdio.h>

Obs_Calibration_Vector::Obs_Calibration_Vector(double offset, double scale){

	for (int i=0;i<3;i++){
		this->offset[i]=0.0;
		this->scale[i]=1.0;
	}

	this->offset[0] = offset;
	this->scale[0]  = scale;

	if (offset==0 && scale==1){
		do_cal=0;
	} else {
		do_cal=1;
	}

}

Obs_Calibration_Vector::Obs_Calibration_Vector(double* offset, double* scale){

	for (int i=0;i<3;i++){
		this->offset[i]=offset[i];
		this->scale[i]=scale[i];
		if (do_cal==0 && (offset[i]!=0 || scale[i]!=1)){
			do_cal=1;
		}
	}

}

Obs_Calibration_Vector::Obs_Calibration_Vector(
		double scale_X, double scale_Y, double scale_Z,
        double offset_X, double offset_Y, double offset_Z){

	do_cal=0;

	this->scale[0]=scale_X;
	this->scale[1]=scale_Y;
	this->scale[2]=scale_Z;
	this->offset[0]=offset_X;
	this->offset[1]=offset_Y;
	this->offset[2]=offset_Z;

	for (int i=0; i<3; i++){
		if (do_cal==0 && (offset[i]!=0 || scale[i]!=1)){
			do_cal=1;
			break;
		}
	}

}

/****************************************************************************
 ****************************************************************************
 *																			*
 * 				PUBLIC METHODS												*
 *																			*
 ****************************************************************************
 ***************************************************************************/

/****************************************************************************
 *                                                                                                                                                      *
 * Calibrate vector data                                                                                                        *
 *                                                                                                                                                      *
 ***************************************************************************/
int Obs_Calibration_Vector::calibrate(data_obs_vector* vdata){

        if (do_cal){

                double data[5];

                vdata->get_data(data);

                for (int i=0;i<3;i++){
                        //printf("SCALE: %f ",data[i]);
                        data[i]=data[i]*scale[i]+offset[i];
                        //printf("%f %f\n",data[i],scale[i]);
                }

                vdata->set_data(data);

        }

        return(0);



}


/****************************************************************************
 *                                                                                                                                                      *
 * Calibrate vector data                                                                                                        *
 *                                                                                                                                                      *
 ***************************************************************************/
int Obs_Calibration_Vector::calibrate(data_obs_scalar* sdata){

        if (do_cal){

                double data;

                data=sdata->get_value();

                data=data*scale[0]+offset[0];

                sdata->set_value(data);

        }

        return(0);

}




/****************************************************************************
 *																		 	*
 * Calibration constants for the ADC													*
 *																			*
 ***************************************************************************/
Achim Morschhauser's avatar
Achim Morschhauser committed
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
int Obs_Calibration_Vector::get_adc_calibrate(std::string config,
        std::string &offset, std::string &scale){

    std::unordered_map<std::string, struct adc_cal>::iterator it;
    struct adc_cal cal;
    
    it = map_ADC.find(config);
    
    if (it != map_ADC.end()) {
        cal     = it->second;
        scale   = cal.scale;
        offset  = cal.offset;
    }
    
    return(-1);
Achim Morschhauser's avatar
Achim Morschhauser committed
146
147
148
149
150
151
152
153

}

/****************************************************************************
 *																		 	*
 * Calibration constants for the ADC													*
 *																			*
 ***************************************************************************/
Achim Morschhauser's avatar
Achim Morschhauser committed
154
155
int Obs_Calibration_Vector::add_adc_calibrate(std::string config, 
        std::string offset, std::string scale){
Achim Morschhauser's avatar
Achim Morschhauser committed
156

Achim Morschhauser's avatar
Achim Morschhauser committed
157
158
159
160
161
162
163
164
    struct adc_cal cal;
    
    cal.offset = offset;
    cal.scale  = scale;
    
    map_ADC[config]=cal;

    return(0);
Achim Morschhauser's avatar
Achim Morschhauser committed
165
166

}
Achim Morschhauser's avatar
Achim Morschhauser committed
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230

/****************************************************************************
 *
 * Save the calibration constants to file
 *
 ***************************************************************************/
 int Obs_Calibration_Vector::save_adc_calibrate(std::string filename) {
     
     /* PROBLEM: Save the comments in ADC file when new configurations are
      * stored.
      * SOLUTION: Save whole file in string before writing. Then, rewrite
      * file. Comment out duplicates instead of overwriting.
      */
     
     return(0);
 }
        
/****************************************************************************
 *
 * Read the calibration constants to file
 *
 ***************************************************************************/
 int Obs_Calibration_Vector::read_adc_calibrate(std::string filename) {
     
    std::ifstream  file;
    std::string    line;
    
    std::string    config;  // configuration string used as key
    struct adc_cal cal;     // calibration constants used as value
    
    size_t pos_c;
    
    // Open file
    file.open(filename,std::ios::out);
    
    // Read from file
    if (file.is_open()) {
      while ( getline (file,line) )  {
        // Find comment token ("#")
        pos_c = line.find('#');
        line=line.substr(0,pos_c);
        if (line.size()==0) continue;
        // Find configuration string
        pos_c=line.find(',');
        config=line.substr(0,pos_c);
        // Find calibration string
        line=line.substr(pos_c+1);
        pos_c=line.find(',');
        cal.offset = line.substr(0,pos_c);
        cal.scale  = line.substr(pos_c+1);
        // Save key-value pair
        map_ADC[config]=cal;
        std::cout << "Read ADC: " << config << " " << cal.offset << 
                " " << cal.scale << std::endl;
      }
      file.close();
    } else {
      std::cout << "Unable to open file: " << filename << std::endl;
    }


    return 0;
     
 }