commit f2be3769efa6526e8876a69f8f4cfaf1bd291f84 Author: Justin Lewis Salmon Date: Sat Apr 23 15:46:24 2016 +0200 Initial commit of Julian's code diff --git a/Adafruit_10DOF.cpp b/Adafruit_10DOF.cpp new file mode 100644 index 0000000..97d1b34 --- /dev/null +++ b/Adafruit_10DOF.cpp @@ -0,0 +1,357 @@ +/*************************************************************************** + This is a library for the Adafruit 10DOF Breakout + + Designed specifically to work with the Adafruit 10DOF 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 + ***************************************************************************/ +#if ARDUINO >= 100 + #include "Arduino.h" +#else + #include "WProgram.h" +#endif + +#include +#include +#include + +#include "Adafruit_10DOF.h" + +#define PI (3.14159265F); + +/*************************************************************************** + PRIVATE FUNCTIONS + ***************************************************************************/ + + +/*************************************************************************** + CONSTRUCTOR + ***************************************************************************/ + +/**************************************************************************/ +/*! + @brief Instantiates a new Adafruit_10DOF class +*/ +/**************************************************************************/ +Adafruit_10DOF::Adafruit_10DOF(void) +{ +} + +/*************************************************************************** + PUBLIC FUNCTIONS + ***************************************************************************/ + +/**************************************************************************/ +/*! + @brief Setups the HW +*/ +/**************************************************************************/ +bool Adafruit_10DOF::begin() +{ + // Enable I2C + Wire.begin(); + + 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_10DOF::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 t_heading; + 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 */ + /* pitch = 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_10DOF::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_10DOF::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_10DOF::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 --git a/Adafruit_10DOF.h b/Adafruit_10DOF.h new file mode 100644 index 0000000..98e384e --- /dev/null +++ b/Adafruit_10DOF.h @@ -0,0 +1,53 @@ +/*************************************************************************** + This is a library for the Adafruit 10DOF Breakout + + Designed specifically to work with the Adafruit 10DOF 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 __ADAFRUIT_10DOF_H__ +#define __ADAFRUIT_10DOF_H__ + +#if (ARDUINO >= 100) + #include "Arduino.h" +#else + #include "WProgram.h" +#endif + +#include "Adafruit_Sensor.h" +#include "Adafruit_LSM303_U.h" +#include "Adafruit_BMP085_U.h" +#include "Adafruit_L3GD20_U.h" +#include + +/** Sensor axis */ +typedef enum +{ + SENSOR_AXIS_X = (1), + SENSOR_AXIS_Y = (2), + SENSOR_AXIS_Z = (3) +} sensors_axis_t; + +/* Driver for the the 10DOF breakout sensors */ +class Adafruit_10DOF +{ + public: + Adafruit_10DOF(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 --git a/Adafruit_BMP085_U.cpp b/Adafruit_BMP085_U.cpp new file mode 100644 index 0000000..9282ca5 --- /dev/null +++ b/Adafruit_BMP085_U.cpp @@ -0,0 +1,481 @@ +/*************************************************************************** + This is a library for the BMP085 pressure sensor + + Designed specifically to work with the Adafruit BMP085 or BMP180 Breakout + ----> http://www.adafruit.com/products/391 + ----> http://www.adafruit.com/products/1603 + + 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 + ***************************************************************************/ +#if ARDUINO >= 100 + #include "Arduino.h" +#else + #include "WProgram.h" +#endif + +#ifdef __AVR_ATtiny85__ + #include "TinyWireM.h" + #define Wire TinyWireM +#else + #include +#endif + +#include +#include + +#include "Adafruit_BMP085_U.h" + +static bmp085_calib_data _bmp085_coeffs; // Last read accelerometer data will be available here +static uint8_t _bmp085Mode; + +#define BMP085_USE_DATASHEET_VALS (0) /* Set to 1 for sanity check */ + +/*************************************************************************** + PRIVATE FUNCTIONS + ***************************************************************************/ + +/**************************************************************************/ +/*! + @brief Writes an 8 bit value over I2C +*/ +/**************************************************************************/ +static void writeCommand(byte reg, byte value) +{ + Wire.beginTransmission((uint8_t)BMP085_ADDRESS); + #if ARDUINO >= 100 + Wire.write((uint8_t)reg); + Wire.write((uint8_t)value); + #else + Wire.send(reg); + Wire.send(value); + #endif + Wire.endTransmission(); +} + +/**************************************************************************/ +/*! + @brief Reads an 8 bit value over I2C +*/ +/**************************************************************************/ +static void read8(byte reg, uint8_t *value) +{ + Wire.beginTransmission((uint8_t)BMP085_ADDRESS); + #if ARDUINO >= 100 + Wire.write((uint8_t)reg); + #else + Wire.send(reg); + #endif + Wire.endTransmission(); + Wire.requestFrom((uint8_t)BMP085_ADDRESS, (byte)1); + #if ARDUINO >= 100 + *value = Wire.read(); + #else + *value = Wire.receive(); + #endif + Wire.endTransmission(); +} + +/**************************************************************************/ +/*! + @brief Reads a 16 bit value over I2C +*/ +/**************************************************************************/ +static void read16(byte reg, uint16_t *value) +{ + Wire.beginTransmission((uint8_t)BMP085_ADDRESS); + #if ARDUINO >= 100 + Wire.write((uint8_t)reg); + #else + Wire.send(reg); + #endif + Wire.endTransmission(); + Wire.requestFrom((uint8_t)BMP085_ADDRESS, (byte)2); + #if ARDUINO >= 100 + *value = (Wire.read() << 8) | Wire.read(); + #else + *value = (Wire.receive() << 8) | Wire.receive(); + #endif + Wire.endTransmission(); +} + +/**************************************************************************/ +/*! + @brief Reads a signed 16 bit value over I2C +*/ +/**************************************************************************/ +static void readS16(byte reg, int16_t *value) +{ + uint16_t i; + read16(reg, &i); + *value = (int16_t)i; +} + +/**************************************************************************/ +/*! + @brief Reads the factory-set coefficients +*/ +/**************************************************************************/ +static void readCoefficients(void) +{ + #if BMP085_USE_DATASHEET_VALS + _bmp085_coeffs.ac1 = 408; + _bmp085_coeffs.ac2 = -72; + _bmp085_coeffs.ac3 = -14383; + _bmp085_coeffs.ac4 = 32741; + _bmp085_coeffs.ac5 = 32757; + _bmp085_coeffs.ac6 = 23153; + _bmp085_coeffs.b1 = 6190; + _bmp085_coeffs.b2 = 4; + _bmp085_coeffs.mb = -32768; + _bmp085_coeffs.mc = -8711; + _bmp085_coeffs.md = 2868; + _bmp085Mode = 0; + #else + readS16(BMP085_REGISTER_CAL_AC1, &_bmp085_coeffs.ac1); + readS16(BMP085_REGISTER_CAL_AC2, &_bmp085_coeffs.ac2); + readS16(BMP085_REGISTER_CAL_AC3, &_bmp085_coeffs.ac3); + read16(BMP085_REGISTER_CAL_AC4, &_bmp085_coeffs.ac4); + read16(BMP085_REGISTER_CAL_AC5, &_bmp085_coeffs.ac5); + read16(BMP085_REGISTER_CAL_AC6, &_bmp085_coeffs.ac6); + readS16(BMP085_REGISTER_CAL_B1, &_bmp085_coeffs.b1); + readS16(BMP085_REGISTER_CAL_B2, &_bmp085_coeffs.b2); + readS16(BMP085_REGISTER_CAL_MB, &_bmp085_coeffs.mb); + readS16(BMP085_REGISTER_CAL_MC, &_bmp085_coeffs.mc); + readS16(BMP085_REGISTER_CAL_MD, &_bmp085_coeffs.md); + #endif +} + +/**************************************************************************/ +/*! + +*/ +/**************************************************************************/ +static void readRawTemperature(int32_t *temperature) +{ + #if BMP085_USE_DATASHEET_VALS + *temperature = 27898; + #else + uint16_t t; + writeCommand(BMP085_REGISTER_CONTROL, BMP085_REGISTER_READTEMPCMD); + delay(5); + read16(BMP085_REGISTER_TEMPDATA, &t); + *temperature = t; + #endif +} + +/**************************************************************************/ +/*! + +*/ +/**************************************************************************/ +static void readRawPressure(int32_t *pressure) +{ + #if BMP085_USE_DATASHEET_VALS + *pressure = 23843; + #else + uint8_t p8; + uint16_t p16; + int32_t p32; + + writeCommand(BMP085_REGISTER_CONTROL, BMP085_REGISTER_READPRESSURECMD + (_bmp085Mode << 6)); + switch(_bmp085Mode) + { + case BMP085_MODE_ULTRALOWPOWER: + delay(5); + break; + case BMP085_MODE_STANDARD: + delay(8); + break; + case BMP085_MODE_HIGHRES: + delay(14); + break; + case BMP085_MODE_ULTRAHIGHRES: + default: + delay(26); + break; + } + + read16(BMP085_REGISTER_PRESSUREDATA, &p16); + p32 = (uint32_t)p16 << 8; + read8(BMP085_REGISTER_PRESSUREDATA+2, &p8); + p32 += p8; + p32 >>= (8 - _bmp085Mode); + + *pressure = p32; + #endif +} + +/**************************************************************************/ +/*! + @brief Compute B5 coefficient used in temperature & pressure calcs. +*/ +/**************************************************************************/ +int32_t Adafruit_BMP085_Unified::computeB5(int32_t ut) { + int32_t X1 = (ut - (int32_t)_bmp085_coeffs.ac6) * ((int32_t)_bmp085_coeffs.ac5) >> 15; + int32_t X2 = ((int32_t)_bmp085_coeffs.mc << 11) / (X1+(int32_t)_bmp085_coeffs.md); + return X1 + X2; +} + + +/*************************************************************************** + CONSTRUCTOR + ***************************************************************************/ + +/**************************************************************************/ +/*! + @brief Instantiates a new Adafruit_BMP085_Unified class +*/ +/**************************************************************************/ +Adafruit_BMP085_Unified::Adafruit_BMP085_Unified(int32_t sensorID) { + _sensorID = sensorID; +} + +/*************************************************************************** + PUBLIC FUNCTIONS + ***************************************************************************/ + +/**************************************************************************/ +/*! + @brief Setups the HW +*/ +/**************************************************************************/ +bool Adafruit_BMP085_Unified::begin(bmp085_mode_t mode) +{ + // Enable I2C + Wire.begin(); + + /* Mode boundary check */ + if ((mode > BMP085_MODE_ULTRAHIGHRES) || (mode < 0)) + { + mode = BMP085_MODE_ULTRAHIGHRES; + } + + /* Make sure we have the right device */ + uint8_t id; + read8(BMP085_REGISTER_CHIPID, &id); + if(id != 0x55) + { + return false; + } + + /* Set the mode indicator */ + _bmp085Mode = mode; + + /* Coefficients need to be read once */ + readCoefficients(); + + return true; +} + +/**************************************************************************/ +/*! + @brief Gets the compensated pressure level in kPa +*/ +/**************************************************************************/ +void Adafruit_BMP085_Unified::getPressure(float *pressure) +{ + int32_t ut = 0, up = 0, compp = 0; + int32_t x1, x2, b5, b6, x3, b3, p; + uint32_t b4, b7; + + /* Get the raw pressure and temperature values */ + readRawTemperature(&ut); + readRawPressure(&up); + + /* Temperature compensation */ + b5 = computeB5(ut); + + /* Pressure compensation */ + b6 = b5 - 4000; + x1 = (_bmp085_coeffs.b2 * ((b6 * b6) >> 12)) >> 11; + x2 = (_bmp085_coeffs.ac2 * b6) >> 11; + x3 = x1 + x2; + b3 = (((((int32_t) _bmp085_coeffs.ac1) * 4 + x3) << _bmp085Mode) + 2) >> 2; + x1 = (_bmp085_coeffs.ac3 * b6) >> 13; + x2 = (_bmp085_coeffs.b1 * ((b6 * b6) >> 12)) >> 16; + x3 = ((x1 + x2) + 2) >> 2; + b4 = (_bmp085_coeffs.ac4 * (uint32_t) (x3 + 32768)) >> 15; + b7 = ((uint32_t) (up - b3) * (50000 >> _bmp085Mode)); + + if (b7 < 0x80000000) + { + p = (b7 << 1) / b4; + } + else + { + p = (b7 / b4) << 1; + } + + x1 = (p >> 8) * (p >> 8); + x1 = (x1 * 3038) >> 16; + x2 = (-7357 * p) >> 16; + compp = p + ((x1 + x2 + 3791) >> 4); + + /* Assign compensated pressure value */ + *pressure = compp; +} + +/**************************************************************************/ +/*! + @brief Reads the temperatures in degrees Celsius +*/ +/**************************************************************************/ +void Adafruit_BMP085_Unified::getTemperature(float *temp) +{ + int32_t UT, X1, X2, B5; // following ds convention + float t; + + readRawTemperature(&UT); + + #if BMP085_USE_DATASHEET_VALS + // use datasheet numbers! + UT = 27898; + _bmp085_coeffs.ac6 = 23153; + _bmp085_coeffs.ac5 = 32757; + _bmp085_coeffs.mc = -8711; + _bmp085_coeffs.md = 2868; + #endif + + B5 = computeB5(UT); + t = (B5+8) >> 4; + t /= 10; + + *temp = t; +} + +/**************************************************************************/ +/*! + Calculates the altitude (in meters) from the specified atmospheric + pressure (in hPa), and sea-level pressure (in hPa). + + @param seaLevel Sea-level pressure in hPa + @param atmospheric Atmospheric pressure in hPa +*/ +/**************************************************************************/ +float Adafruit_BMP085_Unified::pressureToAltitude(float seaLevel, float atmospheric) +{ + // Equation taken from BMP180 datasheet (page 16): + // http://www.adafruit.com/datasheets/BST-BMP180-DS000-09.pdf + + // Note that using the equation from wikipedia can give bad results + // at high altitude. See this thread for more information: + // http://forums.adafruit.com/viewtopic.php?f=22&t=58064 + + return 44330.0 * (1.0 - pow(atmospheric / seaLevel, 0.1903)); +} + +/**************************************************************************/ +/*! + Calculates the altitude (in meters) from the specified atmospheric + pressure (in hPa), and sea-level pressure (in hPa). Note that this + function just calls the overload of pressureToAltitude which takes + seaLevel and atmospheric pressure--temperature is ignored. The original + implementation of this function was based on calculations from Wikipedia + which are not accurate at higher altitudes. To keep compatibility with + old code this function remains with the same interface, but it calls the + more accurate calculation. + + @param seaLevel Sea-level pressure in hPa + @param atmospheric Atmospheric pressure in hPa + @param temp Temperature in degrees Celsius +*/ +/**************************************************************************/ +float Adafruit_BMP085_Unified::pressureToAltitude(float seaLevel, float atmospheric, float temp) +{ + return pressureToAltitude(seaLevel, atmospheric); +} + +/**************************************************************************/ +/*! + Calculates the pressure at sea level (in hPa) from the specified altitude + (in meters), and atmospheric pressure (in hPa). + + @param altitude Altitude in meters + @param atmospheric Atmospheric pressure in hPa +*/ +/**************************************************************************/ +float Adafruit_BMP085_Unified::seaLevelForAltitude(float altitude, float atmospheric) +{ + // Equation taken from BMP180 datasheet (page 17): + // http://www.adafruit.com/datasheets/BST-BMP180-DS000-09.pdf + + // Note that using the equation from wikipedia can give bad results + // at high altitude. See this thread for more information: + // http://forums.adafruit.com/viewtopic.php?f=22&t=58064 + + return atmospheric / pow(1.0 - (altitude/44330.0), 5.255); +} + +/**************************************************************************/ +/*! + Calculates the pressure at sea level (in hPa) from the specified altitude + (in meters), and atmospheric pressure (in hPa). Note that this + function just calls the overload of seaLevelForAltitude which takes + altitude and atmospheric pressure--temperature is ignored. The original + implementation of this function was based on calculations from Wikipedia + which are not accurate at higher altitudes. To keep compatibility with + old code this function remains with the same interface, but it calls the + more accurate calculation. + + @param altitude Altitude in meters + @param atmospheric Atmospheric pressure in hPa + @param temp Temperature in degrees Celsius +*/ +/**************************************************************************/ +float Adafruit_BMP085_Unified::seaLevelForAltitude(float altitude, float atmospheric, float temp) +{ + return seaLevelForAltitude(altitude, atmospheric); +} + + + +/**************************************************************************/ +/*! + @brief Provides the sensor_t data for this sensor +*/ +/**************************************************************************/ +void Adafruit_BMP085_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, "BMP085", sizeof(sensor->name) - 1); + sensor->name[sizeof(sensor->name)- 1] = 0; + sensor->version = 1; + sensor->sensor_id = _sensorID; + sensor->type = SENSOR_TYPE_PRESSURE; + sensor->min_delay = 0; + sensor->max_value = 300.0F; // 300..1100 hPa + sensor->min_value = 1100.0F; + sensor->resolution = 0.01F; // Datasheet states 0.01 hPa resolution +} + +/**************************************************************************/ +/*! + @brief Reads the sensor and returns the data as a sensors_event_t +*/ +/**************************************************************************/ +bool Adafruit_BMP085_Unified::getEvent(sensors_event_t *event) +{ + float pressure_kPa; + + /* Clear the event */ + memset(event, 0, sizeof(sensors_event_t)); + + event->version = sizeof(sensors_event_t); + event->sensor_id = _sensorID; + event->type = SENSOR_TYPE_PRESSURE; + event->timestamp = 0; + getPressure(&pressure_kPa); + event->pressure = pressure_kPa / 100.0F; + + return true; +} diff --git a/Adafruit_BMP085_U.h b/Adafruit_BMP085_U.h new file mode 100644 index 0000000..0515e83 --- /dev/null +++ b/Adafruit_BMP085_U.h @@ -0,0 +1,122 @@ +/*************************************************************************** + This is a library for the BMP085 pressure sensor + + Designed specifically to work with the Adafruit BMP085 or BMP180 Breakout + ----> http://www.adafruit.com/products/391 + ----> http://www.adafruit.com/products/1603 + + 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 __BMP085_H__ +#define __BMP085_H__ + +#if (ARDUINO >= 100) + #include "Arduino.h" +#else + #include "WProgram.h" +#endif + +#include "Adafruit_Sensor.h" + +#ifdef __AVR_ATtiny85__ + #include "TinyWireM.h" + #define Wire TinyWireM +#else + #include +#endif + +/*========================================================================= + I2C ADDRESS/BITS + -----------------------------------------------------------------------*/ + #define BMP085_ADDRESS (0x77) +/*=========================================================================*/ + +/*========================================================================= + REGISTERS + -----------------------------------------------------------------------*/ + enum + { + BMP085_REGISTER_CAL_AC1 = 0xAA, // R Calibration data (16 bits) + BMP085_REGISTER_CAL_AC2 = 0xAC, // R Calibration data (16 bits) + BMP085_REGISTER_CAL_AC3 = 0xAE, // R Calibration data (16 bits) + BMP085_REGISTER_CAL_AC4 = 0xB0, // R Calibration data (16 bits) + BMP085_REGISTER_CAL_AC5 = 0xB2, // R Calibration data (16 bits) + BMP085_REGISTER_CAL_AC6 = 0xB4, // R Calibration data (16 bits) + BMP085_REGISTER_CAL_B1 = 0xB6, // R Calibration data (16 bits) + BMP085_REGISTER_CAL_B2 = 0xB8, // R Calibration data (16 bits) + BMP085_REGISTER_CAL_MB = 0xBA, // R Calibration data (16 bits) + BMP085_REGISTER_CAL_MC = 0xBC, // R Calibration data (16 bits) + BMP085_REGISTER_CAL_MD = 0xBE, // R Calibration data (16 bits) + BMP085_REGISTER_CHIPID = 0xD0, + BMP085_REGISTER_VERSION = 0xD1, + BMP085_REGISTER_SOFTRESET = 0xE0, + BMP085_REGISTER_CONTROL = 0xF4, + BMP085_REGISTER_TEMPDATA = 0xF6, + BMP085_REGISTER_PRESSUREDATA = 0xF6, + BMP085_REGISTER_READTEMPCMD = 0x2E, + BMP085_REGISTER_READPRESSURECMD = 0x34 + }; +/*=========================================================================*/ + +/*========================================================================= + MODE SETTINGS + -----------------------------------------------------------------------*/ + typedef enum + { + BMP085_MODE_ULTRALOWPOWER = 0, + BMP085_MODE_STANDARD = 1, + BMP085_MODE_HIGHRES = 2, + BMP085_MODE_ULTRAHIGHRES = 3 + } bmp085_mode_t; +/*=========================================================================*/ + +/*========================================================================= + CALIBRATION DATA + -----------------------------------------------------------------------*/ + typedef struct + { + int16_t ac1; + int16_t ac2; + int16_t ac3; + uint16_t ac4; + uint16_t ac5; + uint16_t ac6; + int16_t b1; + int16_t b2; + int16_t mb; + int16_t mc; + int16_t md; + } bmp085_calib_data; +/*=========================================================================*/ + +class Adafruit_BMP085_Unified : public Adafruit_Sensor +{ + public: + Adafruit_BMP085_Unified(int32_t sensorID = -1); + + bool begin(bmp085_mode_t mode = BMP085_MODE_ULTRAHIGHRES); + void getTemperature(float *temp); + void getPressure(float *pressure); + float pressureToAltitude(float seaLvel, float atmospheric); + float seaLevelForAltitude(float altitude, float atmospheric); + // Note that the next two functions are just for compatibility with old + // code that passed the temperature as a third parameter. A newer + // calculation is used which does not need temperature. + float pressureToAltitude(float seaLevel, float atmospheric, float temp); + float seaLevelForAltitude(float altitude, float atmospheric, float temp); + bool getEvent(sensors_event_t*); + void getSensor(sensor_t*); + + private: + int32_t computeB5(int32_t ut); + int32_t _sensorID; +}; + +#endif diff --git a/Adafruit_GPS.cpp b/Adafruit_GPS.cpp new file mode 100644 index 0000000..033f512 --- /dev/null +++ b/Adafruit_GPS.cpp @@ -0,0 +1,500 @@ +/*********************************** +This is our GPS library + +Adafruit invests time and resources providing this open source code, +please support Adafruit and open-source hardware by purchasing +products from Adafruit! + +Written by Limor Fried/Ladyada for Adafruit Industries. +BSD license, check license.txt for more information +All text above must be included in any redistribution +****************************************/ +#ifdef __AVR__ + // Only include software serial on AVR platforms (i.e. not on Due). + #include +#endif +#include "Adafruit_GPS.h" + +// how long are max NMEA lines to parse? +#define MAXLINELENGTH 120 + +// we double buffer: read one line in and leave one for the main program +volatile char line1[MAXLINELENGTH]; +volatile char line2[MAXLINELENGTH]; +// our index into filling the current line +volatile uint8_t lineidx=0; +// pointers to the double buffers +volatile char *currentline; +volatile char *lastline; +volatile boolean recvdflag; +volatile boolean inStandbyMode; + + +boolean Adafruit_GPS::parse(char *nmea) { + // do checksum check + + // first look if we even have one + if (nmea[strlen(nmea)-4] == '*') { + uint16_t sum = parseHex(nmea[strlen(nmea)-3]) * 16; + sum += parseHex(nmea[strlen(nmea)-2]); + + // check checksum + for (uint8_t i=1; i < (strlen(nmea)-4); i++) { + sum ^= nmea[i]; + } + if (sum != 0) { + // bad checksum :( + //return false; + } + } + int32_t degree; + long minutes; + char degreebuff[10]; + // look for a few common sentences + if (strstr(nmea, "$GPGGA")) { + // found GGA + char *p = nmea; + // get time + p = strchr(p, ',')+1; + float timef = atof(p); + uint32_t time = timef; + hour = time / 10000; + minute = (time % 10000) / 100; + seconds = (time % 100); + + milliseconds = fmod(timef, 1.0) * 1000; + + // parse out latitude + p = strchr(p, ',')+1; + if (',' != *p) + { + strncpy(degreebuff, p, 2); + p += 2; + degreebuff[2] = '\0'; + degree = atol(degreebuff) * 10000000; + strncpy(degreebuff, p, 2); // minutes + p += 3; // skip decimal point + strncpy(degreebuff + 2, p, 4); + degreebuff[6] = '\0'; + minutes = 50 * atol(degreebuff) / 3; + latitude_fixed = degree + minutes; + latitude = degree / 100000 + minutes * 0.000006F; + latitudeDegrees = (latitude-100*int(latitude/100))/60.0; + latitudeDegrees += int(latitude/100); + } + + p = strchr(p, ',')+1; + if (',' != *p) + { + if (p[0] == 'S') latitudeDegrees *= -1.0; + if (p[0] == 'N') lat = 'N'; + else if (p[0] == 'S') lat = 'S'; + else if (p[0] == ',') lat = 0; + else return false; + } + + // parse out longitude + p = strchr(p, ',')+1; + if (',' != *p) + { + strncpy(degreebuff, p, 3); + p += 3; + degreebuff[3] = '\0'; + degree = atol(degreebuff) * 10000000; + strncpy(degreebuff, p, 2); // minutes + p += 3; // skip decimal point + strncpy(degreebuff + 2, p, 4); + degreebuff[6] = '\0'; + minutes = 50 * atol(degreebuff) / 3; + longitude_fixed = degree + minutes; + longitude = degree / 100000 + minutes * 0.000006F; + longitudeDegrees = (longitude-100*int(longitude/100))/60.0; + longitudeDegrees += int(longitude/100); + } + + p = strchr(p, ',')+1; + if (',' != *p) + { + if (p[0] == 'W') longitudeDegrees *= -1.0; + if (p[0] == 'W') lon = 'W'; + else if (p[0] == 'E') lon = 'E'; + else if (p[0] == ',') lon = 0; + else return false; + } + + p = strchr(p, ',')+1; + if (',' != *p) + { + fixquality = atoi(p); + } + + p = strchr(p, ',')+1; + if (',' != *p) + { + satellites = atoi(p); + } + + p = strchr(p, ',')+1; + if (',' != *p) + { + HDOP = atof(p); + } + + p = strchr(p, ',')+1; + if (',' != *p) + { + altitude = atof(p); + } + + p = strchr(p, ',')+1; + p = strchr(p, ',')+1; + if (',' != *p) + { + geoidheight = atof(p); + } + return true; + } + if (strstr(nmea, "$GPRMC")) { + // found RMC + char *p = nmea; + + // get time + p = strchr(p, ',')+1; + float timef = atof(p); + uint32_t time = timef; + hour = time / 10000; + minute = (time % 10000) / 100; + seconds = (time % 100); + + milliseconds = fmod(timef, 1.0) * 1000; + + p = strchr(p, ',')+1; + // Serial.println(p); + if (p[0] == 'A') + fix = true; + else if (p[0] == 'V') + fix = false; + else + return false; + + // parse out latitude + p = strchr(p, ',')+1; + if (',' != *p) + { + strncpy(degreebuff, p, 2); + p += 2; + degreebuff[2] = '\0'; + long degree = atol(degreebuff) * 10000000; + strncpy(degreebuff, p, 2); // minutes + p += 3; // skip decimal point + strncpy(degreebuff + 2, p, 4); + degreebuff[6] = '\0'; + long minutes = 50 * atol(degreebuff) / 3; + latitude_fixed = degree + minutes; + latitude = degree / 100000 + minutes * 0.000006F; + latitudeDegrees = (latitude-100*int(latitude/100))/60.0; + latitudeDegrees += int(latitude/100); + } + + p = strchr(p, ',')+1; + if (',' != *p) + { + if (p[0] == 'S') latitudeDegrees *= -1.0; + if (p[0] == 'N') lat = 'N'; + else if (p[0] == 'S') lat = 'S'; + else if (p[0] == ',') lat = 0; + else return false; + } + + // parse out longitude + p = strchr(p, ',')+1; + if (',' != *p) + { + strncpy(degreebuff, p, 3); + p += 3; + degreebuff[3] = '\0'; + degree = atol(degreebuff) * 10000000; + strncpy(degreebuff, p, 2); // minutes + p += 3; // skip decimal point + strncpy(degreebuff + 2, p, 4); + degreebuff[6] = '\0'; + minutes = 50 * atol(degreebuff) / 3; + longitude_fixed = degree + minutes; + longitude = degree / 100000 + minutes * 0.000006F; + longitudeDegrees = (longitude-100*int(longitude/100))/60.0; + longitudeDegrees += int(longitude/100); + } + + p = strchr(p, ',')+1; + if (',' != *p) + { + if (p[0] == 'W') longitudeDegrees *= -1.0; + if (p[0] == 'W') lon = 'W'; + else if (p[0] == 'E') lon = 'E'; + else if (p[0] == ',') lon = 0; + else return false; + } + // speed + p = strchr(p, ',')+1; + if (',' != *p) + { + speed = atof(p); + } + + // angle + p = strchr(p, ',')+1; + if (',' != *p) + { + angle = atof(p); + } + + p = strchr(p, ',')+1; + if (',' != *p) + { + uint32_t fulldate = atof(p); + day = fulldate / 10000; + month = (fulldate % 10000) / 100; + year = (fulldate % 100); + } + // we dont parse the remaining, yet! + return true; + } + + return false; +} + +char Adafruit_GPS::read(void) { + char c = 0; + + if (paused) return c; + +#ifdef __AVR__ + if(gpsSwSerial) { + if(!gpsSwSerial->available()) return c; + c = gpsSwSerial->read(); + } else +#endif + { + if(!gpsHwSerial->available()) return c; + c = gpsHwSerial->read(); + } + + //Serial.print(c); + +// if (c == '$') { //please don't eat the dollar sign - rdl 9/15/14 +// currentline[lineidx] = 0; +// lineidx = 0; +// } + if (c == '\n') { + currentline[lineidx] = 0; + + if (currentline == line1) { + currentline = line2; + lastline = line1; + } else { + currentline = line1; + lastline = line2; + } + + //Serial.println("----"); + //Serial.println((char *)lastline); + //Serial.println("----"); + lineidx = 0; + recvdflag = true; + } + + currentline[lineidx++] = c; + if (lineidx >= MAXLINELENGTH) + lineidx = MAXLINELENGTH-1; + + return c; +} + +#ifdef __AVR__ +// Constructor when using SoftwareSerial or NewSoftSerial +#if ARDUINO >= 100 +Adafruit_GPS::Adafruit_GPS(SoftwareSerial *ser) +#else +Adafruit_GPS::Adafruit_GPS(NewSoftSerial *ser) +#endif +{ + common_init(); // Set everything to common state, then... + gpsSwSerial = ser; // ...override gpsSwSerial with value passed. +} +#endif + +// Constructor when using HardwareSerial +Adafruit_GPS::Adafruit_GPS(HardwareSerial *ser) { + common_init(); // Set everything to common state, then... + gpsHwSerial = ser; // ...override gpsHwSerial with value passed. +} + +// Initialization code used by all constructor types +void Adafruit_GPS::common_init(void) { +#ifdef __AVR__ + gpsSwSerial = NULL; // Set both to NULL, then override correct +#endif + gpsHwSerial = NULL; // port pointer in corresponding constructor + recvdflag = false; + paused = false; + lineidx = 0; + currentline = line1; + lastline = line2; + + hour = minute = seconds = year = month = day = + fixquality = satellites = 0; // uint8_t + lat = lon = mag = 0; // char + fix = false; // boolean + milliseconds = 0; // uint16_t + latitude = longitude = geoidheight = altitude = + speed = angle = magvariation = HDOP = 0.0; // float +} + +void Adafruit_GPS::begin(uint16_t baud) +{ +#ifdef __AVR__ + if(gpsSwSerial) + gpsSwSerial->begin(baud); + else + gpsHwSerial->begin(baud); +#endif + + delay(10); +} + +void Adafruit_GPS::sendCommand(const char *str) { +#ifdef __AVR__ + if(gpsSwSerial) + gpsSwSerial->println(str); + else +#endif + gpsHwSerial->println(str); +} + +boolean Adafruit_GPS::newNMEAreceived(void) { + return recvdflag; +} + +void Adafruit_GPS::pause(boolean p) { + paused = p; +} + +char *Adafruit_GPS::lastNMEA(void) { + recvdflag = false; + return (char *)lastline; +} + +// read a Hex value and return the decimal equivalent +uint8_t Adafruit_GPS::parseHex(char c) { + if (c < '0') + return 0; + if (c <= '9') + return c - '0'; + if (c < 'A') + return 0; + if (c <= 'F') + return (c - 'A')+10; + // if (c > 'F') + return 0; +} + +boolean Adafruit_GPS::waitForSentence(const char *wait4me, uint8_t max) { + char str[20]; + + uint8_t i=0; + while (i < max) { + if (newNMEAreceived()) { + char *nmea = lastNMEA(); + strncpy(str, nmea, 20); + str[19] = 0; + i++; + + if (strstr(str, wait4me)) + return true; + } + } + + return false; +} + +boolean Adafruit_GPS::LOCUS_StartLogger(void) { + sendCommand(PMTK_LOCUS_STARTLOG); + recvdflag = false; + return waitForSentence(PMTK_LOCUS_STARTSTOPACK); +} + +boolean Adafruit_GPS::LOCUS_StopLogger(void) { + sendCommand(PMTK_LOCUS_STOPLOG); + recvdflag = false; + return waitForSentence(PMTK_LOCUS_STARTSTOPACK); +} + +boolean Adafruit_GPS::LOCUS_ReadStatus(void) { + sendCommand(PMTK_LOCUS_QUERY_STATUS); + + if (! waitForSentence("$PMTKLOG")) + return false; + + char *response = lastNMEA(); + uint16_t parsed[10]; + uint8_t i; + + for (i=0; i<10; i++) parsed[i] = -1; + + response = strchr(response, ','); + for (i=0; i<10; i++) { + if (!response || (response[0] == 0) || (response[0] == '*')) + break; + response++; + parsed[i]=0; + while ((response[0] != ',') && + (response[0] != '*') && (response[0] != 0)) { + parsed[i] *= 10; + char c = response[0]; + if (isDigit(c)) + parsed[i] += c - '0'; + else + parsed[i] = c; + response++; + } + } + LOCUS_serial = parsed[0]; + LOCUS_type = parsed[1]; + if (isAlpha(parsed[2])) { + parsed[2] = parsed[2] - 'a' + 10; + } + LOCUS_mode = parsed[2]; + LOCUS_config = parsed[3]; + LOCUS_interval = parsed[4]; + LOCUS_distance = parsed[5]; + LOCUS_speed = parsed[6]; + LOCUS_status = !parsed[7]; + LOCUS_records = parsed[8]; + LOCUS_percent = parsed[9]; + + return true; +} + +// Standby Mode Switches +boolean Adafruit_GPS::standby(void) { + if (inStandbyMode) { + return false; // Returns false if already in standby mode, so that you do not wake it up by sending commands to GPS + } + else { + inStandbyMode = true; + sendCommand(PMTK_STANDBY); + //return waitForSentence(PMTK_STANDBY_SUCCESS); // don't seem to be fast enough to catch the message, or something else just is not working + return true; + } +} + +boolean Adafruit_GPS::wakeup(void) { + if (inStandbyMode) { + inStandbyMode = false; + sendCommand(""); // send byte to wake it up + return waitForSentence(PMTK_AWAKE); + } + else { + return false; // Returns false if not in standby mode, nothing to wakeup + } +} diff --git a/Adafruit_GPS.h b/Adafruit_GPS.h new file mode 100644 index 0000000..9ffd993 --- /dev/null +++ b/Adafruit_GPS.h @@ -0,0 +1,168 @@ +/*********************************** +This is the Adafruit GPS library - the ultimate GPS library +for the ultimate GPS module! + +Tested and works great with the Adafruit Ultimate GPS module +using MTK33x9 chipset + ------> http://www.adafruit.com/products/746 +Pick one up today at the Adafruit electronics shop +and help support open source hardware & software! -ada + +Adafruit invests time and resources providing this open source code, +please support Adafruit and open-source hardware by purchasing +products from Adafruit! + +Written by Limor Fried/Ladyada for Adafruit Industries. +BSD license, check license.txt for more information +All text above must be included in any redistribution +****************************************/ +// Fllybob added lines 34,35 and 40,41 to add 100mHz logging capability + +#ifndef _ADAFRUIT_GPS_H +#define _ADAFRUIT_GPS_H + +#ifdef __AVR__ + #if ARDUINO >= 100 + #include + #else + #include + #endif +#endif + +// different commands to set the update rate from once a second (1 Hz) to 10 times a second (10Hz) +// Note that these only control the rate at which the position is echoed, to actually speed up the +// position fix you must also send one of the position fix rate commands below too. +#define PMTK_SET_NMEA_UPDATE_100_MILLIHERTZ "$PMTK220,10000*2F" // Once every 10 seconds, 100 millihertz. +#define PMTK_SET_NMEA_UPDATE_200_MILLIHERTZ "$PMTK220,5000*1B" // Once every 5 seconds, 200 millihertz. +#define PMTK_SET_NMEA_UPDATE_1HZ "$PMTK220,1000*1F" +#define PMTK_SET_NMEA_UPDATE_5HZ "$PMTK220,200*2C" +#define PMTK_SET_NMEA_UPDATE_10HZ "$PMTK220,100*2F" +// Position fix update rate commands. +#define PMTK_API_SET_FIX_CTL_100_MILLIHERTZ "$PMTK300,10000,0,0,0,0*2C" // Once every 10 seconds, 100 millihertz. +#define PMTK_API_SET_FIX_CTL_200_MILLIHERTZ "$PMTK300,5000,0,0,0,0*18" // Once every 5 seconds, 200 millihertz. +#define PMTK_API_SET_FIX_CTL_1HZ "$PMTK300,1000,0,0,0,0*1C" +#define PMTK_API_SET_FIX_CTL_5HZ "$PMTK300,200,0,0,0,0*2F" +// Can't fix position faster than 5 times a second! + + +#define PMTK_SET_BAUD_57600 "$PMTK251,57600*2C" +#define PMTK_SET_BAUD_9600 "$PMTK251,9600*17" + +// turn on only the second sentence (GPRMC) +#define PMTK_SET_NMEA_OUTPUT_RMCONLY "$PMTK314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29" +// turn on GPRMC and GGA +#define PMTK_SET_NMEA_OUTPUT_RMCGGA "$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28" +// turn on ALL THE DATA +#define PMTK_SET_NMEA_OUTPUT_ALLDATA "$PMTK314,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28" +// turn off output +#define PMTK_SET_NMEA_OUTPUT_OFF "$PMTK314,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28" + +// to generate your own sentences, check out the MTK command datasheet and use a checksum calculator +// such as the awesome http://www.hhhh.org/wiml/proj/nmeaxor.html + +#define PMTK_LOCUS_STARTLOG "$PMTK185,0*22" +#define PMTK_LOCUS_STOPLOG "$PMTK185,1*23" +#define PMTK_LOCUS_STARTSTOPACK "$PMTK001,185,3*3C" +#define PMTK_LOCUS_QUERY_STATUS "$PMTK183*38" +#define PMTK_LOCUS_ERASE_FLASH "$PMTK184,1*22" +#define LOCUS_OVERLAP 0 +#define LOCUS_FULLSTOP 1 + +#define PMTK_ENABLE_SBAS "$PMTK313,1*2E" +#define PMTK_ENABLE_WAAS "$PMTK301,2*2E" + +// standby command & boot successful message +#define PMTK_STANDBY "$PMTK161,0*28" +#define PMTK_STANDBY_SUCCESS "$PMTK001,161,3*36" // Not needed currently +#define PMTK_AWAKE "$PMTK010,002*2D" + +// ask for the release and version +#define PMTK_Q_RELEASE "$PMTK605*31" + +// request for updates on antenna status +#define PGCMD_ANTENNA "$PGCMD,33,1*6C" +#define PGCMD_NOANTENNA "$PGCMD,33,0*6D" + +// how long to wait when we're looking for a response +#define MAXWAITSENTENCE 5 + +#if ARDUINO >= 100 + #include "Arduino.h" +#if defined (__AVR__) && !defined(__AVR_ATmega32U4__) + #include "SoftwareSerial.h" +#endif +#else + #include "WProgram.h" + #include "NewSoftSerial.h" +#endif + + +class Adafruit_GPS { + public: + void begin(uint16_t baud); + +#ifdef __AVR__ + #if ARDUINO >= 100 + Adafruit_GPS(SoftwareSerial *ser); // Constructor when using SoftwareSerial + #else + Adafruit_GPS(NewSoftSerial *ser); // Constructor when using NewSoftSerial + #endif +#endif + Adafruit_GPS(HardwareSerial *ser); // Constructor when using HardwareSerial + + char *lastNMEA(void); + boolean newNMEAreceived(); + void common_init(void); + + void sendCommand(const char *); + + void pause(boolean b); + + boolean parseNMEA(char *response); + uint8_t parseHex(char c); + + char read(void); + boolean parse(char *); + void interruptReads(boolean r); + + boolean wakeup(void); + boolean standby(void); + + uint8_t hour, minute, seconds, year, month, day; + uint16_t milliseconds; + // Floating point latitude and longitude value in degrees. + float latitude, longitude; + // Fixed point latitude and longitude value with degrees stored in units of 1/100000 degrees, + // and minutes stored in units of 1/100000 degrees. See pull #13 for more details: + // https://github.com/adafruit/Adafruit-GPS-Library/pull/13 + int32_t latitude_fixed, longitude_fixed; + float latitudeDegrees, longitudeDegrees; + float geoidheight, altitude; + float speed, angle, magvariation, HDOP; + char lat, lon, mag; + boolean fix; + uint8_t fixquality, satellites; + + boolean waitForSentence(const char *wait, uint8_t max = MAXWAITSENTENCE); + boolean LOCUS_StartLogger(void); + boolean LOCUS_StopLogger(void); + boolean LOCUS_ReadStatus(void); + + uint16_t LOCUS_serial, LOCUS_records; + uint8_t LOCUS_type, LOCUS_mode, LOCUS_config, LOCUS_interval, LOCUS_distance, LOCUS_speed, LOCUS_status, LOCUS_percent; + private: + boolean paused; + + uint8_t parseResponse(char *response); +#ifdef __AVR__ + #if ARDUINO >= 100 + SoftwareSerial *gpsSwSerial; + #else + NewSoftSerial *gpsSwSerial; + #endif +#endif + HardwareSerial *gpsHwSerial; +}; + + +#endif diff --git a/Adafruit_HTU21DF.cpp b/Adafruit_HTU21DF.cpp new file mode 100644 index 0000000..c1ba562 --- /dev/null +++ b/Adafruit_HTU21DF.cpp @@ -0,0 +1,98 @@ +/*************************************************** + This is a library for the HTU21DF Humidity & Temp Sensor + + Designed specifically to work with the HTU21DF sensor from Adafruit + ----> https://www.adafruit.com/products/1899 + + 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 and open-source hardware by purchasing + products from Adafruit! + + Written by Limor Fried/Ladyada for Adafruit Industries. + BSD license, all text above must be included in any redistribution + ****************************************************/ + +#include "Adafruit_HTU21DF.h" +//#include + +Adafruit_HTU21DF::Adafruit_HTU21DF() { +} + + +boolean Adafruit_HTU21DF::begin(void) { + Wire1.begin(); + + reset(); + + Wire1.beginTransmission(HTU21DF_I2CADDR); + Wire1.write(HTU21DF_READREG); + Wire1.endTransmission(); + Wire1.requestFrom(HTU21DF_I2CADDR, 1); + return (Wire1.read() == 0x2); // after reset should be 0x2 +} + +void Adafruit_HTU21DF::reset(void) { + Wire1.beginTransmission(HTU21DF_I2CADDR); + Wire1.write(HTU21DF_RESET); + Wire1.endTransmission(); + delay(15); +} + + +float Adafruit_HTU21DF::readTemperature(void) { + + // OK lets ready! + Wire1.beginTransmission(HTU21DF_I2CADDR); + Wire1.write(HTU21DF_READTEMP); + Wire1.endTransmission(); + + delay(50); // add delay between request and actual read! + + Wire1.requestFrom(HTU21DF_I2CADDR, 3); + while (!Wire1.available()) {} + + uint16_t t = Wire1.read(); + t <<= 8; + t |= Wire1.read(); + + uint8_t crc = Wire1.read(); + + float temp = t; + temp *= 175.72; + temp /= 65536; + temp -= 46.85; + + return temp; +} + + +float Adafruit_HTU21DF::readHumidity(void) { + // OK lets ready! + Wire1.beginTransmission(HTU21DF_I2CADDR); + Wire1.write(HTU21DF_READHUM); + Wire1.endTransmission(); + + delay(50); // add delay between request and actual read! + + Wire1.requestFrom(HTU21DF_I2CADDR, 3); + while (!Wire1.available()) {} + + uint16_t h = Wire1.read(); + h <<= 8; + h |= Wire1.read(); + + uint8_t crc = Wire1.read(); + + float hum = h; + hum *= 125; + hum /= 65536; + hum -= 6; + + return hum; +} + + + +/*********************************************************************/ diff --git a/Adafruit_HTU21DF.h b/Adafruit_HTU21DF.h new file mode 100644 index 0000000..b7d8574 --- /dev/null +++ b/Adafruit_HTU21DF.h @@ -0,0 +1,45 @@ +/*************************************************** + This is a library for the HTU21D-F Humidity & Temp Sensor + + Designed specifically to work with the HTU21D-F sensor from Adafruit + ----> https://www.adafruit.com/products/1899 + + 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 and open-source hardware by purchasing + products from Adafruit! + + Written by Limor Fried/Ladyada for Adafruit Industries. + BSD license, all text above must be included in any redistribution + ****************************************************/ + +#if (ARDUINO >= 100) + #include "Arduino.h" +#else + #include "WProgram.h" +#endif +#include "Wire.h" +extern TwoWire Wire1; + +#define HTU21DF_I2CADDR 0x40 +#define HTU21DF_READTEMP 0xE3 +#define HTU21DF_READHUM 0xE5 +#define HTU21DF_WRITEREG 0xE6 +#define HTU21DF_READREG 0xE7 +#define HTU21DF_RESET 0xFE + + + +class Adafruit_HTU21DF { + public: + Adafruit_HTU21DF(); + boolean begin(void); + float readTemperature(void); + float readHumidity(void); + void reset(void); + private: + boolean readData(void); + float humidity, temp; +}; + diff --git a/Adafruit_L3GD20_U.cpp b/Adafruit_L3GD20_U.cpp new file mode 100644 index 0000000..38dfef3 --- /dev/null +++ b/Adafruit_L3GD20_U.cpp @@ -0,0 +1,364 @@ +/*************************************************** + 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 + ****************************************************/ +#if ARDUINO >= 100 + #include "Arduino.h" +#else + #include "WProgram.h" +#endif + +#include +#include + +#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) +{ + Wire.beginTransmission(L3GD20_ADDRESS); + #if ARDUINO >= 100 + Wire.write((uint8_t)reg); + Wire.write((uint8_t)value); + #else + Wire.send(reg); + Wire.send(value); + #endif + Wire.endTransmission(); +} + +/**************************************************************************/ +/*! + @brief Abstract away platform differences in Arduino wire library +*/ +/**************************************************************************/ +byte Adafruit_L3GD20_Unified::read8(byte reg) +{ + byte value; + + Wire.beginTransmission((byte)L3GD20_ADDRESS); + #if ARDUINO >= 100 + Wire.write((uint8_t)reg); + #else + Wire.send(reg); + #endif + Wire.endTransmission(); + Wire.requestFrom((byte)L3GD20_ADDRESS, (byte)1); + while (!Wire.available()); // Wait for data to arrive. + #if ARDUINO >= 100 + value = Wire.read(); + #else + value = Wire.receive(); + #endif + Wire.endTransmission(); + + 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) +{ + /* Enable I2C */ + Wire.begin(); + + /* 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 +*/ +/**************************************************************************/ +bool 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(); + + /* Read 6 bytes from the sensor */ + Wire.beginTransmission((byte)L3GD20_ADDRESS); + #if ARDUINO >= 100 + Wire.write(GYRO_REGISTER_OUT_X_L | 0x80); + #else + Wire.send(GYRO_REGISTER_OUT_X_L | 0x80); + #endif + Wire.endTransmission(); + Wire.requestFrom((byte)L3GD20_ADDRESS, (byte)6); + + /* Wait around until enough data is available */ + while (Wire.available() < 6); + + #if ARDUINO >= 100 + uint8_t xlo = Wire.read(); + uint8_t xhi = Wire.read(); + uint8_t ylo = Wire.read(); + uint8_t yhi = Wire.read(); + uint8_t zlo = Wire.read(); + uint8_t zhi = Wire.read(); + #else + uint8_t xlo = Wire.receive(); + uint8_t xhi = Wire.receive(); + uint8_t ylo = Wire.receive(); + uint8_t yhi = Wire.receive(); + uint8_t zlo = Wire.receive(); + uint8_t zhi = Wire.receive(); + #endif + + /* Shift values to create properly formed integer (low byte first) */ + event->gyro.x = (int16_t)(xlo | (xhi << 8)); + event->gyro.y = (int16_t)(ylo | (yhi << 8)); + event->gyro.z = (int16_t)(zlo | (zhi << 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; + + return true; +} + +/**************************************************************************/ +/*! + @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 --git a/Adafruit_L3GD20_U.h b/Adafruit_L3GD20_U.h new file mode 100644 index 0000000..192b0b1 --- /dev/null +++ b/Adafruit_L3GD20_U.h @@ -0,0 +1,104 @@ +/*************************************************** + 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__ + +#if (ARDUINO >= 100) + #include "Arduino.h" +#else + #include "WProgram.h" +#endif + +#include "Adafruit_Sensor.h" +#include + +/*========================================================================= + 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 ); + bool 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 --git a/Adafruit_LSM303_U.cpp b/Adafruit_LSM303_U.cpp new file mode 100644 index 0000000..52505b3 --- /dev/null +++ b/Adafruit_LSM303_U.cpp @@ -0,0 +1,569 @@ +/*************************************************************************** + 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 + ***************************************************************************/ +#if ARDUINO >= 100 + #include "Arduino.h" +#else + #include "WProgram.h" +#endif + +#ifdef __AVR_ATtiny85__ + #include "TinyWireM.h" + #define Wire TinyWireM +#else + #include +#endif + +#include + +#include "Adafruit_LSM303_U.h" + +/* enabling this #define will enable the debug print blocks +#define LSM303_DEBUG +*/ + +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) +{ + Wire.beginTransmission(address); + #if ARDUINO >= 100 + Wire.write((uint8_t)reg); + Wire.write((uint8_t)value); + #else + Wire.send(reg); + Wire.send(value); + #endif + Wire.endTransmission(); +} + +/**************************************************************************/ +/*! + @brief Abstract away platform differences in Arduino wire library +*/ +/**************************************************************************/ +byte Adafruit_LSM303_Accel_Unified::read8(byte address, byte reg) +{ + byte value; + + Wire.beginTransmission(address); + #if ARDUINO >= 100 + Wire.write((uint8_t)reg); + #else + Wire.send(reg); + #endif + Wire.endTransmission(); + Wire.requestFrom(address, (byte)1); + #if ARDUINO >= 100 + value = Wire.read(); + #else + value = Wire.receive(); + #endif + Wire.endTransmission(); + + return value; +} + +/**************************************************************************/ +/*! + @brief Reads the raw data from the sensor +*/ +/**************************************************************************/ +void Adafruit_LSM303_Accel_Unified::read() +{ + // Read the accelerometer + Wire.beginTransmission((byte)LSM303_ADDRESS_ACCEL); + #if ARDUINO >= 100 + Wire.write(LSM303_REGISTER_ACCEL_OUT_X_L_A | 0x80); + #else + Wire.send(LSM303_REGISTER_ACCEL_OUT_X_L_A | 0x80); + #endif + Wire.endTransmission(); + Wire.requestFrom((byte)LSM303_ADDRESS_ACCEL, (byte)6); + + // Wait around until enough data is available + while (Wire.available() < 6); + + #if ARDUINO >= 100 + uint8_t xlo = Wire.read(); + uint8_t xhi = Wire.read(); + uint8_t ylo = Wire.read(); + uint8_t yhi = Wire.read(); + uint8_t zlo = Wire.read(); + uint8_t zhi = Wire.read(); + #else + uint8_t xlo = Wire.receive(); + uint8_t xhi = Wire.receive(); + uint8_t ylo = Wire.receive(); + uint8_t yhi = Wire.receive(); + uint8_t zlo = Wire.receive(); + uint8_t zhi = Wire.receive(); + #endif + + // Shift values to create properly formed integer (low byte first) + _accelData.x = (int16_t)(xlo | (xhi << 8)) >> 4; + _accelData.y = (int16_t)(ylo | (yhi << 8)) >> 4; + _accelData.z = (int16_t)(zlo | (zhi << 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 + Wire.begin(); + + // 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 +*/ +/**************************************************************************/ +bool 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; + + return true; +} + +/**************************************************************************/ +/*! + @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) +{ + Wire.beginTransmission(address); + #if ARDUINO >= 100 + Wire.write((uint8_t)reg); + Wire.write((uint8_t)value); + #else + Wire.send(reg); + Wire.send(value); + #endif + Wire.endTransmission(); +} + +/**************************************************************************/ +/*! + @brief Abstract away platform differences in Arduino wire library +*/ +/**************************************************************************/ +byte Adafruit_LSM303_Mag_Unified::read8(byte address, byte reg) +{ + byte value; + + Wire.beginTransmission(address); + #if ARDUINO >= 100 + Wire.write((uint8_t)reg); + #else + Wire.send(reg); + #endif + Wire.endTransmission(); + Wire.requestFrom(address, (byte)1); + #if ARDUINO >= 100 + value = Wire.read(); + #else + value = Wire.receive(); + #endif + Wire.endTransmission(); + + return value; +} + +/**************************************************************************/ +/*! + @brief Reads the raw data from the sensor +*/ +/**************************************************************************/ +void Adafruit_LSM303_Mag_Unified::read() +{ + // Read the magnetometer + Wire.beginTransmission((byte)LSM303_ADDRESS_MAG); + #if ARDUINO >= 100 + Wire.write(LSM303_REGISTER_MAG_OUT_X_H_M); + #else + Wire.send(LSM303_REGISTER_MAG_OUT_X_H_M); + #endif + Wire.endTransmission(); + Wire.requestFrom((byte)LSM303_ADDRESS_MAG, (byte)6); + + // Wait around until enough data is available + while (Wire.available() < 6); + + // Note high before low (different than accel) + #if ARDUINO >= 100 + uint8_t xhi = Wire.read(); + uint8_t xlo = Wire.read(); + uint8_t zhi = Wire.read(); + uint8_t zlo = Wire.read(); + uint8_t yhi = Wire.read(); + uint8_t ylo = Wire.read(); + #else + uint8_t xhi = Wire.receive(); + uint8_t xlo = Wire.receive(); + uint8_t zhi = Wire.receive(); + uint8_t zlo = Wire.receive(); + uint8_t yhi = Wire.receive(); + uint8_t ylo = Wire.receive(); + #endif + + // Shift values to create properly formed integer (low byte first) + _magData.x = (int16_t)(xlo | ((int16_t)xhi << 8)); + _magData.y = (int16_t)(ylo | ((int16_t)yhi << 8)); + _magData.z = (int16_t)(zlo | ((int16_t)zhi << 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 + Wire.begin(); + + // Enable the magnetometer + write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_MR_REG_M, 0x00); + +#ifdef RUBBISH + + // 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; + +#else + uint8_t ira=0, irb=0, irc=0; + ira = read8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_IRA_REG_M); + irb = read8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_IRB_REG_M); + irc = read8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_IRC_REG_M); + if ((ira = 0x48) && (irb == 0x34) && (irc == 0x33)) { + setMagGain(LSM303_MAGGAIN_1_3); + return true; + } + return false; +#endif +} + +/**************************************************************************/ +/*! + @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 Sets the magnetometer's update rate +*/ +/**************************************************************************/ +void Adafruit_LSM303_Mag_Unified::setMagRate(lsm303MagRate rate) +{ + byte reg_m = ((byte)rate & 0x07) << 2; + write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRA_REG_M, reg_m); +} + + +/**************************************************************************/ +/*! + @brief Gets the most recent sensor event +*/ +/**************************************************************************/ +bool Adafruit_LSM303_Mag_Unified::getEvent(sensors_event_t *event) { + bool readingValid = false; + + /* Clear the event */ + memset(event, 0, sizeof(sensors_event_t)); + + while(!readingValid) + { + + uint8_t reg_mg = read8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_SR_REG_M); + if (!(reg_mg & 0x1)) { + return false; + } + + /* Read new data */ + read(); + + /* Make sure the sensor isn't saturating if auto-ranging is enabled */ + if (!_autoRangeEnabled) + { + readingValid = true; + } + else + { +#ifdef LSM303_DEBUG + Serial.print(_magData.x); Serial.print(" "); + Serial.print(_magData.y); Serial.print(" "); + Serial.print(_magData.z); Serial.println(" "); +#endif + /* 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; +#ifdef LSM303_DEBUG + Serial.println("Changing range to +/- 8.1"); +#endif + break; + case LSM303_MAGGAIN_4_7: + setMagGain(LSM303_MAGGAIN_5_6); + readingValid = false; +#ifdef LSM303_DEBUG + Serial.println("Changing range to +/- 5.6"); +#endif + break; + case LSM303_MAGGAIN_4_0: + setMagGain(LSM303_MAGGAIN_4_7); + readingValid = false; +#ifdef LSM303_DEBUG + Serial.println("Changing range to +/- 4.7"); +#endif + break; + case LSM303_MAGGAIN_2_5: + setMagGain(LSM303_MAGGAIN_4_0); + readingValid = false; +#ifdef LSM303_DEBUG + Serial.println("Changing range to +/- 4.0"); +#endif + break; + case LSM303_MAGGAIN_1_9: + setMagGain(LSM303_MAGGAIN_2_5); + readingValid = false; +#ifdef LSM303_DEBUG + Serial.println("Changing range to +/- 2.5"); +#endif + break; + case LSM303_MAGGAIN_1_3: + setMagGain(LSM303_MAGGAIN_1_9); + readingValid = false; +#ifdef LSM303_DEBUG + Serial.println("Changing range to +/- 1.9"); +#endif + 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; + + return true; +} + +/**************************************************************************/ +/*! + @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 --git a/Adafruit_LSM303_U.h b/Adafruit_LSM303_U.h new file mode 100644 index 0000000..b7cb1ac --- /dev/null +++ b/Adafruit_LSM303_U.h @@ -0,0 +1,200 @@ +/*************************************************************************** + 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__ + +#if (ARDUINO >= 100) + #include "Arduino.h" +#else + #include "WProgram.h" +#endif + +#include "Adafruit_Sensor.h" +#ifdef __AVR_ATtiny85__ + #include "TinyWireM.h" + #define Wire TinyWireM +#else + #include +#endif + + +/*========================================================================= + 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_M = 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; +/*=========================================================================*/ + +/*========================================================================= + MAGNETOMETER UPDATE RATE SETTINGS + -----------------------------------------------------------------------*/ + typedef enum + { + LSM303_MAGRATE_0_7 = 0x00, // 0.75 Hz + LSM303_MAGRATE_1_5 = 0x01, // 1.5 Hz + LSM303_MAGRATE_3_0 = 0x62, // 3.0 Hz + LSM303_MAGRATE_7_5 = 0x03, // 7.5 Hz + LSM303_MAGRATE_15 = 0x04, // 15 Hz + LSM303_MAGRATE_30 = 0x05, // 30 Hz + LSM303_MAGRATE_75 = 0x06, // 75 Hz + LSM303_MAGRATE_220 = 0x07 // 200 Hz + } lsm303MagRate; +/*=========================================================================*/ + +/*========================================================================= + INTERNAL MAGNETOMETER DATA TYPE + -----------------------------------------------------------------------*/ + typedef struct lsm303MagData_s + { + float x; + float y; + float z; + } 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); + bool getEvent(sensors_event_t*); + void getSensor(sensor_t*); + + void write8(byte address, byte reg, byte value); + byte read8(byte address, byte reg); + void read(void); + + private: + lsm303AccelData _accelData; // Last read accelerometer data will be available here + int32_t _sensorID; +}; + +/* 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 setMagRate(lsm303MagRate rate); + bool getEvent(sensors_event_t*); + void getSensor(sensor_t*); + + void write8(byte address, byte reg, byte value); + byte read8(byte address, byte reg); + void read(void); + + private: + lsm303MagGain _magGain; + lsm303MagData _magData; // Last read magnetometer data will be available here + int32_t _sensorID; + bool _autoRangeEnabled; + +}; + +#endif diff --git a/Adafruit_Sensor.cpp b/Adafruit_Sensor.cpp new file mode 100644 index 0000000..ee59b27 --- /dev/null +++ b/Adafruit_Sensor.cpp @@ -0,0 +1,2 @@ +#include "Adafruit_Sensor.h" +#include diff --git a/Adafruit_Sensor.h b/Adafruit_Sensor.h new file mode 100644 index 0000000..7742afc --- /dev/null +++ b/Adafruit_Sensor.h @@ -0,0 +1,154 @@ +/* +* 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. +*/ + +/* 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 + +#if ARDUINO >= 100 + #include "Arduino.h" + #include "Print.h" +#else + #include "WProgram.h" +#endif + +/* 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) + Adafruit_Sensor() {} + virtual ~Adafruit_Sensor() {} + + // These must be defined by the subclass + virtual void enableAutoRange(bool enabled) {}; + virtual bool getEvent(sensors_event_t*) = 0; + virtual void getSensor(sensor_t*) = 0; + + private: + bool _autoRange; +}; + +#endif diff --git a/cosmicpi-arduino.cc b/cosmicpi-arduino.cc new file mode 120000 index 0000000..d71f80f --- /dev/null +++ b/cosmicpi-arduino.cc @@ -0,0 +1 @@ +cosmicpi-arduino.ino \ No newline at end of file diff --git a/cosmicpi-arduino.ino b/cosmicpi-arduino.ino new file mode 100644 index 0000000..0061d1d --- /dev/null +++ b/cosmicpi-arduino.ino @@ -0,0 +1,1104 @@ +// This program collects up to PPS_EVENTS events each second into a double buffer. +// While the ISR fills one event buffer, the user space loop function reads from the other. +// Each PPS interrupt the read/write buffers are swapped over by the event ISR +// If any events are available in the read buffer the loop function puts them onto a queue +// along with the UTC time string where they get stored. Once there are at +// least DUMP_THRESHOLD entries on the queue, the loop function outputs them over the serial +// line for processing. + +// In this version interrupts come from the accelerometer chip, if the acceleration exceeds +// the threshold (in meters/sec/sec) in any direction the chip interrupts and its logged to +// the serial output stream. + +// Julian Lewis lewis.julian@gmail.com + +#define VERS "2016/Mar/29" + +// In this sketch I am using an Adafruite ultimate GPS breakout which exposes the PPS output +// The Addafruite Rx is connected to the DUE TX1 (Pin 18) and its Tx to DUE RX1 (Pin 19) +// The Adafruite 3.3V power is provided from the DUE 3.3V and ground pins +// N.B. Go to the Adafruit downloads page and copy Adafruit_GPS.h and Adafruit_GPS.cc to +// your sketch directory. + +// The output from this program is processed by a Python monitor on the other end of the +// serial line. There has to be mutual aggreement between this program and the monitor. + +// Output strings +// All fields in all output strings conform to the json standard + +// Here is the list of all records where 'f' denotes float and 'i' denotes integer ... +// {'HTU':{'Tmh':f,'Hum':f}} +// HTU21DF record containing Tmh:temperature in C Hum:humidity percent +// +// {'BMP':{'Tmb':f,'Prs':f,'Alb':f}} +// BMP085 record containing Tmb:temperature Prs:pressure Alb:Barrometric altitude +// +// {'VIB':{'Vax':i,'Vcn':i}} +// Vibration record containing Vax:3 bit xyz direction mask Vcn:vibration count +// This record is always immediatly followed by 3 more records, TIM, ACL, and MAG +// +// {'MAG':{'Mgx':f,'Mgy':f,'Mgz':f}} +// LSM303DLH magnatometer record containing Mgx:the x field strength Mgy:the y field Mgz:ther z field +// +// {'MOG':{'Mox':f,'Moy':f,'Moz':f}} +// LSM303DLH magnatometer record containing Mox:x orientation Moy:y orientation Moz:z orientation +// This record is optional, by default its turned off (it can always be calculated later - Python) +// +// {'ACL':{'Acx':f,'Acy':f,'Acz':f}} +// LSM303DLH acclerometer record containing Acx:the x acceleration Acy:the y acceleration Acz:the z acceleration +// If this record immediatly follows a VIB record the fields were hardware latched when the g threshold was exceeded +// +// {'AOL':{'Aox':f,'Aoy':f,'Aoz':f}} +// LSM303DLH accelerometer record containing Aox:x orientation Aoy:y orientation Aoz:z orientation +// This record is optional, by default its turned off (it can always be calculated later - Python) +// +// {'LOC':{'Lat':f,'Lon':f,'Alt':f}} +// GPS location record containing Lat:latitude in degrees Lon:longitude in degrees Alt:altitude in meters +// +// {'TIM':{'Upt':i,'Frq':i,'Sec':i}} +// Time record containing Upt:up time seconds Frq:counter frequency Sec:time string +// +// {'STS':{'Qsz':i,'Mis':i,'Ter':i,'Htu':i,'Bmp':i,'Acl':i,'Mag':i}} +// Status record containing Qsz:events on queue Mis:missed events Ter:buffer error Htu:status Bmp:status Acl:status Mag:status +// +// {'EVT':{'Evt':i,'Frq':i,'Tks':i,'Etm':f,'Adc':[[i,i,i,i,i,i,i,i][i,i,i,i,i,i,i,i]]}} +// Event record containing Evt:event number in second Frq:timer frequency Tks:ticks since last event in second +// Etm:event time stamp to 100ns Adc:[[Channel 0 values][Channel 1 values]] + +// N.B. These records pass the data to a python monitor over the serial line. Python has awsome string handling and looks them up in +// associative arrays to build records of any arbitary format you want. So this is only the start of the story of record processing. +// N.B. Also some of these records are sent out at regular intervals and or when an event occurs. + +// This program also accepts commands sent to it on the serial line. +// When a command arrives it is immediatly executed. + +#include +#include + +#include "Adafruit_BMP085_U.h" // Barrometric pressure + +#include "Adafruit_HTU21DF.h" // Humidity and temperature sensor + +// GPS chips typically return NMEA strings over a serial line. +// They can be programmed to send different NMEA strings according to what you configure. +// The string RMCGGA has the altitude but misses the yy/mm/dd from the date, it only has hh/mm/ss. +// This is easilly made up for in the Python monitor which gets this information from it system time. + +#include "Adafruit_GPS.h" // GPS chip +#define GPSECHO true +#define RMCGGA // Altitude on, yy/mm/dd off + +#include "Adafruit_L3GD20_U.h" // Magoscope + +// WARNING: I had to modify this library, its no longer standard +#include "Adafruit_LSM303_U.h" // Accelerometer and magnentometer/compass + +#include "Adafruit_10DOF.h" // 10DOF breakout driver - scale to SI units + +// Configuration constants + +// The size of the one second event buffer +#define PPS_EVENTS 8 // The maximum number of events stored per second +#define ADC_BUF_LEN 8 // Number of ADC values per event + +// This is the event queue size +#define EVENT_QSIZE 32 // The number of events that can be queued for serial output + +// Handle text buffer serial output overflow errors +// When the output buffer overflows due to data comming too fast, we just stop printing due +// to insufficient bandwidth or slow things down if HANDLE_OVERFLOW is set (not recomended) +// #define HANDLE_OVERFLOW + +// This is the text ring buffer for real time output to serial line with interrupt on +#define TBLEN 4096 // Serial line output ring buffer size + +// Define some output debug pins to monitor whats going on via an oscilloscope +#define PPS_PIN 13 // PPS (Pulse Per Second) and LED +#define EVT_PIN 12 // Cosmic ray event detected +#define FLG_PIN 11 // Debug flag + +// For siesmic event input +#define ACL_PIN 10 // Accelarometer INT1 interrupt pin + +// Baud rates +#define SERIAL_BAUD_RATE 9600 // Serial line +#define GPS_BAUD_RATE 9600 // GPS and Serial1 line + +// Instantiate external hardware breakouts + +Adafruit_GPS gps(&Serial1); // GPS Serial1 on pins RX1 and TX1 + +Adafruit_HTU21DF htu = Adafruit_HTU21DF(); // Humidity and temperature measurment +boolean htu_ok = false; // Chip OK + +#define BMPID 18001 +Adafruit_BMP085_Unified bmp = Adafruit_BMP085_Unified(BMPID); // Barometric pressure +boolean bmp_ok = false; + +// The 10DOF isn't a chip, its just a utility to convert say mago values into headings etc + +Adafruit_10DOF dof = Adafruit_10DOF(); // The 10 Degrees-Of-Freedom DOF breakout +boolean dof_ok = false; // board driver, scales units to SI + +#define ACLID 30301 +Adafruit_LSM303_Accel_Unified acl = Adafruit_LSM303_Accel_Unified(ACLID); // Accelerometer Compass + boolean acl_ok = false; + +#define MAGID 30302 +Adafruit_LSM303_Mag_Unified mag = Adafruit_LSM303_Mag_Unified(MAGID); // Magoscope +boolean mag_ok = false; + +// Control the output data rates by setting defaults, these values can be modified at run time +// via commands from the serial interface. Some output like position isn't supposed to be changing +// very fast if at all, so no need to clutter up the serial line with it. The Python monitor keeps +// the last sent values when it builds event messages to be sent over the internet to the server +// or logged to a file. + +uint32_t latlon_display_rate = 12; // Display latitude and longitude each X seconds +uint32_t humtmp_display_rate = 12; // Display humidity and HTU temperature each X seconds +uint32_t alttmp_display_rate = 12; // Display altitude and BMP temperature each X seconds +uint32_t frqutc_display_rate = 1; // Display frequency and UTC time each X seconds +uint32_t status_display_rate = 4; // Display status (UpTime, QueueSize, MissedEvents, HardwareOK) +uint32_t accelr_display_rate = 1; // Display accelarometer x,y,z +uint32_t magnot_display_rate = 12; // Display magnotometer data x,y,z + +uint32_t events_display_size = 20; // Display events after recieving X events + +// Siesmic event trigger parameters + +uint32_t accelr_event_threshold = 2; // Trigger level for siesmic events in milli-g +uint32_t accelr_event_cutoff_fr = 30; // Siesmic event cutoff frequency + +// Commands can be sent over the serial line to configure the display rates or whatever + +typedef enum { + NOOP, // No-operation + HELP, // Help + HTUX, // Reset HTU chip + HTUD, // HUT display rate + BMPD, // BMP display rate + LOCD, // Location display rate + TIMD, // Timing display rate + STSD, // Status display rate + EVQT, // Event queue dump threshold + + ACLD, // Accelerometer display rate + MAGD, // Magnetometer display rate + + ACLT, // Accelerometer event threshold + + CMDS }; // Command count + +typedef struct { + int Id; // Command ID number + void (*proc)(int arg); // Function to call + char *Name; // Command name + char *Help; // Command help text + int Par; // Command parameter flag +} CmdStruct; + +// Function forward references + +void noop(int arg); +void help(int arg); +void htux(int arg); +void htud(int arg); +void bmpd(int arg); +void locd(int arg); +void timd(int arg); +void stsd(int arg); +void evqt(int arg); +void acld(int arg); +void magd(int arg); +void aclt(int arg); + +// Command table + +CmdStruct cmd_table[CMDS] = { + { NOOP, noop, "NOOP", "Do nothing", 0 }, + { HELP, help, "HELP", "Display commands", 0 }, + { HTUX, htux, "HTUX", "Reset the HTU chip", 0 }, + { HTUD, htud, "HTUD", "HTU Temperature-Humidity display rate", 1 }, + { BMPD, bmpd, "BMPD", "BMP Temperature-Altitude display rate", 1 }, + { LOCD, locd, "LOCD", "Location latitude-longitude display rate", 1 }, + { TIMD, timd, "TIMD", "Timing uptime-frequency-utc display rate", 1 }, + { STSD, stsd, "STSD", "Status info display rate", 1 }, + { EVQT, evqt, "EVQT", "Event queue dump threshold", 1 }, + { ACLD, acld, "ACLD", "Accelerometer display rate", 1 }, + { MAGD, magd, "MAGD", "Magnatometer display rate", 1 }, + { ACLT, aclt, "ACLT", "Accelerometer event trigger threshold", 1 } +}; + +#define CMDLEN 32 +static char cmd[CMDLEN]; // Command input buffer +static int irdp=0, irdy=0, istp=0; // Read, ready, stop + +static char txtb[TBLEN]; // Text ring buffer +static uint32_t txtw = 0, txtr = 0, // Write and Read indexes + tsze = 0, terr = 0; // Buffer size and error code + +typedef enum { TXT_NOERR=0, TXT_TOOBIG=1, TXT_OVERFL=2 } TxtErr; + +#define TXTLEN 256 +static char txt[TXTLEN]; // For writing to serial + +// Initialize the timer chips to measure time between the PPS pulses and the EVENT pulse +// The PPS enters pin D2, the PPS is forwarded accross an isolating diode to pin D5 +// The event pulse is also connected to pin D5. So D5 sees the LOR of the PPS and the +// event, while D2 sees only the PPS. In this way we measure the frequency of the +// clock MCLK/2 each second on the first counter, and the time between EVENTs on the second + +void TimersStart() { + + uint32_t config = 0; + + // Set up the power management controller for TC0 and TC2 + + pmc_set_writeprotect(false); // Enable write access to power management chip + pmc_enable_periph_clk(ID_TC0); // Turn on power for timer block 0 channel 0 + pmc_enable_periph_clk(ID_TC6); // Turn on power for timer block 2 channel 0 + + // Timer block zero channel zero is connected only to the PPS + // We set it up to load regester RA on each PPS and reset + // So RA will contain the number of clock ticks between two PPS, this + // value should be very stable +/- one tick + + config = TC_CMR_TCCLKS_TIMER_CLOCK1 | // Select fast clock MCK/2 = 42 MHz + TC_CMR_ETRGEDG_RISING | // External trigger rising edge on TIOA0 + TC_CMR_ABETRG | // Use the TIOA external input line + TC_CMR_LDRA_RISING; // Latch counter value into RA + + TC_Configure(TC0, 0, config); // Configure channel 0 of TC0 + TC_Start(TC0, 0); // Start timer running + + TC0->TC_CHANNEL[0].TC_IER = TC_IER_LDRAS; // Enable the load AR channel 0 interrupt each PPS + TC0->TC_CHANNEL[0].TC_IDR = ~TC_IER_LDRAS; // and disable the rest of the interrupt sources + NVIC_EnableIRQ(TC0_IRQn); // Enable interrupt handler for channel 0 + + // Timer block 2 channel zero is connected to the OR of the PPS and the RAY event + + config = TC_CMR_TCCLKS_TIMER_CLOCK1 | // Select fast clock MCK/2 = 42 MHz + TC_CMR_ETRGEDG_RISING | // External trigger rising edge on TIOA1 + TC_CMR_ABETRG | // Use the TIOA external input line + TC_CMR_LDRA_RISING; // Latch counter value into RA + + TC_Configure(TC2, 0, config); // Configure channel 0 of TC2 + TC_Start(TC2, 0); // Start timer running + + TC2->TC_CHANNEL[0].TC_IER = TC_IER_LDRAS; // Enable the load AR channel 0 interrupt each PPS + TC2->TC_CHANNEL[0].TC_IDR = ~TC_IER_LDRAS; // and disable the rest of the interrupt sources + NVIC_EnableIRQ(TC6_IRQn); // Enable interrupt handler for channel 0 + + // Set up the PIO controller to route input pins for TC0 and TC2 + + PIO_Configure(PIOC,PIO_INPUT, + PIO_PB25B_TIOA0, // D2 Input + PIO_DEFAULT); + + PIO_Configure(PIOC,PIO_INPUT, + PIO_PC25B_TIOA6, // D5 Input + PIO_DEFAULT); +} + +// Timer chip interrupt handlers try to get time stamps to within 4 system clock ticks + +static uint32_t displ = 0; // Display values in loop + +static uint32_t ppsfl = LOW, // PPS Flag boolean + rega0 = 0, // RA reg + stsr0 = 0, // Interrupt status register + ppcnt = 0; // PPS count + +// Handle the PPS interrupt in counter block 0 ISR + +void TC0_Handler() { + + // This ISR is connected only to the PPS (Pulse Per Second) GPS event + // Each time this runs, set the flag to tell the TC6 ISR we have seen it + // This logic only works if the TC0 handler gets called before the TC6 handler + // hence the debug flag which I look at with a scope to be sure. + // I may introduce a small delay line to ensure this is true, so far it is. + + ppsfl = HIGH; // Seen a rising edge on the PPS +#if FLG_PIN + digitalWrite(FLG_PIN,ppsfl); // Flag set (for debug) +#endif + rega0 = TC0->TC_CHANNEL[0].TC_RA; // Read the RA reg (PPS period) + stsr0 = TC_GetStatus(TC0, 0); // Read status and clear load bits + + ppcnt++; // PPS count + displ = 1; // Display stuff in the loop +} + +// We need a double buffer, one is being written by the ISR while +// the other is read from user space within one second. + +struct Event { + uint16_t Ch0[ADC_BUF_LEN]; // ADC channel 0 values + uint16_t Ch1[ADC_BUF_LEN]; // ADC channel 1 values + uint32_t Tks; // Time since last event in ticks +}; + +static struct Event b1[PPS_EVENTS]; // Event ticks buffeer +static struct Event b2[PPS_EVENTS]; // Event ticks buffer +static struct Event *wbuf = b1; // Write event buffer pointer and its index +static struct Event *rbuf = b2; // Read event buffer pointer and its index +static int ridx, widx; + +// We also need a time value for the current and previous second + +#ifdef RMCGGA +#define DATE_TIME_LEN 9 +#else +#define DATE_TIME_LEN 17 +#endif + +static char t1[DATE_TIME_LEN]; // Date time buffer text string +static char t2[DATE_TIME_LEN]; +static char *wdtm = t1; // Write date/time pointer +static char *rdtm = t2; // Read date/time pointer + +// Swap read write event buffers and indexes along with their time strings +// each second, so we have the current and previous second numbers + +void SwapBufs() { + struct Event *tbuf; // Temp event buf pointer + char *tdtm; // Temp date/time string pointer + tbuf = rbuf; rbuf = wbuf; wbuf = tbuf; // Swap write with read buffer + ridx = widx; widx = 0; // Write count to read, reset the write count + tdtm = rdtm; rdtm = wdtm; wdtm = tdtm; // And swap asociated buffer date/time +} + +// Handle isolated PPS (via diode) LOR with the Event +// The diode is needed to block Event pulses getting back to TC0 +// LOR means Logical inclusive OR + +static uint32_t rega1, stsr1 = 0; + +void TC6_Handler() { + + // This ISR is connected to the OR of the event and the PPS + // If the TC0 has seen the PPS it sets the flag high + // and if its high we are seeing the PPS here, but if the + // flag is not set then this is a cosmic ray event. + + if (ppsfl == HIGH) { // Was ther a PPS ? + ppsfl = LOW; // Yes so we have seen it here + SwapBufs(); // Every PPS swap the read/write buffers +#if EVT_PIN + digitalWrite(EVT_PIN,LOW); // Not an event +#endif + } else { +#if EVT_PIN + digitalWrite(EVT_PIN,HIGH); // Event detected +#endif + if (widx < PPS_EVENTS) { // Up to PPS_EVENTS stored per PPS + + // Read the latched tick count getting the event time + // and then pull the ADC pipe line + + wbuf[widx].Tks = TC2->TC_CHANNEL[0].TC_RA; + AdcPullData(&wbuf[widx]); + widx++; + } + } +#if FLG_PIN + digitalWrite(FLG_PIN,ppsfl); // Flag out +#endif + rega1 = TC2->TC_CHANNEL[0].TC_RA; // Read the RA on channel 1 (PPS period) + stsr1 = TC_GetStatus(TC2, 0); // Read status clear load bits +} + +// Accelerometer setup +// These settings come from reading the LSM303DLH doccumentation, which is more than the +// author of the Adda_fruit library did. It is a badly written load of rubbish, I have +// been forced to correct some bugs and export some private methods. It was a touch and go +// descision wether to just not use the library at all and do it all here. For now I will +// use it with the bug corrections. + +// The following setup makes the accelarometer compare G-forces against a threshold value, and +// latch the output registers until they are read. To avoid excessive interrupt rates the high +// pass filter has been configured to keep the frequency low. + +// N.B. It took me a day to find out that I needed to use Active low and Open drain on the INT1 +// signal, otherwise the Adda_fruit module wont pass it on. Beware !!! + +void AclSetup() { + uint8_t tmp, val; + + if (!acl_ok) return; + +#define PMD 0x20 // Normal power mode (PM0=1,PM1=0:Normal) +#define DRT 0x00 // Data rate 50 Hz (0x08 = 100Hz) +#define AEN 0x07 // XYZ Enabled + + val = PMD | DRT | AEN; + acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG1_A, val); + +#define HPE1 0x04 // High pass filter Int 1 on +#define HPCF 0x03 // High pass cut off frequency + + val = HPE1 | HPCF; + acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG2_A, val); + +#define LIR1 0x06 // Latch Int1 bit Data ready +#define LIR2 0x00 // Latch Int2 bit Data ready (0x20 Latch On) + +#define IHL_OD 0xC0 // Interrupt active low, open drain (Argh !!!) + + val = LIR1 | LIR2 | IHL_OD; + acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG3_A, val); + +#define BDU_FS 0x80 // Block data and scale +-2g + + val = BDU_FS; + acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG4_A, val); + +#define XYZ_HI 0x2A // Hi values ZHIE YHIE XHIE +#define AOI_6D 0x00 // 0xC0 would enable 6 directions + + val = XYZ_HI | AOI_6D; + acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_INT1_CFG_A, val); + + val = accelr_event_threshold & 0x7F; + acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_INT1_THS_A, val); + + // The chip make very wide pulses (100ms), the values on the rising and + // falling edges are different ! + + attachInterrupt(digitalPinToInterrupt(ACL_PIN),Acl_ISR,RISING); +} + +// Magnatometer setup, again the Adda_fruit library was inadequate. + +void MagSetup() { + uint8_t val; + + if (!mag_ok) return; + + val = 0; + mag.write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRA_REG_M, val); + +#define GAIN 0x80 // +- 4.0 Gauss + + val = GAIN; + mag.write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRB_REG_M, val); + +#define MODE 0x0 // 01=Single conversion mode + + val = MODE; + mag.write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_MR_REG_M, val); +} + +// This Accelerometer ISR + +static uint32_t accl_icount = 0, accl_flag = 0; + +void Acl_ISR() { + +#define IA 0x40 + + accl_icount++; + accl_flag = AclReadStatus(); + PushVib(); +} + +// Read accelerometer status +// This just reads the interrupt source INT1 and the overrun status. +// It returns 1 bit for X, Y, or Z (0..7) if the threshold value is exceeded. +// This determins if the board is being shaken - Earth quake - or other reason + +static uint8_t acl_sts = 0; +static uint8_t acl_src = 0; + +uint8_t AclReadStatus() { + uint8_t rval; + + acl_src = acl.read8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_INT1_SOURCE_A); + +#define ZH 0x20 // Z High +#define YH 0x08 // Y High +#define XH 0x02 // X High + + rval = 0; + if (acl_src & IA) { + if (acl_src & ZH) rval |= 4; + if (acl_src & YH) rval |= 2; + if (acl_src & XH) rval |= 1; + } + if (rval) + acl_sts = acl.read8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_STATUS_REG_A); + return rval; +} + +// Set up the ADC channels + +void AdcSetup() { + REG_ADC_MR = 0x10380080; // Free run as fast as you can + REG_ADC_CHER = 3; // Channels 0 and 1 +} + +// Pull all data (16 values) from the ADC into a buffer + +uint8_t AdcPullData(struct Event *b) { + + int i; + + for (i=0; iADC_ISR & 0x01)==0); // Wait for channel 0 (2.5us) + b->Ch0[i] = (uint16_t) ADC->ADC_CDR[0]; // Read ch 0 + while((ADC->ADC_ISR & 0x02)==0); // Wait for channel 1 (2.5us) + b->Ch1[i] = (uint16_t) ADC->ADC_CDR[1]; // Read ch 1 + } +} + +// This is the nmea data string from the GPS chip + +#define GPS_STRING_LEN 256 +static char gps_string[GPS_STRING_LEN + 1]; + +float latitude = 0.0, longitude = 0.0, altitude = 0.0; + +// This function is dependent on the GPS chip implementation +// It should return a date time string as described above +// So you need to re-implements this for whichever chip you are using +// Here I am using the addafruit GPS chip + +char *GetDateTime() { + + int i = 0; + while (Serial1.available()) { + if (i < GPS_STRING_LEN) { + gps_string[i++] = (char) Serial1.read(); + gps_string[i] = 0; + } else i++; + } + if (gps.parse(gps_string)) { + +#ifdef RMCGGA + // I choose RMCGGA by default, and get the altitude but no date. + // Its easy to get the date once the records arrive at the Python end. + // The GPS altitude is far more accurate than the barrometric altitude. + // Warning: The syntax can not be changed, we need an integer hhmmss + + sprintf(wdtm,"%02d%02d%02d", + gps.hour,gps.minute,gps.seconds); + + altitude = gps.altitude; +#else + sprintf(wdtm,"%02d%02d%02d%02d%02d%02d%02d%02d", + gps.year,gps.month,gps.day, + gps.hour,gps.minute,gps.seconds); + + altitude = 0; +#endif + latitude = gps.latitudeDegrees; // Easy place to get location + longitude = gps.longitudeDegrees; // Works well in Google maps + + return rdtm; + } else + return NULL; +} + +// Implement queue access mechanism for events, each second the user space (loop) copies +// any events it has read onto the queue + +struct EventBuf { + char DateTime[DATE_TIME_LEN]; // The date and time string + uint32_t Frequency; // The current clock frequency + uint32_t Ticks; // The number of ticks since the last event or PPS if none + uint16_t Ch0[ADC_BUF_LEN]; // ADC channel 0 values + uint16_t Ch1[ADC_BUF_LEN]; // ADC channel 1 values + uint8_t Count; // The number of events since the PPS +}; + +typedef struct { + uint8_t Size; // Current size of the queue + uint8_t RdPtr; // Read pointer + uint8_t WrPtr; // Write pointer + uint8_t Missed; // Missed events counter due to overflow + uint8_t Lock; // The queue spin lock (not needed here) + struct EventBuf Events[EVENT_QSIZE]; // Queued events +} EventQueue; + +static EventQueue event_queue; + +// Put an event in an EventBuf on the queue, if the queue is full then the oldest event +// is thrown away and the "missed" event count is incremented + +uint8_t PutQueue(struct EventBuf *ebuf) { + + EventQueue *q = &event_queue; + + while(q->Lock) {}; q->Lock = 1; // Spin lock on queue + q->Events[q->WrPtr] = *ebuf; // Write event to the queue + q->WrPtr = (q->WrPtr + 1) % EVENT_QSIZE;// Increment the write pointer + if (q->Size < EVENT_QSIZE) q->Size++; // If we are overwriting old enties that havnt been read + else { + q->Missed++; // Say we missed some events + q->RdPtr = (q->RdPtr + 1) % EVENT_QSIZE; // and throw the oldest event away + } + q->Lock = 0; + return q->Missed; +} + +// Pop an event off the queue, if the queue is empty nothing happens +// the queue size is zero when the queue is empty, and this is the +// return value + +uint8_t PopQueue(struct EventBuf *ebuf) { // Points to where the caller wants the event stored + + EventQueue *q = &event_queue; + + while(q->Lock) {}; q->Lock = 1; // Spin lock on queue + if (q->Size) { + *ebuf = q->Events[q->RdPtr]; + q->RdPtr = (q->RdPtr + 1) % EVENT_QSIZE; + q->Size--; + } + q->Lock = 0; + return q->Size; +} + +// Get the size of the queue + +uint8_t SzeQueue() { + + EventQueue *q = &event_queue; + + return q->Size; +} + +// Initialize the queue + +void InitQueue() { + + EventQueue *q = &event_queue; + + q->Lock = 1; + q->Size = 0; + q->RdPtr = 0; + q->WrPtr = 0; + q->Missed = 0; + q->Lock = 0; +} + +// Arduino setup function, initialize hardware and software +// This is the first function to be called when the sketch is started + +void setup() { + +#if FLG_PIN + pinMode(FLG_PIN, OUTPUT); // Pin for the ppsfl flag for debug +#endif +#if EVT_PIN + pinMode(EVT_PIN, OUTPUT); // Pin for the cosmic ray event +#endif +#if PPS_PIN + pinMode(PPS_PIN, OUTPUT); // Pin for the PPS (LED pin) +#endif + + Serial.begin(SERIAL_BAUD_RATE); // Start the serial line + Serial1.begin(GPS_BAUD_RATE); // and the second + + gps.begin(GPS_BAUD_RATE); // Chip baud rate + +#ifdef RMCGGA + gps.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA); // With altitude but no yy/mm/dd +#else + gps.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); // With yy/mm/dd but no altitude +#endif + gps.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); // each second + + + InitQueue(); // Reset queue pointers, missed count, and size + + strcpy(rdtm,""); // Set initial value for date/time + strcpy(wdtm,""); + + htu_ok = htu.begin(); + bmp_ok = bmp.begin(); + acl_ok = acl.begin(); + mag_ok = mag.begin(); + dof_ok = dof.begin(); + + AclSetup(); + MagSetup(); + AdcSetup(); + + TimersStart(); // Start timers +} + +// These two routines are needed because the Serial.print method prints without using interrupts. +// Calls to Serial.print block interrupts and use a wait in kernel space causing all ISRs to +// be blocked and hence we could miss some timer interrupts. +// To avoid this problem call PushTxt to have stuff delivered to the serial line, PushTxt simply +// stores your text for future print out by PutChar. The PutChar routine removes one character +// from the stored text each time its called. By placing a call to PutChar in the outermost loop +// of the Arduino loop function, then for each loop one character is printed, avoiding blocking +// of interrupts and vastly improving the loops real time behaviour. + +// Copy text to the buffer for future printing + +void PushTxt(char *txt) { + + int i, l = strlen(txt); + + // If this happens there is a programming bug + + if (l > TBLEN) { // Can't handle more than TBLEN at a time + terr = TXT_TOOBIG; // say error and abort + return; + } + + // If the buffer is filling up to fast throw it away and return an error + + if ((l + tsze) >= TBLEN) { // If there is no room in the buffer + terr = TXT_OVERFL; // Buffer overflow + return; // Simply stop printing when txt comming too fast + } + + // Copy the new text onto the ring buffer for later output + // from the loop idle function + + for (i=0; i Earth Quake + +uint32_t old_icount = 0; + + if (accl_flag) { + if (accl_icount != old_icount) { + old_icount = accl_icount; + PushTim(1); // Push these first, and then vib + PushAcl(1); // This is the real latched value + PushMag(1); + sprintf(txt,"{'VIB':{'Vax':%d,'Vcn':%d}}\n",accl_flag,accl_icount); + PushTxt(txt); + } + } +} + +// Push the magnetic field strengths in all three axis in micro tesla (gauss) + +void PushMag(int flg) { // Push the mago stuff + sensors_event_t mag_event; + sensors_vec_t xyz; + + if ((flg) || ((mag_ok) && ((ppcnt % magnot_display_rate) == 0))) { + mag.getEvent(&mag_event); + + // Micro Tesla + + sprintf(txt,"{'MAG':{'Mgx':%f,'Mgy':%f,'Mgz':%f}}\n", + mag_event.magnetic.x, + mag_event.magnetic.y, + mag_event.magnetic.z); + PushTxt(txt); + + // Orientation (Easy to calculate later in Python - dont waste resources) +#ifdef ORIENTATION + if (dof.magGetOrientation(SENSOR_AXIS_Z, &mag_event, &xyz)) { + sprintf(txt,"{'MOG':{'Mox':%f,'Moy':%f,'Moz':%f}}\n",xyz.x,xyz.y,xyz.z); + PushTxt(txt); + } +#endif + } +} + +// Push the acceleration values in xyz in meters per sec squared + +void PushAcl(int flg) { // Push the accelerometer and compass stuff + sensors_event_t acl_event; + sensors_vec_t xyz; + + if ((flg) || ((acl_ok) && ((ppcnt % accelr_display_rate) == 0))) { + acl.getEvent(&acl_event); + + // Meters per second squared + + sprintf(txt,"{'ACL':{'Acx':%f,'Acy':%f,'Acz':%f}}\n", + acl_event.acceleration.x, + acl_event.acceleration.y, + acl_event.acceleration.z); + PushTxt(txt); + + // Orientation (Easy to calculate later in Python - dont waste resources) +#ifdef ORIENTATION + if (dof.accelGetOrientation(&acl_event, &xyz)) { + sprintf(txt,"{'AOL':{'Aox':%f,'Aoy':%f,'Aoz':%f}}\n",xyz.x,xyz.y,xyz.z); + PushTxt(txt); + } +#endif + } +} + +// Push location latitude longitude in degrees so that google maps gets it correct + +void PushLoc(int flg) { + + if ((flg) || ((ppcnt % latlon_display_rate) == 0)) { + sprintf(txt,"{'LOC':{'Lat':%f,'Lon':%f,'Alt':%f}}\n",latitude,longitude,altitude); + PushTxt(txt); + } +} + +// Push timing + +void PushTim(int flg) { + + if ((flg) || ((ppcnt % frqutc_display_rate) == 0)) { + sprintf(txt,"{'TIM':{'Upt':%4d,'Frq':%7d,'Sec':%s}}\n",ppcnt,rega0,rdtm); + PushTxt(txt); + } +} + +// Push status + +void PushSts(int flg, int qsize, int missed) { +uint8_t res; + + if ((flg) || ((ppcnt % status_display_rate) == 0)) { + sprintf(txt,"{'STS':{'Qsz':%2d,'Mis':%2d,'Ter':%d,'Htu':%d,'Bmp':%d,'Acl':%d,'Mag':%d}}\n", + qsize,missed,terr,htu_ok,bmp_ok,acl_ok,mag_ok); + PushTxt(txt); + terr = 0; + } +} + +// Push event queue + +void PushEvq(int flg, int *qsize, int *missed) { + + struct EventBuf eb; // Temporary event buffer + double evtm = 0.0; // Time since last event or PPS in seconds (< 1.0) + char stx[16]; // Second text + int i,j; + + // If there are any events waiting in the event read buffer, put them on the queue + + for (i=0; i= events_display_size) { + + PushTxt("\n"); + while (PopQueue(&eb)) { // While ther are events on the queue + + // Calculate the time in seconds of this event in the second + // N.B. Ticks are since the last event, or from PPS + + if (eb.Count == 1) evtm = 0.0; // Start a new second + evtm += ((double) eb.Ticks / (double) eb.Frequency); // Add time since last event + sprintf(stx,"%9.7f",evtm); // It will be 0.something + + // Build string and push it out to the print buffer + + sprintf(txt, + "{'EVT':{'Evt':%1d,'Frq':%8d,'Tks':%8d,'Etm':%s%s," + "'Adc':[[%d,%d,%d,%d,%d,%d,%d,%d],[%d,%d,%d,%d,%d,%d,%d,%d]]}}\n", + eb.Count, eb.Frequency, eb.Ticks, eb.DateTime, index(stx,'.'), + eb.Ch0[0],eb.Ch0[1],eb.Ch0[2],eb.Ch0[3],eb.Ch0[4],eb.Ch0[5],eb.Ch0[6],eb.Ch0[7], + eb.Ch1[0],eb.Ch1[1],eb.Ch1[2],eb.Ch1[3],eb.Ch1[4],eb.Ch1[5],eb.Ch1[6],eb.Ch1[7]); + PushTxt(txt); + } + PushTxt("\n"); + } +} + +// Read one input character, we have exactly the same problem with +// the serial line read as with writing, so we need the same work around + +void ReadOneChar() { + char c; + + // Suck in all the characters available on the input stream + // put as many as will fit in the cmd buffer, and say ready + + if ((irdy == 0) && (Serial.available())) { // If buffer free + c = (char) Serial.read(); // Read one char + if (c == '\n') istp = 1; // Stop on '\n' + if ((!istp) && (irdp < (CMDLEN -1))) { + cmd[irdp] = c; + irdp = irdp + 1; + cmd[irdp] = 0; + } + } else irdy = 1; +} + +// Implement the command callback functions + +void noop(int arg) { }; // That was easy + +void help(int arg) { // Display the help + int i; + CmdStruct *cms; + + for (i=0; iName,cms->Par,cms->Help); + PushTxt(txt); + } +} + +void htux(int arg) { htu_ok = htu.begin(); } +void htud(int arg) { humtmp_display_rate = arg; } +void bmpd(int arg) { alttmp_display_rate = arg; } +void locd(int arg) { latlon_display_rate = arg; } +void timd(int arg) { frqutc_display_rate = arg; } +void stsd(int arg) { status_display_rate = arg; } + +void evqt(int arg) { + events_display_size = arg % EVENT_QSIZE; + if (events_display_size == 0) + events_display_size = 24; +} + +void acld(int arg) { accelr_display_rate = arg; } +void magd(int arg) { magnot_display_rate = arg; } + +void aclt(int arg) { + uint8_t val = 0; + accelr_event_threshold = arg & 0x7F; + val = accelr_event_threshold; + acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_INT1_THS_A, val); +} + +// Look up a command in the command table for the given command string +// and call it with its single integer parameter + +void ParseCmd() { + int i, p=0, cl=0; + char *cp, *ep; + CmdStruct *cms; + + for (i=0; iName); + if (strncmp(cms->Name,cmd,cl) == 0) { + if ((cms->Par) && (strlen(cmd) > cl)) { + cp = &cmd[cl]; + p = (int) strtoul(cp,&ep,0); + } + cms->proc(p); + break; + } + } +} + +// This waits for a ready buffer from ReadOneChar. Once ready the buffer is +// locked until its been seen here + +void DoCmd() { + if (irdy) { + if (irdp) { + sprintf(txt,"{'CMD':%s}\n",cmd); + PushTxt(txt); + ParseCmd(); + } + bzero((void *) cmd, CMDLEN); + irdp = 0; irdy = 0; istp = 0; + } +} + +// Arduino main loop does all the user space work + +void loop() { + + int missed, qsize; // Queue vars + + if (displ) { // Displ is set in the PPS ISR, we will reset it here +#if PPS_PIN + digitalWrite(PPS_PIN,HIGH); // PPS arrived +#endif + DoCmd(); // Execute any incomming commands + PushEvq(0,&qsize,&missed); // Push any events + PushHtu(0); // Push HTU temperature and humidity + PushBmp(0); // Push BMP temperature and barrometric altitude + PushLoc(0); // Push location latitude and longitude + PushTim(0); // Push timing data + PushMag(0); // Push mago data + PushAcl(0); // Push accelarometer data + PushSts(0,qsize,missed); // Push status + GetDateTime(); // Read the next date/time from the GPS chip +#if PPS_PIN + digitalWrite(PPS_PIN,LOW); // Reset PPS +#endif + displ = 0; // Clear flag for next PPS + } + PutChar(); // Print one character per loop !!! + ReadOneChar(); // Get next input command char +}