Diff: FXAS21002CQ.cpp

Revision:

0:8aa01dbab64e

Child:

1:816ba6bfbb20

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FXAS21002CQ.cpp	Wed Jul 22 13:23:44 2015 +0000
@@ -0,0 +1,175 @@
+/* FXAS21002CQ sensor driver
+ * Copyright (c) 2015 WD
+ *
+ *
+ * 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.
+ */
+
+#include "FXAS21002CQ.h"
+
+static int16_t dummy_int16_t = 0;
+static float dummy_float = 0.0f;
+
+
+FXAS21002CQ::FXAS21002CQ(I2C &i2c, uint8_t addr)
+{
+    _i2c = &i2c;
+    _addr = addr;
+    /*
+    // activate the peripheral
+    uint8_t data[2] = {FXOS8700Q_CTRL_REG1, 0x00};
+    _i2c->frequency(400000);
+    writeRegs(data, 2);
+    data[0] = FXOS8700Q_M_CTRL_REG1;
+    data[1] = 0x1F;
+    writeRegs(data, 2);
+    data[0] = FXOS8700Q_M_CTRL_REG2;
+    data[1] = 0x20;
+    writeRegs(data, 2);
+    data[0] = FXOS8700Q_XYZ_DATA_CFG;
+    data[1] = 0x00;
+    writeRegs(data, 2);
+    data[0] = FXOS8700Q_CTRL_REG1;
+    data[1] = 0x1C;
+    writeRegs(data, 2);
+    */
+}
+
+FXAS21002CQ::~FXAS21002CQ()
+{
+    _i2c = 0;
+    _addr = 0;
+}
+
+void FXAS21002CQ::readRegs(uint8_t addr, uint8_t *data, uint32_t len) 
+{
+    uint8_t t[1] = {addr};
+    _i2c->write(_addr, (char *)t, sizeof(t), true);
+    _i2c->read(_addr, (char *)data, len);
+}
+
+uint8_t FXAS21002CQ::whoAmI() 
+{
+    uint8_t who_am_i = 0;
+    readRegs(FXAS21002CQ_WHO_AM_I, &who_am_i, sizeof(who_am_i));
+    return who_am_i;
+}
+
+void FXAS21002CQ::writeRegs(uint8_t * data, uint32_t len) const
+{
+    _i2c->write(_addr, (char *)data, len);
+}
+
+int16_t FXAS21002CQ::getSensorAxis(uint8_t addr) const
+{
+    uint8_t res[2];
+//    readRegs(addr, res, sizeof(res));
+    return static_cast<int16_t>((res[0] << 8) | res[1]);
+}
+
+void FXAS21002CQ::enable(void) const
+{
+    uint8_t data[2];
+//    readRegs(FXOS8700Q_CTRL_REG1, &data[1], 1);
+//    data[1] |= 0x01;
+//    data[0] = FXOS8700Q_CTRL_REG1;
+//    writeRegs(data, sizeof(data));
+}
+
+void FXAS21002CQ::disable(void) const
+{
+    uint8_t data[2];
+//    readRegs(FXOS8700Q_CTRL_REG1, &data[1], 1);
+//    data[1] &= 0xFE;
+//    data[0] = FXOS8700Q_CTRL_REG1;
+//    writeRegs(data, sizeof(data));
+}
+
+uint32_t FXAS21002CQ::dataReady(void) const
+{
+//    uint8_t stat = 0;
+//    readRegs(FXOS8700Q_STATUS, &stat, 1);
+    return (uint32_t)1;
+}
+
+uint32_t FXAS21002CQ::sampleRate(uint32_t frequency) const
+{
+    return(50); // for now sample rate is fixed at 50Hz
+}
+
+int16_t FXAS21002CQ::getX(int16_t &x = dummy_int16_t) const
+{
+ //   x = getSensorAxis(FXOS8700Q_OUT_X_MSB) >> 2;
+ //   normalize_14bits(x);
+    return x;
+}
+
+int16_t FXAS21002CQ::getY(int16_t &y = dummy_int16_t) const
+{
+//    y = getSensorAxis(FXOS8700Q_OUT_Y_MSB) >> 2;
+//    normalize_14bits(y);
+    return y;
+}
+
+int16_t FXAS21002CQ::getZ(int16_t &z = dummy_int16_t) const
+{
+//    z = getSensorAxis(FXOS8700Q_OUT_Z_MSB) >> 2;
+//    normalize_14bits(z);
+    return z;
+}
+
+float FXAS21002CQ::getX(float &x = dummy_float) const
+{
+//    int16_t val = getSensorAxis(FXOS8700Q_OUT_X_MSB) >> 2;
+//    normalize_14bits(val);
+//    x = val / 4096.0f;
+    return x;
+}
+
+float FXAS21002CQ::getY(float &y = dummy_float) const
+{
+//    int16_t val = getSensorAxis(FXOS8700Q_OUT_Y_MSB) >> 2;
+//    normalize_14bits(val);
+//    y = val / 4096.0f;
+    return y;
+}
+
+float FXAS21002CQ::getZ(float &z = dummy_float) const
+{
+//    int16_t val = getSensorAxis(FXOS8700Q_OUT_Z_MSB) >> 2;
+//    normalize_14bits(val);
+//    z = val / 4096.0f;
+    return z;
+}
+
+void FXAS21002CQ::getAxis(motion_data_counts_t &xyz) const
+{
+    uint8_t res[6];
+//    readRegs(FXOS8700Q_OUT_X_MSB, res, sizeof(res));
+//    xyz.x = static_cast<int16_t>((res[0] << 8) | res[1]) >> 2;
+//    xyz.y = static_cast<int16_t>((res[2] << 8) | res[3]) >> 2;
+//    xyz.z = static_cast<int16_t>((res[4] << 8) | res[5]) >> 2;
+//    normalize_14bits(xyz.x);
+//    normalize_14bits(xyz.y);
+//    normalize_14bits(xyz.z);
+}
+
+void FXAS21002CQ::getAxis(motion_data_units_t &xyz) const
+{
+//    motion_data_counts_t _xyz;
+//    FXOS8700QAccelerometer::getAxis(_xyz);
+//    xyz.x = _xyz.x / 4096.0f;
+//    xyz.y = _xyz.y / 4096.0f;
+//    xyz.z = _xyz.z / 4096.0f;
+}
+
+int8_t FXAS21002CQ::getTemperature(void)
+{
+    uint8_t res;
+    readRegs(FXAS21002CQ_TEMP, &res, sizeof(res));
+    return (int8_t)res;
+}