Bruno Manganelli
Port of Adafruit Arduino code
Revision 0:772bf4786416, committed 2015-03-21
- Comitter:
- bmanga95
- Date:
- Sat Mar 21 12:33:05 2015 +0000
- Commit message:
- First version
Changed in this revision
diff -r 000000000000 -r 772bf4786416 Header/Adafruit_9DOF.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Header/Adafruit_9DOF.h Sat Mar 21 12:33:05 2015 +0000
@@ -0,0 +1,47 @@
+/***************************************************************************
+ This is a library for the Adafruit 9DOF Breakout
+
+ Designed specifically to work with the Adafruit 9DOF Breakout:
+ http://www.adafruit.com/products/1714
+
+ These displays use I2C to communicate, 2 pins are required to interface.
+
+ Adafruit invests time and resources providing this open source code,
+ please support Adafruit andopen-source hardware by purchasing products
+ from Adafruit!
+
+ Written by Kevin Townsend for Adafruit Industries.
+ BSD license, all text above must be included in any redistribution
+ ***************************************************************************/
+#ifndef __ADAFRUIT_9DOF_H__
+#define __ADAFRUIT_9DOF_H__
+
+#include <Adafruit_Sensor.h>
+#include <Adafruit_LSM303_U.h>
+#include <Adafruit_L3GD20_U.h>
+
+
+/** Sensor axis */
+typedef enum
+{
+ SENSOR_AXIS_X = (1),
+ SENSOR_AXIS_Y = (2),
+ SENSOR_AXIS_Z = (3)
+} sensors_axis_t;
+
+/* Driver for the the 9DOF breakout sensors */
+class Adafruit_9DOF
+{
+ public:
+ Adafruit_9DOF(void);
+ bool begin(void);
+
+ bool accelGetOrientation ( sensors_event_t *event, sensors_vec_t *orientation );
+ bool magTiltCompensation ( sensors_axis_t axis, sensors_event_t *mag_event, sensors_event_t *accel_event );
+ bool magGetOrientation ( sensors_axis_t axis, sensors_event_t *event, sensors_vec_t *mag_orientation );
+ bool fusionGetOrientation ( sensors_event_t *accel_event, sensors_event_t *mag_event, sensors_vec_t *orientation );
+
+ private:
+};
+
+#endif
diff -r 000000000000 -r 772bf4786416 Header/Adafruit_L3GD20_U.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Header/Adafruit_L3GD20_U.h Sat Mar 21 12:33:05 2015 +0000
@@ -0,0 +1,99 @@
+/***************************************************
+ This is a library for the L3GD20 GYROSCOPE
+
+ Designed specifically to work with the Adafruit L3GD20 Breakout
+ ----> https://www.adafruit.com/products/1032
+
+ These sensors use I2C or SPI to communicate, 2 pins (I2C)
+ or 4 pins (SPI) are required to interface.
+
+ Adafruit invests time and resources providing this open source code,
+ please support Adafruit and open-source hardware by purchasing
+ products from Adafruit!
+
+ Written by Kevin "KTOWN" Townsend for Adafruit Industries.
+ BSD license, all text above must be included in any redistribution
+ ****************************************************/
+#ifndef __L3GD20_H__
+#define __L3GD20_H__
+
+
+#include <Adafruit_Sensor.h>
+#include "I2C_base.h"
+
+/*=========================================================================
+ I2C ADDRESS/BITS AND SETTINGS
+ -----------------------------------------------------------------------*/
+ #define L3GD20_ADDRESS (0x6B) // 1101011
+ #define L3GD20_POLL_TIMEOUT (100) // Maximum number of read attempts
+ #define L3GD20_ID 0xD4
+ #define L3GD20H_ID 0xD7
+ #define GYRO_SENSITIVITY_250DPS (0.00875F) // Roughly 22/256 for fixed point match
+ #define GYRO_SENSITIVITY_500DPS (0.0175F) // Roughly 45/256
+ #define GYRO_SENSITIVITY_2000DPS (0.070F) // Roughly 18/256
+/*=========================================================================*/
+
+/*=========================================================================
+ REGISTERS
+ -----------------------------------------------------------------------*/
+ typedef enum
+ { // DEFAULT TYPE
+ GYRO_REGISTER_WHO_AM_I = 0x0F, // 11010100 r
+ GYRO_REGISTER_CTRL_REG1 = 0x20, // 00000111 rw
+ GYRO_REGISTER_CTRL_REG2 = 0x21, // 00000000 rw
+ GYRO_REGISTER_CTRL_REG3 = 0x22, // 00000000 rw
+ GYRO_REGISTER_CTRL_REG4 = 0x23, // 00000000 rw
+ GYRO_REGISTER_CTRL_REG5 = 0x24, // 00000000 rw
+ GYRO_REGISTER_REFERENCE = 0x25, // 00000000 rw
+ GYRO_REGISTER_OUT_TEMP = 0x26, // r
+ GYRO_REGISTER_STATUS_REG = 0x27, // r
+ GYRO_REGISTER_OUT_X_L = 0x28, // r
+ GYRO_REGISTER_OUT_X_H = 0x29, // r
+ GYRO_REGISTER_OUT_Y_L = 0x2A, // r
+ GYRO_REGISTER_OUT_Y_H = 0x2B, // r
+ GYRO_REGISTER_OUT_Z_L = 0x2C, // r
+ GYRO_REGISTER_OUT_Z_H = 0x2D, // r
+ GYRO_REGISTER_FIFO_CTRL_REG = 0x2E, // 00000000 rw
+ GYRO_REGISTER_FIFO_SRC_REG = 0x2F, // r
+ GYRO_REGISTER_INT1_CFG = 0x30, // 00000000 rw
+ GYRO_REGISTER_INT1_SRC = 0x31, // r
+ GYRO_REGISTER_TSH_XH = 0x32, // 00000000 rw
+ GYRO_REGISTER_TSH_XL = 0x33, // 00000000 rw
+ GYRO_REGISTER_TSH_YH = 0x34, // 00000000 rw
+ GYRO_REGISTER_TSH_YL = 0x35, // 00000000 rw
+ GYRO_REGISTER_TSH_ZH = 0x36, // 00000000 rw
+ GYRO_REGISTER_TSH_ZL = 0x37, // 00000000 rw
+ GYRO_REGISTER_INT1_DURATION = 0x38 // 00000000 rw
+ } gyroRegisters_t;
+/*=========================================================================*/
+
+/*=========================================================================
+ OPTIONAL SPEED SETTINGS
+ -----------------------------------------------------------------------*/
+ typedef enum
+ {
+ GYRO_RANGE_250DPS = 250,
+ GYRO_RANGE_500DPS = 500,
+ GYRO_RANGE_2000DPS = 2000
+ } gyroRange_t;
+/*=========================================================================*/
+
+class Adafruit_L3GD20_Unified : public Adafruit_Sensor
+{
+ public:
+ Adafruit_L3GD20_Unified(int32_t sensorID = -1);
+
+ bool begin ( gyroRange_t rng = GYRO_RANGE_250DPS );
+ void enableAutoRange ( bool enabled );
+ void getEvent ( sensors_event_t* );
+ void getSensor ( sensor_t* );
+
+ private:
+ void write8 ( byte reg, byte value );
+ byte read8 ( byte reg );
+ gyroRange_t _range;
+ int32_t _sensorID;
+ bool _autoRangeEnabled;
+};
+
+#endif
diff -r 000000000000 -r 772bf4786416 Header/Adafruit_LSM303_U.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Header/Adafruit_LSM303_U.h Sat Mar 21 12:33:05 2015 +0000
@@ -0,0 +1,172 @@
+/***************************************************************************
+ This is a library for the LSM303 Accelerometer and magnentometer/compass
+
+ Designed specifically to work with the Adafruit LSM303DLHC Breakout
+
+ These displays use I2C to communicate, 2 pins are required to interface.
+
+ Adafruit invests time and resources providing this open source code,
+ please support Adafruit andopen-source hardware by purchasing products
+ from Adafruit!
+
+ Written by Kevin Townsend for Adafruit Industries.
+ BSD license, all text above must be included in any redistribution
+ ***************************************************************************/
+#ifndef __LSM303_H__
+#define __LSM303_H__
+
+#include <Adafruit_Sensor.h>
+#include "I2C_base.h"
+
+/*=========================================================================
+ I2C ADDRESS/BITS
+ -----------------------------------------------------------------------*/
+ #define LSM303_ADDRESS_ACCEL (0x32 >> 1) // 0011001x
+ #define LSM303_ADDRESS_MAG (0x3C >> 1) // 0011110x
+/*=========================================================================*/
+
+/*=========================================================================
+ REGISTERS
+ -----------------------------------------------------------------------*/
+ typedef enum
+ { // DEFAULT TYPE
+ LSM303_REGISTER_ACCEL_CTRL_REG1_A = 0x20, // 00000111 rw
+ LSM303_REGISTER_ACCEL_CTRL_REG2_A = 0x21, // 00000000 rw
+ LSM303_REGISTER_ACCEL_CTRL_REG3_A = 0x22, // 00000000 rw
+ LSM303_REGISTER_ACCEL_CTRL_REG4_A = 0x23, // 00000000 rw
+ LSM303_REGISTER_ACCEL_CTRL_REG5_A = 0x24, // 00000000 rw
+ LSM303_REGISTER_ACCEL_CTRL_REG6_A = 0x25, // 00000000 rw
+ LSM303_REGISTER_ACCEL_REFERENCE_A = 0x26, // 00000000 r
+ LSM303_REGISTER_ACCEL_STATUS_REG_A = 0x27, // 00000000 r
+ LSM303_REGISTER_ACCEL_OUT_X_L_A = 0x28,
+ LSM303_REGISTER_ACCEL_OUT_X_H_A = 0x29,
+ LSM303_REGISTER_ACCEL_OUT_Y_L_A = 0x2A,
+ LSM303_REGISTER_ACCEL_OUT_Y_H_A = 0x2B,
+ LSM303_REGISTER_ACCEL_OUT_Z_L_A = 0x2C,
+ LSM303_REGISTER_ACCEL_OUT_Z_H_A = 0x2D,
+ LSM303_REGISTER_ACCEL_FIFO_CTRL_REG_A = 0x2E,
+ LSM303_REGISTER_ACCEL_FIFO_SRC_REG_A = 0x2F,
+ LSM303_REGISTER_ACCEL_INT1_CFG_A = 0x30,
+ LSM303_REGISTER_ACCEL_INT1_SOURCE_A = 0x31,
+ LSM303_REGISTER_ACCEL_INT1_THS_A = 0x32,
+ LSM303_REGISTER_ACCEL_INT1_DURATION_A = 0x33,
+ LSM303_REGISTER_ACCEL_INT2_CFG_A = 0x34,
+ LSM303_REGISTER_ACCEL_INT2_SOURCE_A = 0x35,
+ LSM303_REGISTER_ACCEL_INT2_THS_A = 0x36,
+ LSM303_REGISTER_ACCEL_INT2_DURATION_A = 0x37,
+ LSM303_REGISTER_ACCEL_CLICK_CFG_A = 0x38,
+ LSM303_REGISTER_ACCEL_CLICK_SRC_A = 0x39,
+ LSM303_REGISTER_ACCEL_CLICK_THS_A = 0x3A,
+ LSM303_REGISTER_ACCEL_TIME_LIMIT_A = 0x3B,
+ LSM303_REGISTER_ACCEL_TIME_LATENCY_A = 0x3C,
+ LSM303_REGISTER_ACCEL_TIME_WINDOW_A = 0x3D
+ } lsm303AccelRegisters_t;
+
+ typedef enum
+ {
+ LSM303_REGISTER_MAG_CRA_REG_M = 0x00,
+ LSM303_REGISTER_MAG_CRB_REG_M = 0x01,
+ LSM303_REGISTER_MAG_MR_REG_M = 0x02,
+ LSM303_REGISTER_MAG_OUT_X_H_M = 0x03,
+ LSM303_REGISTER_MAG_OUT_X_L_M = 0x04,
+ LSM303_REGISTER_MAG_OUT_Z_H_M = 0x05,
+ LSM303_REGISTER_MAG_OUT_Z_L_M = 0x06,
+ LSM303_REGISTER_MAG_OUT_Y_H_M = 0x07,
+ LSM303_REGISTER_MAG_OUT_Y_L_M = 0x08,
+ LSM303_REGISTER_MAG_SR_REG_Mg = 0x09,
+ LSM303_REGISTER_MAG_IRA_REG_M = 0x0A,
+ LSM303_REGISTER_MAG_IRB_REG_M = 0x0B,
+ LSM303_REGISTER_MAG_IRC_REG_M = 0x0C,
+ LSM303_REGISTER_MAG_TEMP_OUT_H_M = 0x31,
+ LSM303_REGISTER_MAG_TEMP_OUT_L_M = 0x32
+ } lsm303MagRegisters_t;
+/*=========================================================================*/
+
+/*=========================================================================
+ MAGNETOMETER GAIN SETTINGS
+ -----------------------------------------------------------------------*/
+ typedef enum
+ {
+ LSM303_MAGGAIN_1_3 = 0x20, // +/- 1.3
+ LSM303_MAGGAIN_1_9 = 0x40, // +/- 1.9
+ LSM303_MAGGAIN_2_5 = 0x60, // +/- 2.5
+ LSM303_MAGGAIN_4_0 = 0x80, // +/- 4.0
+ LSM303_MAGGAIN_4_7 = 0xA0, // +/- 4.7
+ LSM303_MAGGAIN_5_6 = 0xC0, // +/- 5.6
+ LSM303_MAGGAIN_8_1 = 0xE0 // +/- 8.1
+ } lsm303MagGain;
+/*=========================================================================*/
+
+/*=========================================================================
+ INTERNAL MAGNETOMETER DATA TYPE
+ -----------------------------------------------------------------------*/
+ typedef struct lsm303MagData_s
+ {
+ float x;
+ float y;
+ float z;
+ float orientation;
+ } lsm303MagData;
+/*=========================================================================*/
+
+/*=========================================================================
+ INTERNAL ACCELERATION DATA TYPE
+ -----------------------------------------------------------------------*/
+ typedef struct lsm303AccelData_s
+ {
+ float x;
+ float y;
+ float z;
+ } lsm303AccelData;
+/*=========================================================================*/
+
+/*=========================================================================
+ CHIP ID
+ -----------------------------------------------------------------------*/
+ #define LSM303_ID (0b11010100)
+/*=========================================================================*/
+
+/* Unified sensor driver for the accelerometer */
+class Adafruit_LSM303_Accel_Unified : public Adafruit_Sensor
+{
+ public:
+ Adafruit_LSM303_Accel_Unified(int32_t sensorID = -1);
+
+ bool begin(void);
+ void getEvent(sensors_event_t*);
+ void getSensor(sensor_t*);
+ void enableAutoRange(bool enable){}
+
+ private:
+ lsm303AccelData _accelData; // Last read accelerometer data will be available here
+ int32_t _sensorID;
+
+ void write8(byte address, byte reg, byte value);
+ byte read8(byte address, byte reg);
+ void read(void);
+};
+
+/* Unified sensor driver for the magnetometer */
+class Adafruit_LSM303_Mag_Unified : public Adafruit_Sensor
+{
+ public:
+ Adafruit_LSM303_Mag_Unified(int32_t sensorID = -1);
+
+ bool begin(void);
+ void enableAutoRange(bool enable);
+ void setMagGain(lsm303MagGain gain);
+ void getEvent(sensors_event_t*);
+ void getSensor(sensor_t*);
+
+ private:
+ lsm303MagGain _magGain;
+ lsm303MagData _magData; // Last read magnetometer data will be available here
+ int32_t _sensorID;
+ bool _autoRangeEnabled;
+
+ void write8(byte address, byte reg, byte value);
+ byte read8(byte address, byte reg);
+ void read(void);
+};
+
+#endif
diff -r 000000000000 -r 772bf4786416 Header/Adafruit_Sensor.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Header/Adafruit_Sensor.h Sat Mar 21 12:33:05 2015 +0000
@@ -0,0 +1,150 @@
+/*
+* Copyright (C) 2008 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< /span>
+* 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 <cstdint>
+using namespace std;
+
+/* Update by K. Townsend (Adafruit Industries) for lighter typedefs, and
+ * extended sensor support to include color, voltage and current */
+
+#ifndef _ADAFRUIT_SENSOR_H
+#define _ADAFRUIT_SENSOR_H
+
+
+/* Intentionally modeled after sensors.h in the Android API:
+ * https://github.com/android/platform_hardware_libhardware/blob/master/include/hardware/sensors.h */
+
+/* Constants */
+#define SENSORS_GRAVITY_EARTH (9.80665F) /**< Earth's gravity in m/s^2 */
+#define SENSORS_GRAVITY_MOON (1.6F) /**< The moon's gravity in m/s^2 */
+#define SENSORS_GRAVITY_SUN (275.0F) /**< The sun's gravity in m/s^2 */
+#define SENSORS_GRAVITY_STANDARD (SENSORS_GRAVITY_EARTH)
+#define SENSORS_MAGFIELD_EARTH_MAX (60.0F) /**< Maximum magnetic field on Earth's surface */
+#define SENSORS_MAGFIELD_EARTH_MIN (30.0F) /**< Minimum magnetic field on Earth's surface */
+#define SENSORS_PRESSURE_SEALEVELHPA (1013.25F) /**< Average sea level pressure is 1013.25 hPa */
+#define SENSORS_DPS_TO_RADS (0.017453293F) /**< Degrees/s to rad/s multiplier */
+#define SENSORS_GAUSS_TO_MICROTESLA (100) /**< Gauss to micro-Tesla multiplier */
+
+/** Sensor types */
+typedef enum
+{
+ SENSOR_TYPE_ACCELEROMETER = (1), /**< Gravity + linear acceleration */
+ SENSOR_TYPE_MAGNETIC_FIELD = (2),
+ SENSOR_TYPE_ORIENTATION = (3),
+ SENSOR_TYPE_GYROSCOPE = (4),
+ SENSOR_TYPE_LIGHT = (5),
+ SENSOR_TYPE_PRESSURE = (6),
+ SENSOR_TYPE_PROXIMITY = (8),
+ SENSOR_TYPE_GRAVITY = (9),
+ SENSOR_TYPE_LINEAR_ACCELERATION = (10), /**< Acceleration not including gravity */
+ SENSOR_TYPE_ROTATION_VECTOR = (11),
+ SENSOR_TYPE_RELATIVE_HUMIDITY = (12),
+ SENSOR_TYPE_AMBIENT_TEMPERATURE = (13),
+ SENSOR_TYPE_VOLTAGE = (15),
+ SENSOR_TYPE_CURRENT = (16),
+ SENSOR_TYPE_COLOR = (17)
+} sensors_type_t;
+
+/** struct sensors_vec_s is used to return a vector in a common format. */
+typedef struct {
+ union {
+ float v[3];
+ struct {
+ float x;
+ float y;
+ float z;
+ };
+ /* Orientation sensors */
+ struct {
+ float roll; /**< Rotation around the longitudinal axis (the plane body, 'X axis'). Roll is positive and increasing when moving downward. -90�<=roll<=90� */
+ float pitch; /**< Rotation around the lateral axis (the wing span, 'Y axis'). Pitch is positive and increasing when moving upwards. -180�<=pitch<=180�) */
+ float heading; /**< Angle between the longitudinal axis (the plane body) and magnetic north, measured clockwise when viewing from the top of the device. 0-359� */
+ };
+ };
+ int8_t status;
+ uint8_t reserved[3];
+} sensors_vec_t;
+
+/** struct sensors_color_s is used to return color data in a common format. */
+typedef struct {
+ union {
+ float c[3];
+ /* RGB color space */
+ struct {
+ float r; /**< Red component */
+ float g; /**< Green component */
+ float b; /**< Blue component */
+ };
+ };
+ uint32_t rgba; /**< 24-bit RGBA value */
+} sensors_color_t;
+
+/* Sensor event (36 bytes) */
+/** struct sensor_event_s is used to provide a single sensor event in a common format. */
+typedef struct
+{
+ int32_t version; /**< must be sizeof(struct sensors_event_t) */
+ int32_t sensor_id; /**< unique sensor identifier */
+ int32_t type; /**< sensor type */
+ int32_t reserved0; /**< reserved */
+ int32_t timestamp; /**< time is in milliseconds */
+ union
+ {
+ float data[4];
+ sensors_vec_t acceleration; /**< acceleration values are in meter per second per second (m/s^2) */
+ sensors_vec_t magnetic; /**< magnetic vector values are in micro-Tesla (uT) */
+ sensors_vec_t orientation; /**< orientation values are in degrees */
+ sensors_vec_t gyro; /**< gyroscope values are in rad/s */
+ float temperature; /**< temperature is in degrees centigrade (Celsius) */
+ float distance; /**< distance in centimeters */
+ float light; /**< light in SI lux units */
+ float pressure; /**< pressure in hectopascal (hPa) */
+ float relative_humidity; /**< relative humidity in percent */
+ float current; /**< current in milliamps (mA) */
+ float voltage; /**< voltage in volts (V) */
+ sensors_color_t color; /**< color in RGB component values */
+ };
+} sensors_event_t;
+
+/* Sensor details (40 bytes) */
+/** struct sensor_s is used to describe basic information about a specific sensor. */
+typedef struct
+{
+ char name[12]; /**< sensor name */
+ int32_t version; /**< version of the hardware + driver */
+ int32_t sensor_id; /**< unique sensor identifier */
+ int32_t type; /**< this sensor's type (ex. SENSOR_TYPE_LIGHT) */
+ float max_value; /**< maximum value of this sensor's value in SI units */
+ float min_value; /**< minimum value of this sensor's value in SI units */
+ float resolution; /**< smallest difference between two values reported by this sensor */
+ int32_t min_delay; /**< min delay in microseconds between events. zero = not a constant rate */
+} sensor_t;
+
+class Adafruit_Sensor {
+ public:
+ // Constructor(s)
+ void constructor();
+
+ // These must be defined by the subclass
+ virtual void enableAutoRange(bool enabled) = 0;
+ virtual void getEvent(sensors_event_t*) = 0;
+ virtual void getSensor(sensor_t*) = 0;
+
+ private:
+ bool _autoRange;
+};
+
+#endif
diff -r 000000000000 -r 772bf4786416 Header/I2C_base.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Header/I2C_base.h Sat Mar 21 12:33:05 2015 +0000
@@ -0,0 +1,77 @@
+#ifndef I2C_BASE_H
+#define I2C_BASE_H
+
+#define _MBED_
+
+//Base class to extend I2C support to multiple platforms
+typedef char byte;
+
+
+class I2C_base
+{
+ public:
+ virtual void frequency (int frequency) = 0; //set the communication frequency
+ virtual bool read (int address, byte* data, int length) = 0; //read data from the bus
+ virtual bool write (int address, const byte* data, int length) = 0; //write data to the bus
+ virtual bool writeByte (int address, byte data) = 0; //write byte to the bus
+
+ private:
+ int _frequency;
+};
+
+extern I2C_base* i2c;
+
+
+ //MBED programming environmnet
+#ifdef _MBED_
+#include "mbed.h"
+//define SDA and SCL if not already defined
+#ifndef SDA
+ #define SDA p28
+#endif
+
+#ifndef SCL
+ #define SCL p27
+#endif
+//*******************************************
+
+
+/* should be placed somewhere else, just debug code*/
+
+
+
+int millis();
+
+/***************************************************/
+
+class I2C_MBED: public I2C_base
+{
+ public:
+ I2C_MBED (PinName pSDA, PinName pSCL): _i2c(pSDA,pSCL){}
+ virtual void frequency (int hz)
+ {
+ _i2c.frequency(hz);
+ }
+ virtual bool read(int address, byte* data, int length)
+ {
+ return !_i2c.read(address<<1,data,length);
+ }
+ virtual bool write(int address, const byte* data, int length)
+ {
+ return !_i2c.write(address<<1,data,length);
+ }
+ virtual bool writeByte(int address, byte data)
+ {
+ return write(address, &data, 1);
+ }
+
+ private:
+ I2C _i2c;
+};
+
+
+
+#endif //_MBED_
+
+
+#endif //I2C_BASE_H
\ No newline at end of file
diff -r 000000000000 -r 772bf4786416 Header/Serial_base.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Header/Serial_base.h Sat Mar 21 12:33:05 2015 +0000
@@ -0,0 +1,69 @@
+#ifndef SERIAL_BASE_H
+#define SERIAL_BASE_H
+
+#define _MBED_
+
+class Serial_base
+{
+ public:
+
+ virtual void print(const char* data) = 0;
+ virtual void print(int data) = 0;
+ virtual void print(double data) = 0;
+
+ virtual void println(const char* data) = 0;
+ virtual void println(int data) = 0;
+ virtual void println(double data) = 0;
+
+ virtual void setPrecision(int prec) = 0;
+};
+
+extern Serial_base* s_com;
+
+
+#ifdef _MBED_
+#include <iostream>
+#include <iomanip>
+
+class Serial_MBED: public Serial_base
+{
+ public:
+ virtual void print(const char* data){
+ _print(data);
+ }
+ virtual void print(int data){
+ _print(data);
+ }
+ virtual void print(double data){
+ _print(data);
+ }
+
+ virtual void println(const char* data){
+ _println(data);
+ }
+ virtual void println(int data){
+ _println(data);
+ }
+ virtual void println(double data){
+ _println(data);
+ }
+ virtual void setPrecision(int p){
+ std::cout.precision(p);
+ //std::cout.setf( std::ios::fixed, std:: ios::floatfield );
+ }
+ private:
+ template<typename T>
+ void _print(T data) {
+ std::cout<<data<<std::flush;
+ }
+ template<typename T>
+ void _println(T data){
+ std::cout<<data<<std::endl<<std::flush;
+ }
+
+};
+
+
+#endif //_MBED_
+
+#endif
\ No newline at end of file
diff -r 000000000000 -r 772bf4786416 Source/Adafruit_9DOF.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Source/Adafruit_9DOF.cpp Sat Mar 21 12:33:05 2015 +0000
@@ -0,0 +1,351 @@
+/***************************************************************************
+ This is a library for the Adafruit 9DOF Breakout
+
+ Designed specifically to work with the Adafruit 9DOF Breakout:
+ http://www.adafruit.com/products/1714
+
+ These displays use I2C to communicate, 2 pins are required to interface.
+
+ Adafruit invests time and resources providing this open source code,
+ please support Adafruit andopen-source hardware by purchasing products
+ from Adafruit!
+
+ Written by Kevin Townsend for Adafruit Industries.
+ BSD license, all text above must be included in any redistribution
+ ***************************************************************************/
+
+#include <math.h>
+
+#include "Adafruit_9DOF.h"
+
+#define PI (3.14159265F);
+
+/***************************************************************************
+ PRIVATE FUNCTIONS
+ ***************************************************************************/
+
+
+/***************************************************************************
+ CONSTRUCTOR
+ ***************************************************************************/
+
+/**************************************************************************/
+/*!
+ @brief Instantiates a new Adafruit_9DOF class
+*/
+/**************************************************************************/
+Adafruit_9DOF::Adafruit_9DOF(void)
+{
+}
+
+/***************************************************************************
+ PUBLIC FUNCTIONS
+ ***************************************************************************/
+
+/**************************************************************************/
+/*!
+ @brief Setups the HW
+*/
+/**************************************************************************/
+bool Adafruit_9DOF::begin()
+{
+ // Enable I2C
+
+
+ return true;
+}
+
+/**************************************************************************/
+/*!
+ @brief Populates the .pitch/.roll fields in the sensors_vec_t struct
+ with the right angular data (in degree)
+
+ @param event The sensors_event_t variable containing the
+ data from the accelerometer
+ @param orientation The sensors_vec_t object that will have it's
+ .pitch and .roll fields populated
+ @return Returns true if the operation was successful, false if there
+ was an error
+
+ @code
+
+ bool error;
+ sensors_event_t event;
+ sensors_vec_t orientation;
+ ...
+ lsm303accelGetSensorEvent(&event);
+ error = accelGetOrientation(&event, &orientation);
+
+ @endcode
+*/
+/**************************************************************************/
+bool Adafruit_9DOF::accelGetOrientation(sensors_event_t *event, sensors_vec_t *orientation)
+{
+ /* Make sure the input is valid, not null, etc. */
+ if (event == NULL) return false;
+ if (orientation == NULL) return false;
+
+ float t_pitch;
+ float t_roll;
+ float signOfZ = event->acceleration.z >= 0 ? 1.0F : -1.0F;
+
+ /* roll: Rotation around the longitudinal axis (the plane body, 'X axis'). -90<=roll<=90 */
+ /* roll is positive and increasing when moving downward */
+ /* */
+ /* y */
+ /* roll = atan(-----------------) */
+ /* sqrt(x^2 + z^2) */
+ /* where: x, y, z are returned value from accelerometer sensor */
+
+ t_roll = event->acceleration.x * event->acceleration.x + event->acceleration.z * event->acceleration.z;
+ orientation->roll = (float)atan2(event->acceleration.y, sqrt(t_roll)) * 180 / PI;
+
+ /* pitch: Rotation around the lateral axis (the wing span, 'Y axis'). -180<=pitch<=180) */
+ /* pitch is positive and increasing when moving upwards */
+ /* */
+ /* x */
+ /* roll = atan(-----------------) */
+ /* sqrt(y^2 + z^2) */
+ /* where: x, y, z are returned value from accelerometer sensor */
+
+ t_pitch = event->acceleration.y * event->acceleration.y + event->acceleration.z * event->acceleration.z;
+ orientation->pitch = (float)atan2(event->acceleration.x, signOfZ * sqrt(t_pitch)) * 180 / PI;
+
+ return true;
+}
+
+
+/**************************************************************************/
+/*!
+ @brief Utilize the sensor data from an accelerometer to compensate
+ the magnetic sensor measurements when the sensor is tilted
+ (the pitch and roll angles are not equal 0�)
+
+ @param axis The given axis (SENSOR_AXIS_X/Y/Z) that is
+ parallel to the gravity of the Earth
+
+ @param mag_event The raw magnetometer data to adjust for tilt
+
+ @param accel_event The accelerometer event data to use to determine
+ the tilt when compensating the mag_event values
+
+ @code
+
+ // Perform tilt compensation with matching accelerometer data
+ sensors_event_t accel_event;
+ error = lsm303accelGetSensorEvent(&accel_event);
+ if (!error)
+ {
+ magTiltCompensation(SENSOR_AXIS_Z, &mag_event, &accel_event);
+ }
+
+ @endcode
+*/
+/**************************************************************************/
+bool Adafruit_9DOF::magTiltCompensation(sensors_axis_t axis, sensors_event_t *mag_event, sensors_event_t *accel_event)
+{
+ /* Make sure the input is valid, not null, etc. */
+ if (mag_event == NULL) return false;
+ if (accel_event == NULL) return false;
+
+ float accel_X, accel_Y, accel_Z;
+ float *mag_X, *mag_Y, *mag_Z;
+
+ switch (axis)
+ {
+ case SENSOR_AXIS_X:
+ /* The X-axis is parallel to the gravity */
+ accel_X = accel_event->acceleration.y;
+ accel_Y = accel_event->acceleration.z;
+ accel_Z = accel_event->acceleration.x;
+ mag_X = &(mag_event->magnetic.y);
+ mag_Y = &(mag_event->magnetic.z);
+ mag_Z = &(mag_event->magnetic.x);
+ break;
+
+ case SENSOR_AXIS_Y:
+ /* The Y-axis is parallel to the gravity */
+ accel_X = accel_event->acceleration.z;
+ accel_Y = accel_event->acceleration.x;
+ accel_Z = accel_event->acceleration.y;
+ mag_X = &(mag_event->magnetic.z);
+ mag_Y = &(mag_event->magnetic.x);
+ mag_Z = &(mag_event->magnetic.y);
+ break;
+
+ case SENSOR_AXIS_Z:
+ /* The Z-axis is parallel to the gravity */
+ accel_X = accel_event->acceleration.x;
+ accel_Y = accel_event->acceleration.y;
+ accel_Z = accel_event->acceleration.z;
+ mag_X = &(mag_event->magnetic.x);
+ mag_Y = &(mag_event->magnetic.y);
+ mag_Z = &(mag_event->magnetic.z);
+ break;
+
+ default:
+ return false;
+ }
+
+ float t_roll = accel_X * accel_X + accel_Z * accel_Z;
+ float rollRadians = (float)atan2(accel_Y, sqrt(t_roll));
+
+ float t_pitch = accel_Y * accel_Y + accel_Z * accel_Z;
+ float pitchRadians = (float)atan2(accel_X, sqrt(t_pitch));
+
+ float cosRoll = (float)cos(rollRadians);
+ float sinRoll = (float)sin(rollRadians);
+ float cosPitch = (float)cos(-1*pitchRadians);
+ float sinPitch = (float)sin(-1*pitchRadians);
+
+ /* The tilt compensation algorithm */
+ /* Xh = X.cosPitch + Z.sinPitch */
+ /* Yh = X.sinRoll.sinPitch + Y.cosRoll - Z.sinRoll.cosPitch */
+ *mag_X = (*mag_X) * cosPitch + (*mag_Z) * sinPitch;
+ *mag_Y = (*mag_X) * sinRoll * sinPitch + (*mag_Y) * cosRoll - (*mag_Z) * sinRoll * cosPitch;
+
+ return true;
+}
+
+/**************************************************************************/
+/*!
+ @brief Populates the .heading fields in the sensors_vec_t
+ struct with the right angular data (0-359�)
+
+ Heading increases when measuring clockwise
+
+ @param axis The given axis (SENSOR_AXIS_X/Y/Z)
+
+ @param event The raw magnetometer sensor data to use when
+ calculating out heading
+
+ @param orientation The sensors_vec_t object where we will
+ assign an 'orientation.heading' value
+
+ @code
+
+ magGetOrientation(SENSOR_AXIS_Z, &mag_event, &orientation);
+
+ @endcode
+*/
+/**************************************************************************/
+bool Adafruit_9DOF::magGetOrientation(sensors_axis_t axis, sensors_event_t *event, sensors_vec_t *orientation)
+{
+ /* Make sure the input is valid, not null, etc. */
+ if (event == NULL) return false;
+ if (orientation == NULL) return false;
+
+ switch (axis)
+ {
+ case SENSOR_AXIS_X:
+ /* Sensor rotates around X-axis */
+ /* "heading" is the angle between the 'Y axis' and magnetic north on the horizontal plane (Oyz) */
+ /* heading = atan(Mz / My) */
+ orientation->heading = (float)atan2(event->magnetic.z, event->magnetic.y) * 180 / PI;
+ break;
+
+ case SENSOR_AXIS_Y:
+ /* Sensor rotates around Y-axis */
+ /* "heading" is the angle between the 'Z axis' and magnetic north on the horizontal plane (Ozx) */
+ /* heading = atan(Mx / Mz) */
+ orientation->heading = (float)atan2(event->magnetic.x, event->magnetic.z) * 180 / PI;
+ break;
+
+ case SENSOR_AXIS_Z:
+ /* Sensor rotates around Z-axis */
+ /* "heading" is the angle between the 'X axis' and magnetic north on the horizontal plane (Oxy) */
+ /* heading = atan(My / Mx) */
+ orientation->heading = (float)atan2(event->magnetic.y, event->magnetic.x) * 180 / PI;
+ break;
+
+ default:
+ return false;
+ }
+
+ /* Normalize to 0-359� */
+ if (orientation->heading < 0)
+ {
+ orientation->heading = 360 + orientation->heading;
+ }
+
+ return true;
+}
+
+/**************************************************************************/
+/*!
+ @brief Populates the .roll/.pitch/.heading fields in the sensors_vec_t
+ struct with the right angular data (in degree).
+
+ The starting position is set by placing the object flat and
+ pointing northwards (Z-axis pointing upward and X-axis pointing
+ northwards).
+
+ The orientation of the object can be modeled as resulting from
+ 3 consecutive rotations in turn: heading (Z-axis), pitch (Y-axis),
+ and roll (X-axis) applied to the starting position.
+
+
+ @param accel_event The sensors_event_t variable containing the
+ data from the accelerometer
+
+ @param mag_event The sensors_event_t variable containing the
+ data from the magnetometer
+
+ @param orientation The sensors_vec_t object that will have it's
+ .roll, .pitch and .heading fields populated
+*/
+/**************************************************************************/
+bool Adafruit_9DOF::fusionGetOrientation(sensors_event_t *accel_event, sensors_event_t *mag_event, sensors_vec_t *orientation)
+{
+ /* Make sure the input is valid, not null, etc. */
+ if ( accel_event == NULL) return false;
+ if ( mag_event == NULL) return false;
+ if ( orientation == NULL) return false;
+
+ float const PI_F = 3.14159265F;
+
+ /* roll: Rotation around the X-axis. -180 <= roll <= 180 */
+ /* a positive roll angle is defined to be a clockwise rotation about the positive X-axis */
+ /* */
+ /* y */
+ /* roll = atan2(---) */
+ /* z */
+ /* */
+ /* where: y, z are returned value from accelerometer sensor */
+ orientation->roll = (float)atan2(accel_event->acceleration.y, accel_event->acceleration.z);
+
+ /* pitch: Rotation around the Y-axis. -180 <= roll <= 180 */
+ /* a positive pitch angle is defined to be a clockwise rotation about the positive Y-axis */
+ /* */
+ /* -x */
+ /* pitch = atan(-------------------------------) */
+ /* y * sin(roll) + z * cos(roll) */
+ /* */
+ /* where: x, y, z are returned value from accelerometer sensor */
+ if (accel_event->acceleration.y * sin(orientation->roll) + accel_event->acceleration.z * cos(orientation->roll) == 0)
+ orientation->pitch = accel_event->acceleration.x > 0 ? (PI_F / 2) : (-PI_F / 2);
+ else
+ orientation->pitch = (float)atan(-accel_event->acceleration.x / (accel_event->acceleration.y * sin(orientation->roll) + \
+ accel_event->acceleration.z * cos(orientation->roll)));
+
+ /* heading: Rotation around the Z-axis. -180 <= roll <= 180 */
+ /* a positive heading angle is defined to be a clockwise rotation about the positive Z-axis */
+ /* */
+ /* z * sin(roll) - y * cos(roll) */
+ /* heading = atan2(--------------------------------------------------------------------------) */
+ /* x * cos(pitch) + y * sin(pitch) * sin(roll) + z * sin(pitch) * cos(roll)) */
+ /* */
+ /* where: x, y, z are returned value from magnetometer sensor */
+ orientation->heading = (float)atan2(mag_event->magnetic.z * sin(orientation->roll) - mag_event->magnetic.y * cos(orientation->roll), \
+ mag_event->magnetic.x * cos(orientation->pitch) + \
+ mag_event->magnetic.y * sin(orientation->pitch) * sin(orientation->roll) + \
+ mag_event->magnetic.z * sin(orientation->pitch) * cos(orientation->roll));
+
+
+ /* Convert angular data to degree */
+ orientation->roll = orientation->roll * 180 / PI_F;
+ orientation->pitch = orientation->pitch * 180 / PI_F;
+ orientation->heading = orientation->heading * 180 / PI_F;
+
+ return true;
+}
diff -r 000000000000 -r 772bf4786416 Source/Adafruit_L3GD20_U.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Source/Adafruit_L3GD20_U.cpp Sat Mar 21 12:33:05 2015 +0000
@@ -0,0 +1,310 @@
+/***************************************************
+ This is a library for the L3GD20 GYROSCOPE
+
+ Designed specifically to work with the Adafruit L3GD20 Breakout
+ ----> https://www.adafruit.com/products/1032
+
+ These sensors use I2C or SPI to communicate, 2 pins (I2C)
+ or 4 pins (SPI) are required to interface.
+
+ Adafruit invests time and resources providing this open source code,
+ please support Adafruit and open-source hardware by purchasing
+ products from Adafruit!
+
+ Written by Kevin "KTOWN" Townsend for Adafruit Industries.
+ BSD license, all text above must be included in any redistribution
+ ****************************************************/
+
+#include "Adafruit_L3GD20_U.h"
+
+/***************************************************************************
+ PRIVATE FUNCTIONS
+ ***************************************************************************/
+
+/**************************************************************************/
+/*!
+ @brief Abstract away platform differences in Arduino wire library
+*/
+/**************************************************************************/
+void Adafruit_L3GD20_Unified::write8(byte reg, byte value)
+{
+ byte data[2] = {reg,value};
+ i2c->write(L3GD20_ADDRESS, data, 2);
+}
+
+/**************************************************************************/
+/*!
+ @brief Abstract away platform differences in Arduino wire library
+*/
+/**************************************************************************/
+byte Adafruit_L3GD20_Unified::read8(byte reg)
+{
+ byte value = reg;
+ i2c->write(L3GD20_ADDRESS, &value, 1);
+ i2c->read (L3GD20_ADDRESS, &value, 1);
+
+ return value;
+}
+
+/***************************************************************************
+ CONSTRUCTOR
+ ***************************************************************************/
+
+/**************************************************************************/
+/*!
+ @brief Instantiates a new Adafruit_L3GD20_Unified class
+*/
+/**************************************************************************/
+Adafruit_L3GD20_Unified::Adafruit_L3GD20_Unified(int32_t sensorID) {
+ _sensorID = sensorID;
+ _autoRangeEnabled = false;
+}
+
+/***************************************************************************
+ PUBLIC FUNCTIONS
+ ***************************************************************************/
+
+/**************************************************************************/
+/*!
+ @brief Setups the HW
+*/
+/**************************************************************************/
+bool Adafruit_L3GD20_Unified::begin(gyroRange_t rng)
+{
+
+
+ /* Set the range the an appropriate value */
+ _range = rng;
+
+ /* Make sure we have the correct chip ID since this checks
+ for correct address and that the IC is properly connected */
+ uint8_t id = read8(GYRO_REGISTER_WHO_AM_I);
+ //Serial.println(id, HEX);
+ if ((id != L3GD20_ID) && (id != L3GD20H_ID))
+ {
+ return false;
+ }
+
+ /* Set CTRL_REG1 (0x20)
+ ====================================================================
+ BIT Symbol Description Default
+ --- ------ --------------------------------------------- -------
+ 7-6 DR1/0 Output data rate 00
+ 5-4 BW1/0 Bandwidth selection 00
+ 3 PD 0 = Power-down mode, 1 = normal/sleep mode 0
+ 2 ZEN Z-axis enable (0 = disabled, 1 = enabled) 1
+ 1 YEN Y-axis enable (0 = disabled, 1 = enabled) 1
+ 0 XEN X-axis enable (0 = disabled, 1 = enabled) 1 */
+
+ /* Reset then switch to normal mode and enable all three channels */
+ write8(GYRO_REGISTER_CTRL_REG1, 0x00);
+ write8(GYRO_REGISTER_CTRL_REG1, 0x0F);
+ /* ------------------------------------------------------------------ */
+
+ /* Set CTRL_REG2 (0x21)
+ ====================================================================
+ BIT Symbol Description Default
+ --- ------ --------------------------------------------- -------
+ 5-4 HPM1/0 High-pass filter mode selection 00
+ 3-0 HPCF3..0 High-pass filter cutoff frequency selection 0000 */
+
+ /* Nothing to do ... keep default values */
+ /* ------------------------------------------------------------------ */
+
+ /* Set CTRL_REG3 (0x22)
+ ====================================================================
+ BIT Symbol Description Default
+ --- ------ --------------------------------------------- -------
+ 7 I1_Int1 Interrupt enable on INT1 (0=disable,1=enable) 0
+ 6 I1_Boot Boot status on INT1 (0=disable,1=enable) 0
+ 5 H-Lactive Interrupt active config on INT1 (0=high,1=low) 0
+ 4 PP_OD Push-Pull/Open-Drain (0=PP, 1=OD) 0
+ 3 I2_DRDY Data ready on DRDY/INT2 (0=disable,1=enable) 0
+ 2 I2_WTM FIFO wtrmrk int on DRDY/INT2 (0=dsbl,1=enbl) 0
+ 1 I2_ORun FIFO overrun int on DRDY/INT2 (0=dsbl,1=enbl) 0
+ 0 I2_Empty FIFI empty int on DRDY/INT2 (0=dsbl,1=enbl) 0 */
+
+ /* Nothing to do ... keep default values */
+ /* ------------------------------------------------------------------ */
+
+ /* Set CTRL_REG4 (0x23)
+ ====================================================================
+ BIT Symbol Description Default
+ --- ------ --------------------------------------------- -------
+ 7 BDU Block Data Update (0=continuous, 1=LSB/MSB) 0
+ 6 BLE Big/Little-Endian (0=Data LSB, 1=Data MSB) 0
+ 5-4 FS1/0 Full scale selection 00
+ 00 = 250 dps
+ 01 = 500 dps
+ 10 = 2000 dps
+ 11 = 2000 dps
+ 0 SIM SPI Mode (0=4-wire, 1=3-wire) 0 */
+
+ /* Adjust resolution if requested */
+ switch(_range)
+ {
+ case GYRO_RANGE_250DPS:
+ write8(GYRO_REGISTER_CTRL_REG4, 0x00);
+ break;
+ case GYRO_RANGE_500DPS:
+ write8(GYRO_REGISTER_CTRL_REG4, 0x10);
+ break;
+ case GYRO_RANGE_2000DPS:
+ write8(GYRO_REGISTER_CTRL_REG4, 0x20);
+ break;
+ }
+ /* ------------------------------------------------------------------ */
+
+ /* Set CTRL_REG5 (0x24)
+ ====================================================================
+ BIT Symbol Description Default
+ --- ------ --------------------------------------------- -------
+ 7 BOOT Reboot memory content (0=normal, 1=reboot) 0
+ 6 FIFO_EN FIFO enable (0=FIFO disable, 1=enable) 0
+ 4 HPen High-pass filter enable (0=disable,1=enable) 0
+ 3-2 INT1_SEL INT1 Selection config 00
+ 1-0 OUT_SEL Out selection config 00 */
+
+ /* Nothing to do ... keep default values */
+ /* ------------------------------------------------------------------ */
+
+ return true;
+}
+
+/**************************************************************************/
+/*!
+ @brief Enables or disables auto-ranging
+*/
+/**************************************************************************/
+void Adafruit_L3GD20_Unified::enableAutoRange(bool enabled)
+{
+ _autoRangeEnabled = enabled;
+}
+
+/**************************************************************************/
+/*!
+ @brief Gets the most recent sensor event
+*/
+/**************************************************************************/
+void Adafruit_L3GD20_Unified::getEvent(sensors_event_t* event)
+{
+ bool readingValid = false;
+
+ /* Clear the event */
+ memset(event, 0, sizeof(sensors_event_t));
+
+ event->version = sizeof(sensors_event_t);
+ event->sensor_id = _sensorID;
+ event->type = SENSOR_TYPE_GYROSCOPE;
+
+ while(!readingValid)
+ {
+ event->timestamp = millis();
+
+
+ i2c->writeByte(L3GD20_ADDRESS, GYRO_REGISTER_OUT_X_L | 0x80);
+
+ byte data[6];
+
+ i2c->read(L3GD20_ADDRESS, data, 6);
+
+ /* Shift values to create properly formed integer (low byte first) */
+ event->gyro.x = (int16_t)(data[0] | (data[1] << 8));
+ event->gyro.y = (int16_t)(data[2] | (data[3] << 8));
+ event->gyro.z = (int16_t)(data[4] | (data[5] << 8));
+
+ /* Make sure the sensor isn't saturating if auto-ranging is enabled */
+ if (!_autoRangeEnabled)
+ {
+ readingValid = true;
+ }
+ else
+ {
+ /* Check if the sensor is saturating or not */
+ if ( (event->gyro.x >= 32760) | (event->gyro.x <= -32760) |
+ (event->gyro.y >= 32760) | (event->gyro.y <= -32760) |
+ (event->gyro.z >= 32760) | (event->gyro.z <= -32760) )
+ {
+ /* Saturating .... increase the range if we can */
+ switch(_range)
+ {
+ case GYRO_RANGE_500DPS:
+ /* Push the range up to 2000dps */
+ _range = GYRO_RANGE_2000DPS;
+ write8(GYRO_REGISTER_CTRL_REG1, 0x00);
+ write8(GYRO_REGISTER_CTRL_REG1, 0x0F);
+ write8(GYRO_REGISTER_CTRL_REG4, 0x20);
+ write8(GYRO_REGISTER_CTRL_REG5, 0x80);
+ readingValid = false;
+ // Serial.println("Changing range to 2000DPS");
+ break;
+ case GYRO_RANGE_250DPS:
+ /* Push the range up to 500dps */
+ _range = GYRO_RANGE_500DPS;
+ write8(GYRO_REGISTER_CTRL_REG1, 0x00);
+ write8(GYRO_REGISTER_CTRL_REG1, 0x0F);
+ write8(GYRO_REGISTER_CTRL_REG4, 0x10);
+ write8(GYRO_REGISTER_CTRL_REG5, 0x80);
+ readingValid = false;
+ // Serial.println("Changing range to 500DPS");
+ break;
+ default:
+ readingValid = true;
+ break;
+ }
+ }
+ else
+ {
+ /* All values are withing range */
+ readingValid = true;
+ }
+ }
+ }
+
+ /* Compensate values depending on the resolution */
+ switch(_range)
+ {
+ case GYRO_RANGE_250DPS:
+ event->gyro.x *= GYRO_SENSITIVITY_250DPS;
+ event->gyro.y *= GYRO_SENSITIVITY_250DPS;
+ event->gyro.z *= GYRO_SENSITIVITY_250DPS;
+ break;
+ case GYRO_RANGE_500DPS:
+ event->gyro.x *= GYRO_SENSITIVITY_500DPS;
+ event->gyro.y *= GYRO_SENSITIVITY_500DPS;
+ event->gyro.z *= GYRO_SENSITIVITY_500DPS;
+ break;
+ case GYRO_RANGE_2000DPS:
+ event->gyro.x *= GYRO_SENSITIVITY_2000DPS;
+ event->gyro.y *= GYRO_SENSITIVITY_2000DPS;
+ event->gyro.z *= GYRO_SENSITIVITY_2000DPS;
+ break;
+ }
+
+ /* Convert values to rad/s */
+ event->gyro.x *= SENSORS_DPS_TO_RADS;
+ event->gyro.y *= SENSORS_DPS_TO_RADS;
+ event->gyro.z *= SENSORS_DPS_TO_RADS;
+}
+
+/**************************************************************************/
+/*!
+ @brief Gets the sensor_t data
+*/
+/**************************************************************************/
+void Adafruit_L3GD20_Unified::getSensor(sensor_t* sensor)
+{
+ /* Clear the sensor_t object */
+ memset(sensor, 0, sizeof(sensor_t));
+
+ /* Insert the sensor name in the fixed length char array */
+ strncpy (sensor->name, "L3GD20", sizeof(sensor->name) - 1);
+ sensor->name[sizeof(sensor->name)- 1] = 0;
+ sensor->version = 1;
+ sensor->sensor_id = _sensorID;
+ sensor->type = SENSOR_TYPE_GYROSCOPE;
+ sensor->min_delay = 0;
+ sensor->max_value = (float)this->_range * SENSORS_DPS_TO_RADS;
+ sensor->min_value = (this->_range * -1.0) * SENSORS_DPS_TO_RADS;
+ sensor->resolution = 0.0F; // TBD
+}
diff -r 000000000000 -r 772bf4786416 Source/Adafruit_LSM303_U.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Source/Adafruit_LSM303_U.cpp Sat Mar 21 12:33:05 2015 +0000
@@ -0,0 +1,416 @@
+/***************************************************************************
+ This is a library for the LSM303 Accelerometer and magnentometer/compass
+
+ Designed specifically to work with the Adafruit LSM303DLHC Breakout
+
+ These displays use I2C to communicate, 2 pins are required to interface.
+
+ Adafruit invests time and resources providing this open source code,
+ please support Adafruit andopen-source hardware by purchasing products
+ from Adafruit!
+
+ Written by Kevin Townsend for Adafruit Industries.
+ BSD license, all text above must be included in any redistribution
+ ***************************************************************************/
+
+
+#include "Adafruit_LSM303_U.h"
+#include "Serial_base.h"
+
+static float _lsm303Accel_MG_LSB = 0.001F; // 1, 2, 4 or 12 mg per lsb
+static float _lsm303Mag_Gauss_LSB_XY = 1100.0F; // Varies with gain
+static float _lsm303Mag_Gauss_LSB_Z = 980.0F; // Varies with gain
+
+/***************************************************************************
+ ACCELEROMETER
+ ***************************************************************************/
+/***************************************************************************
+ PRIVATE FUNCTIONS
+ ***************************************************************************/
+
+/**************************************************************************/
+/*!
+ @brief Abstract away platform differences in Arduino wire library
+*/
+/**************************************************************************/
+void Adafruit_LSM303_Accel_Unified::write8(byte address, byte reg, byte value)
+{
+ byte data[2] = {reg, value};
+ i2c->write(address,data,2);
+}
+
+/**************************************************************************/
+/*!
+ @brief Abstract away platform differences in Arduino wire library
+*/
+/**************************************************************************/
+byte Adafruit_LSM303_Accel_Unified::read8(byte address, byte reg)
+{
+ byte value;
+
+ i2c->writeByte(address, reg);
+ i2c->read(address, &value, 1);
+
+ return value;
+}
+
+/**************************************************************************/
+/*!
+ @brief Reads the raw data from the sensor
+*/
+/**************************************************************************/
+void Adafruit_LSM303_Accel_Unified::read()
+{
+ // Read the accelerometer
+ i2c->writeByte(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_OUT_X_L_A | 0x80);
+
+ byte data[6];
+ i2c->read(LSM303_ADDRESS_ACCEL, data, 6);
+
+ // Shift values to create properly formed integer (low byte first)
+ _accelData.x = (int16_t)(data[0] | (data[1] << 8)) >> 4;
+ _accelData.y = (int16_t)(data[2] | (data[3] << 8)) >> 4;
+ _accelData.z = (int16_t)(data[4] | (data[5] << 8)) >> 4;
+}
+
+/***************************************************************************
+ CONSTRUCTOR
+ ***************************************************************************/
+
+/**************************************************************************/
+/*!
+ @brief Instantiates a new Adafruit_LSM303 class
+*/
+/**************************************************************************/
+Adafruit_LSM303_Accel_Unified::Adafruit_LSM303_Accel_Unified(int32_t sensorID) {
+ _sensorID = sensorID;
+}
+
+/***************************************************************************
+ PUBLIC FUNCTIONS
+ ***************************************************************************/
+
+/**************************************************************************/
+/*!
+ @brief Setups the HW
+*/
+/**************************************************************************/
+bool Adafruit_LSM303_Accel_Unified::begin()
+{
+ // Enable I2C
+ // Enable the accelerometer (100Hz)
+ write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG1_A, 0x57);
+
+ // LSM303DLHC has no WHOAMI register so read CTRL_REG1_A back to check
+ // if we are connected or not
+ uint8_t reg1_a = read8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG1_A);
+ if (reg1_a != 0x57)
+ {
+ return false;
+ }
+
+ return true;
+}
+
+/**************************************************************************/
+/*!
+ @brief Gets the most recent sensor event
+*/
+/**************************************************************************/
+void Adafruit_LSM303_Accel_Unified::getEvent(sensors_event_t *event) {
+ /* Clear the event */
+ memset(event, 0, sizeof(sensors_event_t));
+
+ /* Read new data */
+ read();
+
+ event->version = sizeof(sensors_event_t);
+ event->sensor_id = _sensorID;
+ event->type = SENSOR_TYPE_ACCELEROMETER;
+ event->timestamp = millis();
+ event->acceleration.x = _accelData.x * _lsm303Accel_MG_LSB * SENSORS_GRAVITY_STANDARD;
+ event->acceleration.y = _accelData.y * _lsm303Accel_MG_LSB * SENSORS_GRAVITY_STANDARD;
+ event->acceleration.z = _accelData.z * _lsm303Accel_MG_LSB * SENSORS_GRAVITY_STANDARD;
+}
+
+/**************************************************************************/
+/*!
+ @brief Gets the sensor_t data
+*/
+/**************************************************************************/
+void Adafruit_LSM303_Accel_Unified::getSensor(sensor_t *sensor) {
+ /* Clear the sensor_t object */
+ memset(sensor, 0, sizeof(sensor_t));
+
+ /* Insert the sensor name in the fixed length char array */
+ strncpy (sensor->name, "LSM303", sizeof(sensor->name) - 1);
+ sensor->name[sizeof(sensor->name)- 1] = 0;
+ sensor->version = 1;
+ sensor->sensor_id = _sensorID;
+ sensor->type = SENSOR_TYPE_ACCELEROMETER;
+ sensor->min_delay = 0;
+ sensor->max_value = 0.0F; // TBD
+ sensor->min_value = 0.0F; // TBD
+ sensor->resolution = 0.0F; // TBD
+}
+
+/***************************************************************************
+ MAGNETOMETER
+ ***************************************************************************/
+/***************************************************************************
+ PRIVATE FUNCTIONS
+ ***************************************************************************/
+
+/**************************************************************************/
+/*!
+ @brief Abstract away platform differences in Arduino wire library
+*/
+/**************************************************************************/
+void Adafruit_LSM303_Mag_Unified::write8(byte address, byte reg, byte value)
+{
+ byte data[2] = {reg, value};
+ i2c->write(address,data,2);
+}
+
+/**************************************************************************/
+/*!
+ @brief Abstract away platform differences in Arduino wire library
+*/
+/**************************************************************************/
+byte Adafruit_LSM303_Mag_Unified::read8(byte address, byte reg)
+{
+ byte value;
+ i2c->writeByte(address, reg);
+ i2c->read(address, &value, 1);
+
+ return value;
+}
+
+/**************************************************************************/
+/*!
+ @brief Reads the raw data from the sensor
+*/
+/**************************************************************************/
+void Adafruit_LSM303_Mag_Unified::read()
+{
+ // Read the magnetometer
+ i2c->writeByte(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_OUT_X_H_M);
+
+ byte data[6];
+ i2c->read(LSM303_ADDRESS_MAG, data, 6);
+
+
+ // Shift values to create properly formed integer (low byte first)
+ _magData.x = (int16_t)(data[0] | ((int16_t)data[1] << 8));
+ _magData.y = (int16_t)(data[2] | ((int16_t)data[3] << 8));
+ _magData.z = (int16_t)(data[4] | ((int16_t)data[5] << 8));
+
+ // ToDo: Calculate orientation
+ _magData.orientation = 0.0;
+}
+
+/***************************************************************************
+ CONSTRUCTOR
+ ***************************************************************************/
+
+/**************************************************************************/
+/*!
+ @brief Instantiates a new Adafruit_LSM303 class
+*/
+/**************************************************************************/
+Adafruit_LSM303_Mag_Unified::Adafruit_LSM303_Mag_Unified(int32_t sensorID) {
+ _sensorID = sensorID;
+ _autoRangeEnabled = false;
+}
+
+/***************************************************************************
+ PUBLIC FUNCTIONS
+ ***************************************************************************/
+
+/**************************************************************************/
+/*!
+ @brief Setups the HW
+*/
+/**************************************************************************/
+bool Adafruit_LSM303_Mag_Unified::begin()
+{
+ // Enable I2C
+
+
+ // Enable the magnetometer
+ write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_MR_REG_M, 0x00);
+
+ // LSM303DLHC has no WHOAMI register so read CRA_REG_M to check
+ // the default value (0b00010000/0x10)
+ uint8_t reg1_a = read8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRA_REG_M);
+ if (reg1_a != 0x10)
+ {
+ return false;
+ }
+
+ // Set the gain to a known level
+ setMagGain(LSM303_MAGGAIN_1_3);
+
+ return true;
+}
+
+/**************************************************************************/
+/*!
+ @brief Enables or disables auto-ranging
+*/
+/**************************************************************************/
+void Adafruit_LSM303_Mag_Unified::enableAutoRange(bool enabled)
+{
+ _autoRangeEnabled = enabled;
+}
+
+/**************************************************************************/
+/*!
+ @brief Sets the magnetometer's gain
+*/
+/**************************************************************************/
+void Adafruit_LSM303_Mag_Unified::setMagGain(lsm303MagGain gain)
+{
+ write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRB_REG_M, (byte)gain);
+
+ _magGain = gain;
+
+ switch(gain)
+ {
+ case LSM303_MAGGAIN_1_3:
+ _lsm303Mag_Gauss_LSB_XY = 1100;
+ _lsm303Mag_Gauss_LSB_Z = 980;
+ break;
+ case LSM303_MAGGAIN_1_9:
+ _lsm303Mag_Gauss_LSB_XY = 855;
+ _lsm303Mag_Gauss_LSB_Z = 760;
+ break;
+ case LSM303_MAGGAIN_2_5:
+ _lsm303Mag_Gauss_LSB_XY = 670;
+ _lsm303Mag_Gauss_LSB_Z = 600;
+ break;
+ case LSM303_MAGGAIN_4_0:
+ _lsm303Mag_Gauss_LSB_XY = 450;
+ _lsm303Mag_Gauss_LSB_Z = 400;
+ break;
+ case LSM303_MAGGAIN_4_7:
+ _lsm303Mag_Gauss_LSB_XY = 400;
+ _lsm303Mag_Gauss_LSB_Z = 355;
+ break;
+ case LSM303_MAGGAIN_5_6:
+ _lsm303Mag_Gauss_LSB_XY = 330;
+ _lsm303Mag_Gauss_LSB_Z = 295;
+ break;
+ case LSM303_MAGGAIN_8_1:
+ _lsm303Mag_Gauss_LSB_XY = 230;
+ _lsm303Mag_Gauss_LSB_Z = 205;
+ break;
+ }
+}
+
+/**************************************************************************/
+/*!
+ @brief Gets the most recent sensor event
+*/
+/**************************************************************************/
+void Adafruit_LSM303_Mag_Unified::getEvent(sensors_event_t *event) {
+ bool readingValid = false;
+
+ /* Clear the event */
+ memset(event, 0, sizeof(sensors_event_t));
+
+ while(!readingValid)
+ {
+ /* Read new data */
+ read();
+
+ /* Make sure the sensor isn't saturating if auto-ranging is enabled */
+ if (!_autoRangeEnabled)
+ {
+ readingValid = true;
+ }
+ else
+ {
+ s_com->print(_magData.x); s_com->print(" ");
+ s_com->print(_magData.y); s_com->print(" ");
+ s_com->print(_magData.z); s_com->println(" ");
+ /* Check if the sensor is saturating or not */
+ if ( (_magData.x >= 2040) | (_magData.x <= -2040) |
+ (_magData.y >= 2040) | (_magData.y <= -2040) |
+ (_magData.z >= 2040) | (_magData.z <= -2040) )
+ {
+ /* Saturating .... increase the range if we can */
+ switch(_magGain)
+ {
+ case LSM303_MAGGAIN_5_6:
+ setMagGain(LSM303_MAGGAIN_8_1);
+ readingValid = false;
+ s_com->println("Changing range to +/- 8.1");
+ break;
+ case LSM303_MAGGAIN_4_7:
+ setMagGain(LSM303_MAGGAIN_5_6);
+ readingValid = false;
+ s_com->println("Changing range to +/- 5.6");
+ break;
+ case LSM303_MAGGAIN_4_0:
+ setMagGain(LSM303_MAGGAIN_4_7);
+ readingValid = false;
+ s_com->println("Changing range to +/- 4.7");
+ break;
+ case LSM303_MAGGAIN_2_5:
+ setMagGain(LSM303_MAGGAIN_4_0);
+ readingValid = false;
+ s_com->println("Changing range to +/- 4.0");
+ break;
+ case LSM303_MAGGAIN_1_9:
+ setMagGain(LSM303_MAGGAIN_2_5);
+ readingValid = false;
+ s_com->println("Changing range to +/- 2.5");
+ break;
+ case LSM303_MAGGAIN_1_3:
+ setMagGain(LSM303_MAGGAIN_1_9);
+ readingValid = false;
+ s_com->println("Changing range to +/- 1.9");
+ break;
+ default:
+ readingValid = true;
+ break;
+ }
+ }
+ else
+ {
+ /* All values are withing range */
+ readingValid = true;
+ }
+ }
+ }
+
+ event->version = sizeof(sensors_event_t);
+ event->sensor_id = _sensorID;
+ event->type = SENSOR_TYPE_MAGNETIC_FIELD;
+ event->timestamp = millis();
+ event->magnetic.x = _magData.x / _lsm303Mag_Gauss_LSB_XY * SENSORS_GAUSS_TO_MICROTESLA;
+ event->magnetic.y = _magData.y / _lsm303Mag_Gauss_LSB_XY * SENSORS_GAUSS_TO_MICROTESLA;
+ event->magnetic.z = _magData.z / _lsm303Mag_Gauss_LSB_Z * SENSORS_GAUSS_TO_MICROTESLA;
+}
+
+/**************************************************************************/
+/*!
+ @brief Gets the sensor_t data
+*/
+/**************************************************************************/
+void Adafruit_LSM303_Mag_Unified::getSensor(sensor_t *sensor) {
+ /* Clear the sensor_t object */
+ memset(sensor, 0, sizeof(sensor_t));
+
+ /* Insert the sensor name in the fixed length char array */
+ strncpy (sensor->name, "LSM303", sizeof(sensor->name) - 1);
+ sensor->name[sizeof(sensor->name)- 1] = 0;
+ sensor->version = 1;
+ sensor->sensor_id = _sensorID;
+ sensor->type = SENSOR_TYPE_MAGNETIC_FIELD;
+ sensor->min_delay = 0;
+ sensor->max_value = 0.0F; // TBD
+ sensor->min_value = 0.0F; // TBD
+ sensor->resolution = 0.0F; // TBD
+}
diff -r 000000000000 -r 772bf4786416 Source/Adafruit_Sensor.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Source/Adafruit_Sensor.cpp Sat Mar 21 12:33:05 2015 +0000
@@ -0,0 +1,5 @@
+#include "Adafruit_Sensor.h"
+
+
+void Adafruit_Sensor::constructor() {
+}
diff -r 000000000000 -r 772bf4786416 Source/I2C_base.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Source/I2C_base.cpp Sat Mar 21 12:33:05 2015 +0000
@@ -0,0 +1,16 @@
+#include "I2C_base.h"
+
+#ifdef _MBED_
+
+//setup the right i2c class
+I2C_base* i2c = new I2C_MBED(SDA, SCL);
+
+//setup millis function
+Timer p_t;
+int millis()
+{
+ return p_t.read_ms();
+}
+
+
+#endif //_MBED_
\ No newline at end of file
diff -r 000000000000 -r 772bf4786416 Source/Serial_base.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Source/Serial_base.cpp Sat Mar 21 12:33:05 2015 +0000 @@ -0,0 +1,8 @@ +#include "Serial_base.h" + +#ifdef _MBED_ + +//Setup MBED serial communication abstraction layer +Serial_base* s_com = new Serial_MBED(); + +#endif \ No newline at end of file
diff -r 000000000000 -r 772bf4786416 Source/main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Source/main.cpp Sat Mar 21 12:33:05 2015 +0000
@@ -0,0 +1,148 @@
+#include "mbed.h"
+#define _MBED_
+#include "Adafruit_9DOF.h"
+#include "Serial_base.h"
+
+
+
+/* Assign a unique ID to the sensors */
+
+Adafruit_9DOF dof = Adafruit_9DOF();
+
+Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(30301);
+
+Adafruit_LSM303_Mag_Unified mag = Adafruit_LSM303_Mag_Unified(30302);
+
+
+/* Update this with the correct SLP for accurate altitude measurements */
+
+float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;
+
+
+/**************************************************************************
+/
+/
+ @brief Initialises all the sensors used by this example
+
+/
+**************************************************************************/
+
+void initSensors()
+{
+
+ if(!accel.begin())
+ {
+
+ /* There was a problem detecting the LSM303 ... check your connections */
+
+ s_com->println(("Ooops, no LSM303 detected ... Check your wiring!"));
+
+ while(1);
+
+ }
+ if(!mag.begin())
+ {
+
+ /* There was a problem detecting the LSM303 ... check your connections */
+
+ s_com->println("Ooops, no LSM303 detected ... Check your wiring!");
+
+ while(1);
+
+ }
+
+}
+/**************************************************************************/
+
+
+
+/**************************************************************************/
+
+void setup(void)
+{
+
+
+
+ s_com->println(("Adafruit 9 DOF Pitch/Roll/Heading Example"));
+ s_com->println("");
+
+
+ /* Initialise the sensors */
+
+ initSensors();
+
+}
+
+
+/**************************************************************************
+/
+/
+ @brief Constantly check the roll/pitch/heading/altitude/temperature
+
+**************************************************************************/
+
+void loop(void)
+{
+
+ sensors_event_t accel_event;
+
+ sensors_event_t mag_event;
+
+ sensors_vec_t orientation;
+
+
+ /* Calculate pitch and roll from the raw accelerometer data */
+
+ accel.getEvent(&accel_event);
+
+ if (dof.accelGetOrientation(&accel_event, &orientation))
+ {
+
+ /* 'orientation' should have valid .roll and .pitch fields */
+
+ s_com->print(("Roll: "));
+
+ s_com->print(orientation.roll);
+
+ s_com->print(("; "));
+
+ s_com->print(("Pitch: "));
+
+
+ s_com->print(orientation.pitch);
+
+ s_com->print(("; "));
+
+ }
+
+
+ /* Calculate the heading using the magnetometer */
+
+ mag.getEvent(&mag_event);
+
+ if (dof.magGetOrientation(SENSOR_AXIS_Z, &mag_event, &orientation))
+ {
+
+ /* 'orientation' should have valid .heading data now */
+
+ s_com->print(("Heading: "));
+
+ s_com->print(orientation.heading);
+
+ s_com->print(("; "));
+
+ }
+
+
+ s_com->println((""));
+
+ wait(0.1);
+
+}
+
+int main()
+{
+ setup();
+ while(1)
+ loop();
+}
\ No newline at end of file
diff -r 000000000000 -r 772bf4786416 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sat Mar 21 12:33:05 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/9ad691361fac \ No newline at end of file