mmc35240.hpp

/*
 * Author: Lay, Kuan Loon <kuan.loon.lay@intel.com>
 * Copyright (c) 2016 Intel Corporation.
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject to
 * the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 * Thanks to https://github.com/01org/android-iio-sensors-hal for magnetometer
 * calibration and denoise algorithm.
 */
#pragma once

#include <string>
#include <mraa/iio.h>

// Adopt
// https://android.googlesource.com/platform/frameworks/native/+/refs/heads/master/services/sensorservice/mat.h
#include "mat.h"

#define MAGN_DS_SIZE 32

namespace upm
{
class MMC35240
{
  public:
    typedef struct {
        /* hard iron offsets */
        android::mat<double, 3, 1> offset;

        /* soft iron matrix */
        android::mat<double, 3, 3> w_invert;

        /* geomagnetic strength */
        double bfield;

        /* selection data */
        float sample[MAGN_DS_SIZE][3];
        unsigned int sample_count;
        float average[3];
    } compass_cal_t;

    typedef double mat_input_t[MAGN_DS_SIZE][3];

    typedef struct {
        int max_samples;     /* Maximum averaging window size */
        int num_fields;      /* Number of fields per sample (usually 3) */
        float* history;      /* Working buffer containing recorded samples */
        float* history_sum;  /* The current sum of the history elements */
        int history_size;    /* Number of recorded samples */
        int history_entries; /* How many of these are initialized */
        int history_index;   /* Index of sample to evict next time */
    } filter_average_t;

    MMC35240(int device);

    ~MMC35240();

    void installISR(void (*isr)(char*), void* arg);

    int64_t getChannelValue(unsigned char* input, mraa_iio_channel* chan);

    bool enableBuffer(int length);

    bool disableBuffer();

    bool setScale(const float scale);

    bool setSamplingFrequency(const float sampling_frequency);

    bool enable3AxisChannel();

    void extract3Axis(char* data, float* x, float* y, float* z);

    int getCalibratedLevel();

    void initCalibrate();

    void
    getCalibratedData(int* cal_level, double offset[3][1], double w_invert[3][3], double* bfield);

    void
    loadCalibratedData(int cal_level, double offset[3][1], double w_invert[3][3], double bfield);

  private:
    /* Adopt https://github.com/01org/android-iio-sensors-hal/blob/master/compass-calibration.c */
    void resetSample(compass_cal_t* data);
    void calibrateCompass(float* x, float* y, float* z, compass_cal_t* cal_data);
    int compassCollect(float* x, float* y, float* z, compass_cal_t* cal_data);
    int compassReady(compass_cal_t* cal_data);
    double calcSquareErr(compass_cal_t* data);
    int ellipsoidFit(mat_input_t& m,
                     android::mat<double, 3, 1>& offset,
                     android::mat<double, 3, 3>& w_invert,
                     double* bfield);
    void computeEigenvalues(android::mat<double, 3, 3>& A, double* eig1, double* eig2, double* eig3);
    void calcEvector(android::mat<double, 3, 3>& A, double eig, android::mat<double, 3, 1>& vec);
    void scale(float* x, float* y, float* z);
    void compassComputeCal(float* x, float* y, float* z, compass_cal_t* cal_data);

    /* Adopt https://github.com/01org/android-iio-sensors-hal/blob/master/filtering.c */
    void denoise_average(float* x, float* y, float* z);

    mraa_iio_context m_iio;
    int m_iio_device_num;
    float m_sampling_frequency; // sampling frequency
    bool m_mount_matrix_exist;  // is mount matrix exist
    float m_mount_matrix[9];    // mount matrix
    float m_scale;              // data scale
    compass_cal_t m_cal_data;   // calibrate data
    int m_cal_level;            // calibrated level
    filter_average_t m_filter;  // filter data
};
}