diff --git a/examples/c++/CMakeLists.txt b/examples/c++/CMakeLists.txt index f259184e..4d22d9a6 100644 --- a/examples/c++/CMakeLists.txt +++ b/examples/c++/CMakeLists.txt @@ -333,6 +333,7 @@ add_example (sensortemplate) add_example (p9813) add_example (abpdrrt005pg2a5) add_example (lcdks) +add_example (mmc35240) # These are special cases where you specify example binary, source file and module(s) include_directories (${PROJECT_SOURCE_DIR}/src) diff --git a/examples/c++/mmc35240.cxx b/examples/c++/mmc35240.cxx new file mode 100644 index 00000000..f048082c --- /dev/null +++ b/examples/c++/mmc35240.cxx @@ -0,0 +1,111 @@ +/* + * Author: Lay, Kuan Loon + * 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. + */ +#include +#include +#include +#include +#include "mmc35240.hpp" + +using namespace std; + +int shouldRun = true; +upm::MMC35240* magnetometer; + +void +sig_handler(int signo) +{ + if (signo == SIGINT) + shouldRun = false; +} + +void +data_callback(char* data) +{ + float x, y, z; + double azimuth; + int level; + magnetometer->extract3Axis(data, &x, &y, &z); + /* calibrated level + * UNRELIABLE = 0 + * ACCURACY_LOW = 1 + * ACCURACY_MEDIUM = 2 + * ACCURACY_HIGH = >=3 + */ + level = magnetometer->getCalibratedLevel(); + + if ((x == 0) && (y == 0)) { + cout << "Point (0, 0) is invalid!\n" << endl; + return; + } + + if (x == 0) { + if (y > 0) + azimuth = 0; + else + azimuth = 180; + } else if (y == 0) { + if (x > 0) + azimuth = 90; + else + azimuth = 270; + } else { + if (x > 0) + azimuth = 90 - atan(y / x) * 180 / M_PI; + else + azimuth = 270 - atan(y / x) * 180 / M_PI; + } + + cout << "[Calibrated Level:" << level << ']' << "[Azimuth:" << (int) (360 - azimuth) << ']' + << '\t' << (int) x << '\t' << (int) y << '\t' << (int) z << "[uT]" << endl; +} + +int +main() +{ + signal(SIGINT, sig_handler); + //! [Interesting] + // Instantiate a MMC35240 Magnetic Sensor on iio device 5. This configuration is a reference and + // should be changed per platform/board type. + magnetometer = new upm::MMC35240(5); + // Kernel driver does not allow changing the value of scale at run-time, default scale is + // 0.001000 + magnetometer->setScale(0.001000); + // Available sampling frequency are 1.5, 13, 25, 50 + magnetometer->setSamplingFrequency(25.000000); + magnetometer->enable3AxisChannel(); + magnetometer->installISR(data_callback, NULL); + magnetometer->enableBuffer(16); + + while (shouldRun) { + sleep(1); + } + magnetometer->disableBuffer(); + + //! [Interesting] + cout << "Exiting" << endl; + + delete magnetometer; + + return 0; +} diff --git a/src/javaswig_blacklist b/src/javaswig_blacklist index 18cf7854..bd6a23fc 100644 --- a/src/javaswig_blacklist +++ b/src/javaswig_blacklist @@ -1,3 +1,4 @@ nrf8001 kxcjk1013 l3gd20 +mmc35240 diff --git a/src/mmc35240/CMakeLists.txt b/src/mmc35240/CMakeLists.txt new file mode 100644 index 00000000..219092be --- /dev/null +++ b/src/mmc35240/CMakeLists.txt @@ -0,0 +1,5 @@ +set (libname "mmc35240") +set (libdescription "mmc35240 sensor module") +set (module_src ${libname}.cxx) +set (module_hpp ${libname}.hpp) +upm_module_init() diff --git a/src/mmc35240/jsupm_mmc35240.i b/src/mmc35240/jsupm_mmc35240.i new file mode 100644 index 00000000..b9f943cf --- /dev/null +++ b/src/mmc35240/jsupm_mmc35240.i @@ -0,0 +1,8 @@ +%module jsupm_mmc35240 +%include "../upm.i" + +%{ + #include "mmc35240.hpp" +%} + +%include "mmc35240.hpp" diff --git a/src/mmc35240/mat.h b/src/mmc35240/mat.h new file mode 100644 index 00000000..cc5fc6dc --- /dev/null +++ b/src/mmc35240/mat.h @@ -0,0 +1,406 @@ +/* + * Copyright (C) 2011 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ANDROID_MAT_H +#define ANDROID_MAT_H + +#include "vec.h" +#include "traits.h" + +// ----------------------------------------------------------------------- + +namespace android { + +template +class mat; + +namespace helpers { + +template +mat& doAssign( + mat& lhs, + typename TypeTraits::ParameterType rhs) { + for (size_t i=0 ; i +mat PURE doMul( + const mat& lhs, + const mat& rhs) +{ + mat res; + for (size_t c=0 ; c +vec PURE doMul( + const mat& lhs, + const vec& rhs) +{ + vec res; + for (size_t r=0 ; r +mat PURE doMul( + const vec& lhs, + const mat& rhs) +{ + mat res; + for (size_t c=0 ; c +mat PURE doMul( + const mat& rhs, + typename TypeTraits::ParameterType v) +{ + mat res; + for (size_t c=0 ; c +mat PURE doMul( + typename TypeTraits::ParameterType v, + const mat& rhs) +{ + mat res; + for (size_t c=0 ; c +class mat : public vec< vec, C > { + typedef typename TypeTraits::ParameterType pTYPE; + typedef vec< vec, C > base; +public: + // STL-like interface. + typedef TYPE value_type; + typedef TYPE& reference; + typedef TYPE const& const_reference; + typedef size_t size_type; + size_type size() const { return R*C; } + enum { ROWS = R, COLS = C }; + + + // ----------------------------------------------------------------------- + // default constructors + + mat() { } + mat(const mat& rhs) : base(rhs) { } + mat(const base& rhs) : base(rhs) { } + + // ----------------------------------------------------------------------- + // conversion constructors + + // sets the diagonal to the value, off-diagonal to zero + mat(pTYPE rhs) { + helpers::doAssign(*this, rhs); + } + + // ----------------------------------------------------------------------- + // Assignment + + mat& operator=(const mat& rhs) { + base::operator=(rhs); + return *this; + } + + mat& operator=(const base& rhs) { + base::operator=(rhs); + return *this; + } + + mat& operator=(pTYPE rhs) { + return helpers::doAssign(*this, rhs); + } + + // ----------------------------------------------------------------------- + // non-member function declaration and definition + + friend inline mat PURE operator + (const mat& lhs, const mat& rhs) { + return helpers::doAdd( + static_cast(lhs), + static_cast(rhs)); + } + friend inline mat PURE operator - (const mat& lhs, const mat& rhs) { + return helpers::doSub( + static_cast(lhs), + static_cast(rhs)); + } + + // matrix*matrix + template + friend mat PURE operator * ( + const mat& lhs, + const mat& rhs) { + return helpers::doMul(lhs, rhs); + } + + // matrix*vector + friend vec PURE operator * ( + const mat& lhs, const vec& rhs) { + return helpers::doMul(lhs, rhs); + } + + // vector*matrix + friend mat PURE operator * ( + const vec& lhs, const mat& rhs) { + return helpers::doMul(lhs, rhs); + } + + // matrix*scalar + friend inline mat PURE operator * (const mat& lhs, pTYPE v) { + return helpers::doMul(lhs, v); + } + + // scalar*matrix + friend inline mat PURE operator * (pTYPE v, const mat& rhs) { + return helpers::doMul(v, rhs); + } + + // ----------------------------------------------------------------------- + // streaming operator to set the columns of the matrix: + // example: + // mat33_t m; + // m << v0 << v1 << v2; + + // column_builder<> stores the matrix and knows which column to set + template + struct column_builder { + mat& matrix; + column_builder(mat& matrix) : matrix(matrix) { } + }; + + // operator << is not a method of column_builder<> so we can + // overload it for unauthorized values (partial specialization + // not allowed in class-scope). + // we just set the column and return the next column_builder<> + template + friend column_builder operator << ( + const column_builder& lhs, + const vec& rhs) { + lhs.matrix[PREV_COLUMN+1] = rhs; + return column_builder(lhs.matrix); + } + + // we return void here so we get a compile-time error if the + // user tries to set too many columns + friend void operator << ( + const column_builder& lhs, + const vec& rhs) { + lhs.matrix[C-1] = rhs; + } + + // this is where the process starts. we set the first columns and + // return the next column_builder<> + column_builder<0> operator << (const vec& rhs) { + (*this)[0] = rhs; + return column_builder<0>(*this); + } +}; + +// Specialize column matrix so they're exactly equivalent to a vector +template +class mat : public vec { + typedef vec base; +public: + // STL-like interface. + typedef TYPE value_type; + typedef TYPE& reference; + typedef TYPE const& const_reference; + typedef size_t size_type; + size_type size() const { return R; } + enum { ROWS = R, COLS = 1 }; + + mat() { } + mat(const base& rhs) : base(rhs) { } + mat(const mat& rhs) : base(rhs) { } + mat(const TYPE& rhs) { helpers::doAssign(*this, rhs); } + mat& operator=(const mat& rhs) { base::operator=(rhs); return *this; } + mat& operator=(const base& rhs) { base::operator=(rhs); return *this; } + mat& operator=(const TYPE& rhs) { return helpers::doAssign(*this, rhs); } + // we only have one column, so ignore the index + const base& operator[](size_t) const { return *this; } + base& operator[](size_t) { return *this; } + void operator << (const vec& rhs) { base::operator[](0) = rhs; } +}; + +// ----------------------------------------------------------------------- +// matrix functions + +// transpose. this handles matrices of matrices +inline int PURE transpose(int v) { return v; } +inline float PURE transpose(float v) { return v; } +inline double PURE transpose(double v) { return v; } + +// Transpose a matrix +template +mat PURE transpose(const mat& m) { + mat r; + for (size_t i=0 ; i +mat PURE multiply(const mat& m1, const mat& m2) +{ + mat r; + for (size_t i = 0; i < M; i++) + for (size_t k = 0; k < P; k++) { + r [i][k] = 0; + for (size_t j = 0; j < N; j++) + r [i][k] += m1[i][j] * m2 [j][k]; + } + return r; +} + +// Calculate the trace of a matrix +template static TYPE trace(const mat& m) { + TYPE t; + for (size_t i=0 ; i +static bool isPositiveSemidefinite(const mat& m, TYPE tolerance) { + for (size_t i=0 ; i tolerance) + return false; + + return true; +} + +// Transpose a vector +template < + template class VEC, + typename TYPE, + size_t SIZE +> +mat PURE transpose(const VEC& v) { + mat r; + for (size_t i=0 ; i +mat PURE invert(const mat& src) { + T t; + size_t swap; + mat tmp(src); + mat inverse(1); + + for (size_t i=0 ; i fabs(tmp[i][i])) { + swap = j; + } + } + + if (swap != i) { + /* swap rows. */ + for (size_t k=0 ; k mat22_t; +typedef mat mat33_t; +typedef mat mat44_t; + +// ----------------------------------------------------------------------- + +}; // namespace android + +#endif /* ANDROID_MAT_H */ diff --git a/src/mmc35240/mmc35240.cxx b/src/mmc35240/mmc35240.cxx new file mode 100644 index 00000000..24cba9fb --- /dev/null +++ b/src/mmc35240/mmc35240.cxx @@ -0,0 +1,811 @@ +/* + * Author: Lay, Kuan Loon + * 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. + */ + +#include +#include +#include +#include +#include +#include "mmc35240.hpp" + +#define NUMBER_OF_BITS_IN_BYTE 8 + +/* Compass defines */ +#define CONVERT_GAUSS_TO_MICROTESLA(x) ((x) *100) +#define EPSILON 0.000000001 +#define MAGNETIC_LOW 960 /* 31 micro tesla squared */ +#define CAL_STEPS 5 + +/* Filter defines */ +#define FILTER_MAX_SAMPLE 20 +#define FILTER_NUM_FILED 3 + +using namespace upm; +using namespace android; + +/* We'll have multiple calibration levels so that we can provide an estimation as fast as possible + */ +static const float min_diffs[CAL_STEPS] = { 0.2, 0.25, 0.4, 0.6, 1.0 }; +static const float max_sqr_errs[CAL_STEPS] = { 10.0, 10.0, 8.0, 5.0, 3.5 }; +static const unsigned int lookback_counts[CAL_STEPS] = { 2, 3, 4, 5, 6 }; + +MMC35240::MMC35240(int device) +{ + float mag_scale; + char trigger[64]; + + if (!(m_iio = mraa_iio_init(device))) { + throw std::invalid_argument(std::string(__FUNCTION__) + + ": mraa_iio_init() failed, invalid device?"); + return; + } + m_scale = 1; + m_iio_device_num = device; + sprintf(trigger, "hrtimer-mmc35240-hr-dev%d", device); + + if (mraa_iio_create_trigger(m_iio, trigger) != MRAA_SUCCESS) + fprintf(stderr, "Create trigger %s failed\n", trigger); + + if (mraa_iio_get_mount_matrix(m_iio, "in_mount_matrix", m_mount_matrix) == MRAA_SUCCESS) + m_mount_matrix_exist = true; + else + m_mount_matrix_exist = false; + + if (mraa_iio_read_float(m_iio, "in_magn_scale", &mag_scale) == MRAA_SUCCESS) + m_scale = mag_scale; + + // calibration init data + initCalibrate(); + + // filter init data + memset(&m_filter, 0, sizeof(filter_average_t)); + m_filter.max_samples = FILTER_MAX_SAMPLE; + m_filter.num_fields = FILTER_NUM_FILED; +} + +MMC35240::~MMC35240() +{ + if (m_filter.history) { + free(m_filter.history); + m_filter.history = NULL; + } + if (m_filter.history_sum) { + free(m_filter.history_sum); + m_filter.history_sum = NULL; + } + if (m_iio) + mraa_iio_close(m_iio); +} + +void +MMC35240::installISR(void (*isr)(char*), void* arg) +{ + mraa_iio_trigger_buffer(m_iio, isr, NULL); +} + +int64_t +MMC35240::getChannelValue(unsigned char* input, mraa_iio_channel* chan) +{ + uint64_t u64 = 0; + int i; + int storagebits = chan->bytes * NUMBER_OF_BITS_IN_BYTE; + int realbits = chan->bits_used; + int zeroed_bits = storagebits - realbits; + uint64_t sign_mask; + uint64_t value_mask; + + if (!chan->lendian) + for (i = 0; i < storagebits / NUMBER_OF_BITS_IN_BYTE; i++) + u64 = (u64 << 8) | input[i]; + else + for (i = storagebits / NUMBER_OF_BITS_IN_BYTE - 1; i >= 0; i--) + u64 = (u64 << 8) | input[i]; + + u64 = (u64 >> chan->shift) & (~0ULL >> zeroed_bits); + + if (!chan->signedd) + return (int64_t) u64; /* We don't handle unsigned 64 bits int */ + + /* Signed integer */ + + switch (realbits) { + case 0 ... 1: + return 0; + + case 8: + return (int64_t)(int8_t) u64; + + case 16: + return (int64_t)(int16_t) u64; + + case 32: + return (int64_t)(int32_t) u64; + + case 64: + return (int64_t) u64; + + default: + sign_mask = 1 << (realbits - 1); + value_mask = sign_mask - 1; + + if (u64 & sign_mask) + return -((~u64 & value_mask) + 1); /* Negative value: return 2-complement */ + else + return (int64_t) u64; /* Positive value */ + } +} + +bool +MMC35240::enableBuffer(int length) +{ + mraa_iio_write_int(m_iio, "buffer/length", length); + // enable must be last step, else will have error in writing above config + mraa_iio_write_int(m_iio, "buffer/enable", 1); + return true; +} + +bool +MMC35240::disableBuffer() +{ + mraa_iio_write_int(m_iio, "buffer/enable", 0); + return true; +} + +bool +MMC35240::setScale(float scale) +{ + m_scale = scale; + mraa_iio_write_float(m_iio, "in_magn_scale", scale); + return true; +} + +bool +MMC35240::setSamplingFrequency(float sampling_frequency) +{ + m_sampling_frequency = sampling_frequency; + mraa_iio_write_float(m_iio, "in_magn_sampling_frequency", sampling_frequency); + return true; +} + +bool +MMC35240::enable3AxisChannel() +{ + char trigger[64]; + sprintf(trigger, "mmc35240-hr-dev%d", m_iio_device_num); + + mraa_iio_write_string(m_iio, "trigger/current_trigger", trigger); + mraa_iio_write_int(m_iio, "scan_elements/in_magn_x_en", 1); + mraa_iio_write_int(m_iio, "scan_elements/in_magn_y_en", 1); + mraa_iio_write_int(m_iio, "scan_elements/in_magn_z_en", 1); + + // need update channel data size after enable + mraa_iio_update_channels(m_iio); + return true; +} + +void +MMC35240::extract3Axis(char* data, float* x, float* y, float* z) +{ + mraa_iio_channel* channels = mraa_iio_get_channels(m_iio); + float tmp[3]; + int64_t iio_x, iio_y, iio_z; + + iio_x = getChannelValue((unsigned char*) (data), &channels[0]); + iio_y = getChannelValue((unsigned char*) (data + 4), &channels[1]); + iio_z = getChannelValue((unsigned char*) (data + 8), &channels[2]); + + // Raw data is magnetic field along axis x, y, and z. Units after application of scale are Gauss + *x = CONVERT_GAUSS_TO_MICROTESLA(iio_x * m_scale); + *y = CONVERT_GAUSS_TO_MICROTESLA(iio_y * m_scale); + *z = CONVERT_GAUSS_TO_MICROTESLA(iio_z * m_scale); + + if (m_mount_matrix_exist) { + tmp[0] = *x * m_mount_matrix[0] + *y * m_mount_matrix[1] + *z * m_mount_matrix[2]; + tmp[1] = *x * m_mount_matrix[3] + *y * m_mount_matrix[4] + *z * m_mount_matrix[5]; + tmp[2] = *x * m_mount_matrix[6] + *y * m_mount_matrix[7] + *z * m_mount_matrix[8]; + + *x = tmp[0]; + *y = tmp[1]; + *z = tmp[2]; + } + + calibrateCompass(x, y, z, &m_cal_data); + denoise_average(x, y, z); +} + +int +MMC35240::getCalibratedLevel() +{ + return m_cal_level; +} + +void +MMC35240::initCalibrate() +{ + m_cal_level = 0; + resetSample(&m_cal_data); + + m_cal_data.offset[0][0] = 0; + m_cal_data.offset[1][0] = 0; + m_cal_data.offset[2][0] = 0; + m_cal_data.w_invert[0][0] = 1; + m_cal_data.w_invert[1][0] = 0; + m_cal_data.w_invert[2][0] = 0; + m_cal_data.w_invert[0][1] = 0; + m_cal_data.w_invert[1][1] = 1; + m_cal_data.w_invert[2][1] = 0; + m_cal_data.w_invert[0][2] = 0; + m_cal_data.w_invert[1][2] = 0; + m_cal_data.w_invert[2][2] = 1; + m_cal_data.bfield = 0; +} + +void +MMC35240::getCalibratedData(int* cal_level, double offset[3][1], double w_invert[3][3], double* bfield) +{ + *cal_level = m_cal_level; + offset[0][0] = m_cal_data.offset[0][0]; + offset[1][0] = m_cal_data.offset[1][0]; + offset[2][0] = m_cal_data.offset[2][0]; + w_invert[0][0] = m_cal_data.w_invert[0][0]; + w_invert[1][0] = m_cal_data.w_invert[1][0]; + w_invert[2][0] = m_cal_data.w_invert[2][0]; + w_invert[0][1] = m_cal_data.w_invert[0][1]; + w_invert[1][1] = m_cal_data.w_invert[1][1]; + w_invert[2][1] = m_cal_data.w_invert[2][1]; + w_invert[0][2] = m_cal_data.w_invert[0][2]; + w_invert[1][2] = m_cal_data.w_invert[1][2]; + w_invert[2][2] = m_cal_data.w_invert[2][2]; + *bfield = m_cal_data.bfield; +} + +void +MMC35240::loadCalibratedData(int cal_level, double offset[3][1], double w_invert[3][3], double bfield) +{ + m_cal_level = cal_level; + m_cal_data.offset[0][0] = offset[0][0]; + m_cal_data.offset[1][0] = offset[1][0]; + m_cal_data.offset[2][0] = offset[2][0]; + m_cal_data.w_invert[0][0] = w_invert[0][0]; + m_cal_data.w_invert[1][0] = w_invert[1][0]; + m_cal_data.w_invert[2][0] = w_invert[2][0]; + m_cal_data.w_invert[0][1] = w_invert[0][1]; + m_cal_data.w_invert[1][1] = w_invert[1][1]; + m_cal_data.w_invert[2][1] = w_invert[2][1]; + m_cal_data.w_invert[0][2] = w_invert[0][2]; + m_cal_data.w_invert[1][2] = w_invert[1][2]; + m_cal_data.w_invert[2][2] = w_invert[2][2]; + m_cal_data.bfield = bfield; +} + +void +MMC35240::resetSample(compass_cal_t* data) +{ + int i, j; + data->sample_count = 0; + for (i = 0; i < MAGN_DS_SIZE; i++) + for (j = 0; j < 3; j++) + data->sample[i][j] = 0; + + data->average[0] = data->average[1] = data->average[2] = 0; +} + +void +MMC35240::calibrateCompass(float* x, float* y, float* z, compass_cal_t* cal_data) +{ + int cal_level; + + /* Calibration is continuous */ + compassCollect(x, y, z, cal_data); + + cal_level = compassReady(cal_data); + + switch (cal_level) { + case 0: + scale(x, y, z); + break; + + default: + compassComputeCal(x, y, z, cal_data); + break; + } +} + +int +MMC35240::compassCollect(float* x, float* y, float* z, compass_cal_t* cal_data) +{ + float data[3] = { *x, *y, *z }; + unsigned int index, j; + unsigned int lookback_count; + float min_diff; + + /* Discard the point if not valid */ + if (data[0] == 0 || data[1] == 0 || data[2] == 0) + return -1; + + lookback_count = lookback_counts[m_cal_level]; + min_diff = min_diffs[m_cal_level]; + + /* For the current point to be accepted, each x/y/z value must be different enough to the last + * several collected points */ + if (cal_data->sample_count > 0 && cal_data->sample_count < MAGN_DS_SIZE) { + unsigned int lookback = + lookback_count < cal_data->sample_count ? lookback_count : cal_data->sample_count; + for (index = 0; index < lookback; index++) + for (j = 0; j < 3; j++) + if (fabsf(data[j] - cal_data->sample[cal_data->sample_count - 1 - index][j]) < + min_diff) { +#ifdef DEBUG + printf("CompassCalibration:point reject: [%f,%f,%f], selected_count=%d\n", + data[0], + data[1], + data[2], + cal_data->sample_count); +#endif + return 0; + } + } + + if (cal_data->sample_count < MAGN_DS_SIZE) { + memcpy(cal_data->sample[cal_data->sample_count], data, sizeof(float) * 3); + cal_data->sample_count++; + cal_data->average[0] += data[0]; + cal_data->average[1] += data[1]; + cal_data->average[2] += data[2]; +#ifdef DEBUG + printf("CompassCalibration:point collected [%f,%f,%f], selected_count=%d\n", + (double) data[0], + (double) data[1], + (double) data[2], + cal_data->sample_count); +#endif + } + return 1; +} + +int +MMC35240::compassReady(compass_cal_t* cal_data) +{ + mat_input_t mat; + int i; + float max_sqr_err; + + compass_cal_t new_cal_data; + + /* + * Some sensors take unrealistically long to calibrate at higher levels. We'll use a + * max_cal_level if we have such a property setup, + * or go with the default settings if not. + */ + int cal_steps = CAL_STEPS; + + if (cal_data->sample_count < MAGN_DS_SIZE) + return m_cal_level; + + max_sqr_err = max_sqr_errs[m_cal_level]; + + /* Enough points have been collected, do the ellipsoid calibration */ + + /* Compute average per axis */ + cal_data->average[0] /= MAGN_DS_SIZE; + cal_data->average[1] /= MAGN_DS_SIZE; + cal_data->average[2] /= MAGN_DS_SIZE; + + for (i = 0; i < MAGN_DS_SIZE; i++) { + mat[i][0] = cal_data->sample[i][0]; + mat[i][1] = cal_data->sample[i][1]; + mat[i][2] = cal_data->sample[i][2]; + } + + /* Check if result is good. The sample data must remain the same */ + new_cal_data = *cal_data; + + if (ellipsoidFit(mat, new_cal_data.offset, new_cal_data.w_invert, &new_cal_data.bfield)) { + double new_err = calcSquareErr(&new_cal_data); +#ifdef DEBUG + printf("new err is %f, max sqr err id %f\n", new_err, max_sqr_err); +#endif + if (new_err < max_sqr_err) { + double err = calcSquareErr(cal_data); + if (new_err < err) { + /* New cal data is better, so we switch to the new */ + cal_data->offset = new_cal_data.offset; + cal_data->w_invert = new_cal_data.w_invert; + cal_data->bfield = new_cal_data.bfield; + if (m_cal_level < (cal_steps - 1)) + m_cal_level++; +#ifdef DEBUG + printf("CompassCalibration: ready check success, caldata: %f %f %f %f %f %f %f %f " + "%f %f %f %f %f, err %f\n", + cal_data->offset[0][0], + cal_data->offset[1][0], + cal_data->offset[2][0], + cal_data->w_invert[0][0], + cal_data->w_invert[0][1], + cal_data->w_invert[0][2], + cal_data->w_invert[1][0], + cal_data->w_invert[1][1], + cal_data->w_invert[1][2], + cal_data->w_invert[2][0], + cal_data->w_invert[2][1], + cal_data->w_invert[2][2], + cal_data->bfield, + new_err); +#endif + } + } + } + resetSample(cal_data); + return m_cal_level; +} + +double +MMC35240::calcSquareErr(compass_cal_t* data) +{ + double err = 0; + mat raw, result, mat_diff; + int i; + float stdev[3] = { 0, 0, 0 }; + double diff; + + for (i = 0; i < MAGN_DS_SIZE; i++) { + raw[0][0] = data->sample[i][0]; + raw[1][0] = data->sample[i][1]; + raw[2][0] = data->sample[i][2]; + + + stdev[0] += (raw[0][0] - data->average[0]) * (raw[0][0] - data->average[0]); + stdev[1] += (raw[1][0] - data->average[1]) * (raw[1][0] - data->average[1]); + stdev[2] += (raw[2][0] - data->average[2]) * (raw[2][0] - data->average[2]); + + mat_diff = raw - data->offset; + result = multiply(data->w_invert, mat_diff); + + diff = sqrt(result[0][0] * result[0][0] + result[1][0] * result[1][0] + + result[2][0] * result[2][0]) - + data->bfield; + + err += diff * diff; + } + + stdev[0] = sqrt(stdev[0] / MAGN_DS_SIZE); + stdev[1] = sqrt(stdev[1] / MAGN_DS_SIZE); + stdev[2] = sqrt(stdev[2] / MAGN_DS_SIZE); + + /* A sanity check - if we have too little variation for an axis it's best to reject the + * calibration than risking a wrong calibration */ + if (stdev[0] <= 1 || stdev[1] <= 1 || stdev[2] <= 1) + return max_sqr_errs[0]; + + err /= MAGN_DS_SIZE; + return err; +} + +int +MMC35240::ellipsoidFit(mat_input_t& m, + mat& offset, + mat& w_invert, + double* bfield) +{ + int i; + mat h; + mat w; + mat h_trans; + mat p_temp1; + mat p_temp2; + mat temp1, temp; + mat temp1_inv; + mat temp2; + mat result; + mat p; + mat a, sqrt_evals, evecs, evecs_trans; + mat evec1, evec2, evec3; + + for (i = 0; i < MAGN_DS_SIZE; i++) { + w[i][0] = m[i][0] * m[i][0]; + h[i][0] = m[i][0]; + h[i][1] = m[i][1]; + h[i][2] = m[i][2]; + h[i][3] = -1 * m[i][0] * m[i][1]; + h[i][4] = -1 * m[i][0] * m[i][2]; + h[i][5] = -1 * m[i][1] * m[i][2]; + h[i][6] = -1 * m[i][1] * m[i][1]; + h[i][7] = -1 * m[i][2] * m[i][2]; + h[i][8] = 1; + } + + h_trans = transpose(h); + result = multiply(h_trans, h); + p_temp1 = invert(result); + p_temp2 = multiply(p_temp1, h_trans); + p = multiply(p_temp2, w); + + temp1[0][0] = 2; + temp1[0][1] = p[3][0]; + temp1[0][2] = p[4][0]; + temp1[1][0] = p[3][0]; + temp1[1][1] = 2 * p[6][0]; + temp1[1][2] = p[5][0]; + temp1[2][0] = p[4][0]; + temp1[2][1] = p[5][0]; + temp1[2][2] = 2 * p[7][0]; + + temp2[0][0] = p[0][0]; + temp2[1][0] = p[1][0]; + temp2[2][0] = p[2][0]; + + temp1_inv = invert(temp1); + offset = multiply(temp1_inv, temp2); + + double off_x = offset[0][0]; + double off_y = offset[1][0]; + double off_z = offset[2][0]; + + a[0][0] = 1.0 / (p[8][0] + off_x * off_x + p[6][0] * off_y * off_y + p[7][0] * off_z * off_z + + p[3][0] * off_x * off_y + p[4][0] * off_x * off_z + p[5][0] * off_y * off_z); + + a[0][1] = p[3][0] * a[0][0] / 2; + a[0][2] = p[4][0] * a[0][0] / 2; + a[1][2] = p[5][0] * a[0][0] / 2; + a[1][1] = p[6][0] * a[0][0]; + a[2][2] = p[7][0] * a[0][0]; + a[2][1] = a[1][2]; + a[1][0] = a[0][1]; + a[2][0] = a[0][2]; + + double eig1 = 0, eig2 = 0, eig3 = 0; + computeEigenvalues(a, &eig1, &eig2, &eig3); + + if (eig1 <= 0 || eig2 <= 0 || eig3 <= 0) + return 0; + + sqrt_evals[0][0] = sqrt(eig1); + sqrt_evals[1][0] = 0; + sqrt_evals[2][0] = 0; + sqrt_evals[0][1] = 0; + sqrt_evals[1][1] = sqrt(eig2); + sqrt_evals[2][1] = 0; + sqrt_evals[0][2] = 0; + sqrt_evals[1][2] = 0; + sqrt_evals[2][2] = sqrt(eig3); + + calcEvector(a, eig1, evec1); + calcEvector(a, eig2, evec2); + calcEvector(a, eig3, evec3); + + evecs[0][0] = evec1[0][0]; + evecs[1][0] = evec1[1][0]; + evecs[2][0] = evec1[2][0]; + evecs[0][1] = evec2[0][0]; + evecs[1][1] = evec2[1][0]; + evecs[2][1] = evec2[2][0]; + evecs[0][2] = evec3[0][0]; + evecs[1][2] = evec3[1][0]; + evecs[2][2] = evec3[2][0]; + + temp1 = multiply(evecs, sqrt_evals); + evecs_trans = transpose(evecs); + temp = multiply(temp1, evecs_trans); + w_invert = transpose(temp); + + *bfield = pow(sqrt(1 / eig1) * sqrt(1 / eig2) * sqrt(1 / eig3), 1.0 / 3.0); + if (*bfield < 0) + return 0; + + w_invert = w_invert * (*bfield); + + return 1; +} + +/* Given an real symmetric 3x3 matrix A, compute the eigenvalues */ +void +MMC35240::computeEigenvalues(mat& A, double* eig1, double* eig2, double* eig3) +{ + double p = A[0][1] * A[0][1] + A[0][2] * A[0][2] + A[1][2] * A[1][2]; + + if (p < EPSILON) { + *eig1 = A[0][0]; + *eig2 = A[1][1]; + *eig3 = A[2][2]; + return; + } + + double q = (A[0][0] + A[1][1] + A[2][2]) / 3; + double temp1 = A[0][0] - q; + double temp2 = A[1][1] - q; + double temp3 = A[2][2] - q; + + p = temp1 * temp1 + temp2 * temp2 + temp3 * temp3 + 2 * p; + p = sqrt(p / 6); + + mat B = A; + B[0][0] -= q; + B[1][1] -= q; + B[2][2] -= q; + B = (1 / p) * B; + + double r = + (B[0][0] * B[1][1] * B[2][2] + B[0][1] * B[1][2] * B[2][0] + B[0][2] * B[1][0] * B[2][1] - + B[0][2] * B[1][1] * B[2][0] - B[0][0] * B[1][2] * B[2][1] - B[0][1] * B[1][0] * B[2][2]) / + 2; + + double phi; + if (r <= -1.0) + phi = M_PI / 3; + else if (r >= 1.0) + phi = 0; + else + phi = acos(r) / 3; + + *eig3 = q + 2 * p* cos(phi); + *eig1 = q + 2 * p* cos(phi + 2 * M_PI / 3); + *eig2 = 3 * q - *eig1 - *eig3; +} + +void +MMC35240::calcEvector(mat& A, double eig, mat& vec) +{ + mat h; + mat x_tmp; + + h = A; + h[0][0] -= eig; + h[1][1] -= eig; + h[2][2] -= eig; + + mat x; + x[0][0] = h[1][1]; + x[0][1] = h[1][2]; + x[1][0] = h[2][1]; + x[1][1] = h[2][2]; + + x_tmp = invert(x); + x = x_tmp; + + double temp1 = x[0][0] * (-h[1][0]) + x[0][1] * (-h[2][0]); + double temp2 = x[1][0] * (-h[1][0]) + x[1][1] * (-h[2][0]); + double norm = sqrt(1 + temp1 * temp1 + temp2 * temp2); + + vec[0][0] = 1.0 / norm; + vec[1][0] = temp1 / norm; + vec[2][0] = temp2 / norm; +} + +void +MMC35240::scale(float* x, float* y, float* z) +{ + float sqr_norm = 0; + float sanity_norm = 0; + float scale = 1; + + sqr_norm = (*x * *x + *y * *y + *z * *z); + + if (sqr_norm < MAGNETIC_LOW) + sanity_norm = MAGNETIC_LOW; + + if (sanity_norm && sqr_norm) { + scale = sanity_norm / sqr_norm; + scale = sqrt(scale); + *x = *x* scale; + *y = *y* scale; + *z = *z* scale; + } +} + +void +MMC35240::compassComputeCal(float* x, float* y, float* z, compass_cal_t* cal_data) +{ + mat result, raw, diff; + + if (!m_cal_level) + return; + + raw[0][0] = *x; + raw[1][0] = *y; + raw[2][0] = *z; + + diff = raw - cal_data->offset; + result = multiply(cal_data->w_invert, diff); + + *x = result[0][0]; + *y = result[1][0]; + *z = result[2][0]; + + scale(x, y, z); +} + +void +MMC35240::denoise_average(float* x, float* y, float* z) +{ + /* + * Smooth out incoming data using a moving average over a number of + * samples. We accumulate one second worth of samples, or max_samples, + * depending on which is lower. + */ + float* data[3]; + int f; + int history_size; + int history_full = 0; + filter_average_t* filter; + + data[0] = x; + data[1] = y; + data[2] = z; + + /* Don't denoise anything if we have less than two samples per second */ + if (m_sampling_frequency < 2) + return; + + filter = (filter_average_t*) &m_filter; + + if (!filter) + return; + + /* Restrict window size to the min of sampling_rate and max_samples */ + if (m_sampling_frequency > filter->max_samples) + history_size = filter->max_samples; + else + history_size = m_sampling_frequency; + + /* Reset history if we're operating on an incorrect window size */ + if (filter->history_size != history_size) { + filter->history_size = history_size; + filter->history_entries = 0; + filter->history_index = 0; + filter->history = + (float*) realloc(filter->history, filter->history_size * filter->num_fields * sizeof(float)); + if (filter->history) { + filter->history_sum = + (float*) realloc(filter->history_sum, filter->num_fields * sizeof(float)); + if (filter->history_sum) + memset(filter->history_sum, 0, filter->num_fields * sizeof(float)); + } + } + + if (!filter->history || !filter->history_sum) + return; /* Unlikely, but still... */ + + /* Update initialized samples count */ + if (filter->history_entries < filter->history_size) + filter->history_entries++; + else + history_full = 1; + + /* Record new sample and calculate the moving sum */ + for (f = 0; f < filter->num_fields; f++) { + /** A field is going to be overwritten if history is full, so decrease the history sum */ + if (history_full) + filter->history_sum[f] -= + filter->history[filter->history_index * filter->num_fields + f]; + + filter->history[filter->history_index * filter->num_fields + f] = *data[f]; + filter->history_sum[f] += *data[f]; + + /* For now simply compute a mobile mean for each field and output filtered data */ + *data[f] = filter->history_sum[f] / filter->history_entries; + } + + /* Update our rolling index (next evicted cell) */ + filter->history_index = (filter->history_index + 1) % filter->history_size; +} diff --git a/src/mmc35240/mmc35240.hpp b/src/mmc35240/mmc35240.hpp new file mode 100644 index 00000000..0a892d7e --- /dev/null +++ b/src/mmc35240/mmc35240.hpp @@ -0,0 +1,213 @@ +/* + * Author: Lay, Kuan Loon + * 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 +#include + +// 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 +{ +/** + * @brief MMC35240 Tri-axis Magnetic Sensor + * @defgroup mmc35240 libupm-mmc35240 + * @ingroup STMicroelectronics iio i2c tri-axis magnetic sensor + */ + +/** + * @library mmc35240 + * @sensor mmc35240 + * @comname MMC35240 Tri-axis Magnetic Sensor + * @type compass + * @man Memsic + * @con iio i2c + * + * @brief MMC35240 Tri-axis Magnetic Sensor API + * + * The MMC3524xPJ is a complete 3-axis magnetic sensor + * + * @snippet mmc35240.cxx Interesting + */ + +class MMC35240 +{ + public: + typedef struct { + /* hard iron offsets */ + android::mat offset; + + /* soft iron matrix */ + android::mat 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 Tri-axis Magnetic Sensor + * + * @param iio device number + */ + MMC35240(int device); + + /** + * MMC35240 destructor + */ + ~MMC35240(); + + /** + * Installs an interrupt service routine (ISR) to be called when + * an interrupt occurs + * + * @param interrupt channel + * @param fptr Pointer to a function to be called on interrupt + * @param arg Pointer to an object to be supplied as an + * argument to the ISR. + */ + void installISR(void (*isr)(char*), void* arg); + + /** + * Extract the channel value based on channel type + * @param input Channel data + * @param chan MRAA iio-layer channel info + */ + int64_t getChannelValue(unsigned char* input, mraa_iio_channel* chan); + + /** + * Enable trigger buffer + * @param trigger buffer length in integer + */ + bool enableBuffer(int length); + + /** + * Disable trigger buffer + */ + bool disableBuffer(); + + /** + * Set scale + * @param scale in float + * Kernel driver does not support changing the value of scale on run-time + * Default scale is 0.001000 + */ + bool setScale(const float scale); + + /** + * Set sampling frequency + * @param sampling frequency in float + * Available sampling frequency are 1.5, 13, 25, 50 + * Default sampling frequency is 1.500000 + */ + bool setSamplingFrequency(const float sampling_frequency); + + /** + * Enable 3 axis scan element + */ + bool enable3AxisChannel(); + + /** + * Process enabled channel buffer and return x, y, z axis + * @param data Enabled channel data, 6 bytes, each axis 2 bytes + * @param x X-Axis + * @param y Y-Axis + * @param z Z-Axis + */ + void extract3Axis(char* data, float* x, float* y, float* z); + + /** + * Get calibrated level + */ + int getCalibratedLevel(); + + /** + * Reset calibration data and start collect calibration data again + */ + void initCalibrate(); + + /** + * Get calibrated data + */ + void + getCalibratedData(int* cal_level, double offset[3][1], double w_invert[3][3], double* bfield); + + /** + * Load calibrated data + */ + 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& offset, + android::mat& w_invert, + double* bfield); + void computeEigenvalues(android::mat& A, double* eig1, double* eig2, double* eig3); + void calcEvector(android::mat& A, double eig, android::mat& 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 +}; +} diff --git a/src/mmc35240/pyupm_mmc35240.i b/src/mmc35240/pyupm_mmc35240.i new file mode 100644 index 00000000..3995e825 --- /dev/null +++ b/src/mmc35240/pyupm_mmc35240.i @@ -0,0 +1,12 @@ +%module pyupm_mmc35240 +// Include doxygen-generated documentation +%include "pyupm_doxy2swig.i" + +%include "../upm.i" + +%feature("autodoc", "3"); + +%include "mmc35240.hpp" +%{ + #include "mmc35240.hpp" +%} diff --git a/src/mmc35240/traits.h b/src/mmc35240/traits.h new file mode 100644 index 00000000..da4c599a --- /dev/null +++ b/src/mmc35240/traits.h @@ -0,0 +1,118 @@ +/* + * Copyright (C) 2011 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ANDROID_TRAITS_H +#define ANDROID_TRAITS_H + +// ----------------------------------------------------------------------- +// Typelists + +namespace android { + +// end-of-list marker +class NullType {}; + +// type-list node +template +struct TypeList { + typedef T Head; + typedef U Tail; +}; + +// helpers to build typelists +#define TYPELIST_1(T1) TypeList +#define TYPELIST_2(T1, T2) TypeList +#define TYPELIST_3(T1, T2, T3) TypeList +#define TYPELIST_4(T1, T2, T3, T4) TypeList + +// typelists algorithms +namespace TL { +template struct IndexOf; + +template +struct IndexOf { + enum { value = -1 }; +}; + +template +struct IndexOf, T> { + enum { value = 0 }; +}; + +template +struct IndexOf, T> { +private: + enum { temp = IndexOf::value }; +public: + enum { value = temp == -1 ? -1 : 1 + temp }; +}; + +}; // namespace TL + +// type selection based on a boolean +template +struct Select { + typedef T Result; +}; +template +struct Select { + typedef U Result; +}; + +// ----------------------------------------------------------------------- +// Type traits + +template +class TypeTraits { + typedef TYPELIST_4( + unsigned char, unsigned short, + unsigned int, unsigned long int) UnsignedInts; + + typedef TYPELIST_4( + signed char, signed short, + signed int, signed long int) SignedInts; + + typedef TYPELIST_1( + bool) OtherInts; + + typedef TYPELIST_3( + float, double, long double) Floats; + + template struct PointerTraits { + enum { result = false }; + typedef NullType PointeeType; + }; + template struct PointerTraits { + enum { result = true }; + typedef U PointeeType; + }; + +public: + enum { isStdUnsignedInt = TL::IndexOf::value >= 0 }; + enum { isStdSignedInt = TL::IndexOf::value >= 0 }; + enum { isStdIntegral = TL::IndexOf::value >= 0 || isStdUnsignedInt || isStdSignedInt }; + enum { isStdFloat = TL::IndexOf::value >= 0 }; + enum { isPointer = PointerTraits::result }; + enum { isStdArith = isStdIntegral || isStdFloat }; + + // best parameter type for given type + typedef typename Select::Result ParameterType; +}; + +// ----------------------------------------------------------------------- +}; // namespace android + +#endif /* ANDROID_TRAITS_H */ diff --git a/src/mmc35240/vec.h b/src/mmc35240/vec.h new file mode 100644 index 00000000..a142bad3 --- /dev/null +++ b/src/mmc35240/vec.h @@ -0,0 +1,438 @@ +/* + * Copyright (C) 2011 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ANDROID_VEC_H +#define ANDROID_VEC_H + +#include + +#include +#include + +#include "traits.h" + +// ----------------------------------------------------------------------- + +#define PURE __attribute__((pure)) + +namespace android { + +// ----------------------------------------------------------------------- +// non-inline helpers + +template +class vec; + +template +struct vbase; + +namespace helpers { + +template inline T min(T a, T b) { return a inline T max(T a, T b) { return a>b ? a : b; } + +template < template class VEC, + typename TYPE, size_t SIZE, size_t S> +vec& doAssign( + vec& lhs, const VEC& rhs) { + const size_t minSize = min(SIZE, S); + const size_t maxSize = max(SIZE, S); + for (size_t i=0 ; i class VLHS, + template class VRHS, + typename TYPE, + size_t SIZE +> +VLHS PURE doAdd( + const VLHS& lhs, + const VRHS& rhs) { + VLHS r; + for (size_t i=0 ; i class VLHS, + template class VRHS, + typename TYPE, + size_t SIZE +> +VLHS PURE doSub( + const VLHS& lhs, + const VRHS& rhs) { + VLHS r; + for (size_t i=0 ; i class VEC, + typename TYPE, + size_t SIZE +> +VEC PURE doMulScalar( + const VEC& lhs, + typename TypeTraits::ParameterType rhs) { + VEC r; + for (size_t i=0 ; i class VEC, + typename TYPE, + size_t SIZE +> +VEC PURE doScalarMul( + typename TypeTraits::ParameterType lhs, + const VEC& rhs) { + VEC r; + for (size_t i=0 ; i. Without this, an extra conversion to vec<> would be needed. +// +// example: +// vec4_t a; +// vec3_t b; +// vec3_t c = a.xyz + b; +// +// "a.xyz + b" is a mixed-operation between a vbase<> and a vec<>, requiring +// a conversion of vbase<> to vec<>. The template gunk below avoids this, +// by allowing the addition on these different vector types directly +// + +template < + template class VLHS, + template class VRHS, + typename TYPE, + size_t SIZE +> +inline VLHS PURE operator + ( + const VLHS& lhs, + const VRHS& rhs) { + return helpers::doAdd(lhs, rhs); +} + +template < + template class VLHS, + template class VRHS, + typename TYPE, + size_t SIZE +> +inline VLHS PURE operator - ( + const VLHS& lhs, + const VRHS& rhs) { + return helpers::doSub(lhs, rhs); +} + +template < + template class VEC, + typename TYPE, + size_t SIZE +> +inline VEC PURE operator * ( + const VEC& lhs, + typename TypeTraits::ParameterType rhs) { + return helpers::doMulScalar(lhs, rhs); +} + +template < + template class VEC, + typename TYPE, + size_t SIZE +> +inline VEC PURE operator * ( + typename TypeTraits::ParameterType lhs, + const VEC& rhs) { + return helpers::doScalarMul(lhs, rhs); +} + + +template < + template class VLHS, + template class VRHS, + typename TYPE, + size_t SIZE +> +TYPE PURE dot_product( + const VLHS& lhs, + const VRHS& rhs) { + TYPE r(0); + for (size_t i=0 ; i class V, + typename TYPE, + size_t SIZE +> +TYPE PURE length(const V& v) { + return sqrt(dot_product(v, v)); +} + +template < + template class V, + typename TYPE, + size_t SIZE +> +TYPE PURE length_squared(const V& v) { + return dot_product(v, v); +} + +template < + template class V, + typename TYPE, + size_t SIZE +> +V PURE normalize(const V& v) { + return v * (1/length(v)); +} + +template < + template class VLHS, + template class VRHS, + typename TYPE +> +VLHS PURE cross_product( + const VLHS& u, + const VRHS& v) { + VLHS r; + r.x = u.y*v.z - u.z*v.y; + r.y = u.z*v.x - u.x*v.z; + r.z = u.x*v.y - u.y*v.x; + return r; +} + + +template +vec PURE operator - (const vec& lhs) { + vec r; + for (size_t i=0 ; i +struct vbase { + TYPE v[SIZE]; + inline const TYPE& operator[](size_t i) const { return v[i]; } + inline TYPE& operator[](size_t i) { return v[i]; } +}; +template<> struct vbase { + union { + float v[2]; + struct { float x, y; }; + struct { float s, t; }; + }; + inline const float& operator[](size_t i) const { return v[i]; } + inline float& operator[](size_t i) { return v[i]; } +}; +template<> struct vbase { + union { + float v[3]; + struct { float x, y, z; }; + struct { float s, t, r; }; + vbase xy; + vbase st; + }; + inline const float& operator[](size_t i) const { return v[i]; } + inline float& operator[](size_t i) { return v[i]; } +}; +template<> struct vbase { + union { + float v[4]; + struct { float x, y, z, w; }; + struct { float s, t, r, q; }; + vbase xyz; + vbase str; + vbase xy; + vbase st; + }; + inline const float& operator[](size_t i) const { return v[i]; } + inline float& operator[](size_t i) { return v[i]; } +}; + +// ----------------------------------------------------------------------- + +template +class vec : public vbase +{ + typedef typename TypeTraits::ParameterType pTYPE; + typedef vbase base; + +public: + // STL-like interface. + typedef TYPE value_type; + typedef TYPE& reference; + typedef TYPE const& const_reference; + typedef size_t size_type; + + typedef TYPE* iterator; + typedef TYPE const* const_iterator; + iterator begin() { return base::v; } + iterator end() { return base::v + SIZE; } + const_iterator begin() const { return base::v; } + const_iterator end() const { return base::v + SIZE; } + size_type size() const { return SIZE; } + + // ----------------------------------------------------------------------- + // default constructors + + vec() { } + vec(const vec& rhs) : base(rhs) { } + vec(const base& rhs) : base(rhs) { } + + // ----------------------------------------------------------------------- + // conversion constructors + + vec(pTYPE rhs) { + for (size_t i=0 ; i class VEC, size_t S> + explicit vec(const VEC& rhs) { + helpers::doAssign(*this, rhs); + } + + explicit vec(TYPE const* array) { + for (size_t i=0 ; i class VEC, size_t S> + vec& operator = (const VEC& rhs) { + return helpers::doAssign(*this, rhs); + } + + // ----------------------------------------------------------------------- + // operation-assignment + + vec& operator += (const vec& rhs); + vec& operator -= (const vec& rhs); + vec& operator *= (pTYPE rhs); + + // ----------------------------------------------------------------------- + // non-member function declaration and definition + // NOTE: we declare the non-member function as friend inside the class + // so that they are known to the compiler when the class is instantiated. + // This helps the compiler doing template argument deduction when the + // passed types are not identical. Essentially this helps with + // type conversion so that you can multiply a vec by an scalar int + // (for instance). + + friend inline vec PURE operator + (const vec& lhs, const vec& rhs) { + return helpers::doAdd(lhs, rhs); + } + friend inline vec PURE operator - (const vec& lhs, const vec& rhs) { + return helpers::doSub(lhs, rhs); + } + friend inline vec PURE operator * (const vec& lhs, pTYPE v) { + return helpers::doMulScalar(lhs, v); + } + friend inline vec PURE operator * (pTYPE v, const vec& rhs) { + return helpers::doScalarMul(v, rhs); + } + friend inline TYPE PURE dot_product(const vec& lhs, const vec& rhs) { + return android::dot_product(lhs, rhs); + } +}; + +// ----------------------------------------------------------------------- + +template +vec& vec::operator += (const vec& rhs) { + vec& lhs(*this); + for (size_t i=0 ; i +vec& vec::operator -= (const vec& rhs) { + vec& lhs(*this); + for (size_t i=0 ; i +vec& vec::operator *= (vec::pTYPE rhs) { + vec& lhs(*this); + for (size_t i=0 ; i vec2_t; +typedef vec vec3_t; +typedef vec vec4_t; + +// ----------------------------------------------------------------------- + +}; // namespace android + +#endif /* ANDROID_VEC_H */