upm  1.7.1
Sensor/Actuator repository for libmraa (v2.0.0)
mmc35240.hpp
1 /*
2  * Author: Lay, Kuan Loon <kuan.loon.lay@intel.com>
3  * Copyright (c) 2016 Intel Corporation.
4  *
5  * Permission is hereby granted, free of charge, to any person obtaining
6  * a copy of this software and associated documentation files (the
7  * "Software"), to deal in the Software without restriction, including
8  * without limitation the rights to use, copy, modify, merge, publish,
9  * distribute, sublicense, and/or sell copies of the Software, and to
10  * permit persons to whom the Software is furnished to do so, subject to
11  * the following conditions:
12  *
13  * The above copyright notice and this permission notice shall be
14  * included in all copies or substantial portions of the Software.
15  *
16  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
18  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
20  * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
21  * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
22  * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
23  * Thanks to https://github.com/01org/android-iio-sensors-hal for magnetometer
24  * calibration and denoise algorithm.
25  */
26 #pragma once
27 
28 #include <string>
29 #include <mraa/iio.h>
30 
31 // Adopt
32 // https://android.googlesource.com/platform/frameworks/native/+/refs/heads/master/services/sensorservice/mat.h
33 #include "mat.h"
34 
35 #define MAGN_DS_SIZE 32
36 
37 namespace upm
38 {
60 class MMC35240
61 {
62  public:
63  typedef struct {
64  /* hard iron offsets */
66 
67  /* soft iron matrix */
69 
70  /* geomagnetic strength */
71  double bfield;
72 
73  /* selection data */
74  float sample[MAGN_DS_SIZE][3];
75  unsigned int sample_count;
76  float average[3];
77  } compass_cal_t;
78 
79  typedef double mat_input_t[MAGN_DS_SIZE][3];
80 
81  typedef struct {
82  int max_samples; /* Maximum averaging window size */
83  int num_fields; /* Number of fields per sample (usually 3) */
84  float* history; /* Working buffer containing recorded samples */
85  float* history_sum; /* The current sum of the history elements */
86  int history_size; /* Number of recorded samples */
87  int history_entries; /* How many of these are initialized */
88  int history_index; /* Index of sample to evict next time */
90 
96  MMC35240(int device);
97 
101  ~MMC35240();
102 
111  void installISR(void (*isr)(char*, void*), void* arg);
112 
118  int64_t getChannelValue(unsigned char* input, mraa_iio_channel* chan);
119 
124  bool enableBuffer(int length);
125 
129  bool disableBuffer();
130 
137  bool setScale(const float scale);
138 
145  bool setSamplingFrequency(const float sampling_frequency);
146 
150  bool enable3AxisChannel();
151 
159  void extract3Axis(char* data, float* x, float* y, float* z);
160 
164  int getCalibratedLevel();
165 
169  void initCalibrate();
170 
174  void
175  getCalibratedData(int* cal_level, double offset[3][1], double w_invert[3][3], double* bfield);
176 
180  void
181  loadCalibratedData(int cal_level, double offset[3][1], double w_invert[3][3], double bfield);
182 
183  private:
184  /* Adopt https://github.com/01org/android-iio-sensors-hal/blob/master/compass-calibration.c */
185  void resetSample(compass_cal_t* data);
186  void calibrateCompass(float* x, float* y, float* z, compass_cal_t* cal_data);
187  int compassCollect(float* x, float* y, float* z, compass_cal_t* cal_data);
188  int compassReady(compass_cal_t* cal_data);
189  double calcSquareErr(compass_cal_t* data);
190  int ellipsoidFit(mat_input_t& m,
192  android::mat<double, 3, 3>& w_invert,
193  double* bfield);
194  void computeEigenvalues(android::mat<double, 3, 3>& A, double* eig1, double* eig2, double* eig3);
195  void calcEvector(android::mat<double, 3, 3>& A, double eig, android::mat<double, 3, 1>& vec);
196  void scale(float* x, float* y, float* z);
197  void compassComputeCal(float* x, float* y, float* z, compass_cal_t* cal_data);
198 
199  /* Adopt https://github.com/01org/android-iio-sensors-hal/blob/master/filtering.c */
200  void denoise_average(float* x, float* y, float* z);
201 
202  mraa_iio_context m_iio;
203  int m_iio_device_num;
204  float m_sampling_frequency; // sampling frequency
205  bool m_mount_matrix_exist; // is mount matrix exist
206  float m_mount_matrix[9]; // mount matrix
207  float m_scale; // data scale
208  compass_cal_t m_cal_data; // calibrate data
209  int m_cal_level; // calibrated level
210  filter_average_t m_filter; // filter data
211 };
212 }
void loadCalibratedData(int cal_level, double offset[3][1], double w_invert[3][3], double bfield)
Definition: mmc35240.cxx:284
bool enableBuffer(int length)
Definition: mmc35240.cxx:160
bool disableBuffer()
Definition: mmc35240.cxx:169
void extract3Axis(char *data, float *x, float *y, float *z)
Definition: mmc35240.cxx:208
int getCalibratedLevel()
Definition: mmc35240.cxx:238
int64_t getChannelValue(unsigned char *input, mraa_iio_channel *chan)
Definition: mmc35240.cxx:108
C++ API wrapper for the bh1749 driver.
Definition: a110x.hpp:29
MMC35240(int device)
Definition: mmc35240.cxx:53
bool enable3AxisChannel()
Definition: mmc35240.cxx:192
~MMC35240()
Definition: mmc35240.cxx:87
Definition: mmc35240.hpp:81
void getCalibratedData(int *cal_level, double offset[3][1], double w_invert[3][3], double *bfield)
Definition: mmc35240.cxx:265
bool setScale(const float scale)
Definition: mmc35240.cxx:176
MMC35240 Tri-axis Magnetic Sensor API.
Definition: mmc35240.hpp:60
void installISR(void(*isr)(char *, void *), void *arg)
Definition: mmc35240.cxx:102
Definition: mmc35240.hpp:63
bool setSamplingFrequency(const float sampling_frequency)
Definition: mmc35240.cxx:184
void initCalibrate()
Definition: mmc35240.cxx:244