More info soon

Video


Gallery

Program code

//author: Krzysztof Kapusta, All rights reserved /*****************************************************************/ /* LSM303DLH */ /* Digital Accelerometer & Magnetometer */ /* for */ /* Lego Mindstorms NXT 2.0 */ /* */ /* Author: Krzysztof Kapusta */ /* Release date: 12.02.2012 */ /* Version: 1.0 */ /* */ /* I cannot guarantee that this library will work */ /* and I cannot guarantee that it will not cause damage. */ /* This file is still under development. */ /* */ /* If you have any problems using this file or */ /* you've found a bug, please report to me to */ /* admin@legorobot.pl */ /*****************************************************************/ #ifndef _ACCMAG_NXC_ #define _ACCMAG_NXC_ // Next definition is for adding delay before every I2C command. If you don't want that, just comment next line #define _ACCMAG_PROBLEMS #define _ACCMAG_DELAY 2 struct Vector { s16 X,Y,Z; }; ///////////////////////////// MAGNETOMETER INTERFACE /////////////////////////// #define MAG_ADR 0x3C #define CRA_REG_M 0x00 #define CRB_REG_M 0x01 #define MR_REG_M 0x02 #define OUT_X_H_M 0x03 #define OUT_X_L_M 0x04 #define OUT_Y_H_M 0x05 #define OUT_Y_L_M 0x06 #define OUT_Z_H_M 0x07 #define OUT_Z_L_M 0x08 #define SR_REG_M 0x09 #define IRA_REG_M 0x0A #define IRB_REG_M 0x0B #define IRC_REG_M 0x0C #define IRA_REG_M_RESPONSE 0x48 #define IRB_REG_M_RESPONSE 0x34 #define IRC_REG_M_RESPONSE 0x33 #define CRA_REG_M_DATA_OUTPUT_RATE_MASK 0x1C #define CRA_REG_M_CONF_MASK 0x03 #define CRB_REG_M_GAIN_MASK 0xE0 #define MR_REG_M_MODE_MASK 0x03 #define SR_REG_M_REGULATOR_ENABLED_MASK 0x04 #define SR_REG_M_OUTPUT_REGISTER_LOCK_MASK 0x02 #define SR_REG_M_READY_MASK 0x01 #define MAG_DATA_OUTPUT_RATE_0_75_HZ 0x00 #define MAG_DATA_OUTPUT_RATE_1_5_HZ 0x04 #define MAG_DATA_OUTPUT_RATE_3_HZ 0x08 #define MAG_DATA_OUTPUT_RATE_7_5_HZ 0x0C #define MAG_DATA_OUTPUT_RATE_15_HZ 0x10 #define MAG_DATA_OUTPUT_RATE_30_HZ 0x14 #define MAG_DATA_OUTPUT_RATE_75_HZ 0x18 #define MAG_CONF_NORMAL 0x00 #define MAG_CONF_POSITIVE_BIAS 0x01 #define MAG_CONF_NEGATIVE_BIAS 0x02 #define MAG_GAIN_1_3_GAUSS 0x20 #define MAG_GAIN_1_9_GAUSS 0x40 #define MAG_GAIN_2_5_GAUSS 0x60 #define MAG_GAIN_4_0_GAUSS 0x80 #define MAG_GAIN_4_7_GAUSS 0xA0 #define MAG_GAIN_5_6_GAUSS 0xC0 #define MAG_GAIN_8_1_GAUSS 0xE0 #define MAG_MODE_CONTINUOUS 0x00 #define MAG_MODE_SINGLE 0x01 #define MAG_MODE_SLEEP 0x03 /* Output data rate for the magnetometer */ enum MagDataOutputRate { MRATE_0_75_HZ = MAG_DATA_OUTPUT_RATE_0_75_HZ, // 0.75 Hz MRATE_1_5_HZ = MAG_DATA_OUTPUT_RATE_1_5_HZ, // 1.5 Hz MRATE_3_HZ = MAG_DATA_OUTPUT_RATE_3_HZ, // 3 Hz MRATE_7_5_HZ = MAG_DATA_OUTPUT_RATE_7_5_HZ, // 7.5 Hz MRATE_15_HZ = MAG_DATA_OUTPUT_RATE_15_HZ, // 15 Hz MRATE_30_HZ = MAG_DATA_OUTPUT_RATE_30_HZ, // 30 Hz MRATE_75_HZ = MAG_DATA_OUTPUT_RATE_75_HZ }; // 75 Hz /* Configuration of the magnetometer */ enum MagConfiguration { MNORMAL = MAG_CONF_NORMAL, // normal confuguration for normal use MPOSITIVE_BIAS = MAG_CONF_POSITIVE_BIAS, // for testing MNEGATIVE_BIAS = MAG_CONF_NEGATIVE_BIAS }; // for testing /* Scale of the magnetometer - it's the maximum value that it will read correctly The less the more accurate readings */ enum MagGain { MGAIN_1_3_GAUSS = MAG_GAIN_1_3_GAUSS, // 1.3 Gauss MGAIN_1_9_GAUSS = MAG_GAIN_1_9_GAUSS, // 1.9 Gauss MGAIN_2_5_GAUSS = MAG_GAIN_2_5_GAUSS, // 2.5 Gauss MGAIN_4_0_GAUSS = MAG_GAIN_4_0_GAUSS, // 4.0 Gauss MGAIN_4_7_GAUSS = MAG_GAIN_4_7_GAUSS, // 4.7 Gauss MGAIN_5_6_GAUSS = MAG_GAIN_5_6_GAUSS, // 5.6 Gauss MGAIN_8_1_GAUSS = MAG_GAIN_8_1_GAUSS }; // 8.1 Gauss /* Magnetometer's mode */ enum MagMode { MCONTINUOUS = MAG_MODE_CONTINUOUS, // Continuous reading MSINGLE = MAG_MODE_SINGLE, // Just one reading MSLEEP = MAG_MODE_SLEEP }; // Sleep mode - no readings /* Initialization of the magnetometer. This function must be called before any reading from the sensor. Return: false if something went wrong; true otherwise */ inline bool SetSensorMagnetic(const byte & port, const MagMode & mode = MCONTINUOUS, const MagDataOutputRate & outputRate = MRATE_75_HZ, const MagGain & gain = MGAIN_1_3_GAUSS, const MagConfiguration & configuration = MNORMAL) { SetSensorType(port, IN_TYPE_LOWSPEED); SetSensorMode(port, IN_MODE_RAW); ResetSensor(port); byte out; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, MAG_ADR, IRA_REG_M, out); if (out != IRA_REG_M_RESPONSE || I2CCheckStatus(port)) return false; byte val = outputRate | configuration; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif if (WriteI2CRegister(port, MAG_ADR, CRA_REG_M, val)) return false; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif if (I2CCheckStatus(port)) return false; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif if (WriteI2CRegister(port, MAG_ADR, CRB_REG_M, gain)) return false; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif if (I2CCheckStatus(port)) return false; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif if (WriteI2CRegister(port, MAG_ADR, MR_REG_M, mode)) return false; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif if (I2CCheckStatus(port)) return false; return true; } /* Returns output rate for the magnetometer */ inline MagDataOutputRate MagGetOutputRate(const byte & port) { byte read; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, MAG_ADR, CRA_REG_M, read); return (read & CRA_REG_M_DATA_OUTPUT_RATE_MASK); } /* Returns magnetometer's configuration */ inline MagConfiguration MagGetConfiguration(const byte & port) { byte read; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, MAG_ADR, CRA_REG_M, read); return (read & CRA_REG_M_CONF_MASK); } /* Returns current magnetometer's gain */ inline MagGain MagGetGain(const byte & port) { byte read; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, MAG_ADR, CRB_REG_M, read); return (read & CRB_REG_M_GAIN_MASK); } /* Returns magnetometer operating mode */ inline MagMode MagGetMode(const byte & port) { byte read; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, MAG_ADR, MR_REG_M, read); return (read & MR_REG_M_MODE_MASK); } /* Checks if the internal regulator is enabled */ inline bool MagIsRegulatorEnabled(const byte & port) { byte read; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, MAG_ADR, SR_REG_M, read); return (read & SR_REG_M_REGULATOR_ENABLED_MASK); } /* Checks if output register is locked - it means that there is a writing pending in the device */ inline bool MagIsOutputRegisterLocked(const byte & port) { byte read; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, MAG_ADR, SR_REG_M, read); return (read & SR_REG_M_OUTPUT_REGISTER_LOCK_MASK); } /* Ckecks if there's new reading available */ inline bool MagIsReady(const byte & port) { byte read; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, MAG_ADR, SR_REG_M, read); return (read & SR_REG_M_READY_MASK); } /* Sets new output rate for the magnetometer. Return: error code (see CommLSCheckStatusType for more information) */ inline char MagSetOutputRate(const byte & port, const MagDataOutputRate & outputRate) { byte write = (outputRate | MagGetConfiguration(port)); #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif return WriteI2CRegister(port, MAG_ADR, CRA_REG_M, write); } /* Sets new configuration for the magnetometer. Return: error code (see CommLSCheckStatusType for more information) */ inline byte MagSetConfiguration(const byte & port, const MagConfiguration & configuration) { byte write = (configuration | MagGetOutputRate(port)); #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif return WriteI2CRegister(port, MAG_ADR, CRA_REG_M, write); } /* Sets new gain for the magnetometer. Return: error code (see CommLSCheckStatusType for more information) */ inline byte MagSetGain(const byte & port, const MagGain & gain) { #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif return WriteI2CRegister(port, MAG_ADR, CRB_REG_M, gain); } /* Sets new mode for the magnetometer. Return: error code (see CommLSCheckStatusType for more information) */ inline byte MagSetMode(const byte & port, const MagMode & mode) { #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif return WriteI2CRegister(port, MAG_ADR, MR_REG_M, mode); } /* Scaled raw value from the magnetometer. The scale is 1 Gauss. You need to provide scale in which the measurment was taken. */ inline VectorType MagRawToScaled(const Vector & raw, const MagGain & gain) { VectorType ret; switch (gain) { case MGAIN_1_3_GAUSS: ret.X = 1.0*raw.X/1055.0; ret.Y = 1.0*raw.Y/1055.0; ret.Z = 1.0*raw.Z/950.0; break; case MGAIN_1_9_GAUSS: ret.X = 1.0*raw.X/795.0; ret.Y = 1.0*raw.Y/795.0; ret.Z = 1.0*raw.Z/710.0; break; case MGAIN_2_5_GAUSS: ret.X = 1.0*raw.X/635.0; ret.Y = 1.0*raw.Y/635.0; ret.Z = 1.0*raw.Z/570.0; break; case MGAIN_4_0_GAUSS: ret.X = 1.0*raw.X/430.0; ret.Y = 1.0*raw.Y/430.0; ret.Z = 1.0*raw.Z/385.0; break; case MGAIN_4_7_GAUSS: ret.X = 1.0*raw.X/375.0; ret.Y = 1.0*raw.Y/375.0; ret.Z = 1.0*raw.Z/335.0; break; case MGAIN_5_6_GAUSS: ret.X = 1.0*raw.X/320.0; ret.Y = 1.0*raw.Y/320.0; ret.Z = 1.0*raw.Z/295.0; break; case MGAIN_8_1_GAUSS: ret.X = 1.0*raw.X/230.0; ret.Y = 1.0*raw.Y/230.0; ret.Z = 1.0*raw.Z/205.0; break; } return ret; } /* Reads raw value from the magnetometer */ inline Vector ReadSensorMagRaw(const byte & port) { Vector result; u16 num; byte out; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, MAG_ADR, OUT_X_H_M, out); num = out<<8; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, MAG_ADR, OUT_X_L_M, out); num |= out; result.X = (num & 0x8000 ? num - 0x10000 : num); #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, MAG_ADR, OUT_Y_H_M, out); num = out<<8; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, MAG_ADR, OUT_Y_L_M, out); num |= out; result.Y = (num & 0x8000 ? num - 0x10000 : num); #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, MAG_ADR, OUT_Z_H_M, out); num = out<<8; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, MAG_ADR, OUT_Z_L_M, out); num |= out; result.Z = (num & 0x8000 ? num - 0x10000 : num); return result; } /* Reads scaled value from the magnetometer. You should specify current gain for the device. It reduces communication to the device Return unit: 1 Gauss */ VectorType ReadSensorMagScaledEx(const byte & port, const MagGain & gain) { VectorType v = MagRawToScaled(ReadSensorMagRaw(port), gain); return v; } /* Reads scaled value from the magnetometer */ VectorType ReadSensorMagScaled(const byte & port) { VectorType v = MagRawToScaled(ReadSensorMagRaw(port), MagGetGain(port)); return v; } /////////////////////////// ACCELEROMETER INTERFACE /////////////////////////// #define ACC_ADR 0x30 #define CTRL_REG1_A 0x20 #define CTRL_REG2_A 0x21 #define CTRL_REG4_A 0x23 #define HP_FILTER_RESET_A 0x25 #define REFERENCE_A 0x26 #define STATUS_REG_A 0x27 #define OUT_X_L_A 0x28 #define OUT_X_H_A 0x29 #define OUT_Y_L_A 0x2A #define OUT_Y_H_A 0x2B #define OUT_Z_L_A 0x2C #define OUT_Z_H_A 0x2D #define CTRL_REG1_A_POWER_MODE_MASK 0xE0 #define CTRL_REG1_A_DATA_RATE_MASK 0x18 #define CTRL_REG1_A_Z_ENABLE_MASK 0x04 #define CTRL_REG1_A_Y_ENABLE_MASK 0x02 #define CTRL_REG1_A_X_ENABLE_MASK 0x01 #define CTRL_REG2_A_BOOT_MASK 0x80 #define CTRL_REG2_A_HIGHPASS_MODE_MASK 0x60 #define CTRL_REG2_A_FILTERED_DATA_MASK 0x10 #define CTRL_REG2_A_HIGHPASS_FREQ_MASK 0x03 #define CTRL_REG4_A_BLOCK_DATA_UPDATE_MASK 0x80 #define CTRL_REG4_A_BIG_LITTLE_ENDIAN_MASK 0x40 #define CTRL_REG4_A_SCALE_MASK 0x30 #define CTRL_REG4_A_SELF_TEST_SIGN_MASK 0x08 #define CTRL_REG4_A_SELF_TEST_ENABLE_MASK 0x20 #define STATUS_REG_A_XYZ_OVERRUN_MASK 0x80 #define STATUS_REG_A_Z_OVERRUN_MASK 0x40 #define STATUS_REG_A_Y_OVERRUN_MASK 0x20 #define STATUS_REG_A_X_OVERRUN_MASK 0x10 #define STATUS_REG_A_XYZ_NEW_DATA_MASK 0x08 #define STATUS_REG_A_Z_NEW_DATA_MASK 0x04 #define STATUS_REG_A_Y_NEW_DATA_MASK 0x02 #define STATUS_REG_A_X_NEW_DATA_MASK 0x01 #define ACC_MODE_SLEEP 0x00 #define ACC_MODE_NORMAL 0x20 #define ACC_MODE_LOWPOWER_05_HZ 0x40 #define ACC_MODE_LOWPOWER_1_HZ 0x60 #define ACC_MODE_LOWPOWER_2_HZ 0x80 #define ACC_MODE_LOWPOWER_5_HZ 0xA0 #define ACC_MODE_LOWPOWER_10_HZ 0xC0 #define ACC_DATA_OUTPUT_RATE_50_HZ 0x00 #define ACC_DATA_OUTPUT_RATE_100_HZ 0x08 #define ACC_DATA_OUTPUT_RATE_400_HZ 0x10 #define ACC_DATA_OUTPUT_RATE_1000_HZ 0x18 #define ACC_HIGHPASS_CONFIGURATION_NORMAL 0x00 #define ACC_HIGHPASS_CONFIGURATION_REFERENCE 0x20 #define ACC_SCALE_2G 0x00 #define ACC_SCALE_4G 0x10 #define ACC_SCALE_8G 0x30 /* Accelerometer mode */ enum AccMode { ASLEEP = ACC_MODE_SLEEP, // sleep mode ANORMAL = ACC_MODE_NORMAL, // normal full power mode ALOWPOWER_05_HZ = ACC_MODE_LOWPOWER_05_HZ, // low power mode, data rate 0.5 Hz ALOWPOWER_1_HZ = ACC_MODE_LOWPOWER_1_HZ, // low power mode, data rate 1 Hz ALOWPOWER_2_HZ = ACC_MODE_LOWPOWER_2_HZ, // low power mode, data rate 2 Hz ALOWPOWER_5_HZ = ACC_MODE_LOWPOWER_5_HZ, // low power mode, data rate 5 Hz ALOWPOWER_10_HZ = ACC_MODE_LOWPOWER_10_HZ }; // low power mode, data rate 10 Hz /* Data output rate for the accelerometer This setting is valid only in mode ANORMAL */ enum AccDataOutputRate { ARATE_50_HZ = ACC_DATA_OUTPUT_RATE_50_HZ, // 50 Hz ARATE_100_HZ = ACC_DATA_OUTPUT_RATE_100_HZ, // 100 Hz ARATE_400_HZ = ACC_DATA_OUTPUT_RATE_400_HZ, // 400 Hz ARATE_1000_HZ = ACC_DATA_OUTPUT_RATE_1000_HZ }; // 1000 Hz /* Setting for the built-in high-pass filter */ enum AccHighpassConfiguration { AHIGHPASS_NORMAL = ACC_HIGHPASS_CONFIGURATION_NORMAL, AHIGHPASS_REFERENCE = ACC_HIGHPASS_CONFIGURATION_REFERENCE }; /* Accelerometer's scale - max reading you can get */ enum AccScale { ASCALE_2G = ACC_SCALE_2G, // 2 g ASCALE_4G = ACC_SCALE_4G, // 4 g ASCALE_8G = ACC_SCALE_8G }; // 8 g /* Initializing acceleration sensor. Must be done before reading from the sensor. */ inline bool SetSensorAcceleration(const byte & port, const AccMode & mode = ANORMAL, const AccScale & scale = ASCALE_2G, const AccDataOutputRate & outputRate = ARATE_400_HZ) { SetSensorType(port, IN_TYPE_LOWSPEED); SetSensorMode(port, IN_MODE_RAW); ResetSensor(port); byte wr = mode | outputRate | CTRL_REG1_A_Z_ENABLE_MASK | CTRL_REG1_A_Y_ENABLE_MASK | CTRL_REG1_A_X_ENABLE_MASK; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif if (WriteI2CRegister(port, ACC_ADR, CTRL_REG1_A, wr)) return false; if (I2CCheckStatus(port)) return false; wr = CTRL_REG4_A_BLOCK_DATA_UPDATE_MASK | scale; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif if (WriteI2CRegister(port, ACC_ADR, CTRL_REG4_A, wr)) return false; if (I2CCheckStatus(port)) return false; return true; } /* Returns current device's scale */ inline AccScale AccGetScale(const byte & port) { byte read; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, ACC_ADR, CTRL_REG4_A, read); return (read & CTRL_REG4_A_SCALE_MASK); } /* Sets new scale for the accelerometer. Return: error code (see CommLSCheckStatusType for more information) */ inline char AccSetScale(const byte & port, const AccScale & scale) { byte prev; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif char out = ReadI2CRegister(port, ACC_ADR, CTRL_REG4_A, prev); if (out != 0) return out; prev &= (~CTRL_REG4_A_SCALE_MASK); prev |= scale; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif return WriteI2CRegister(port, ACC_ADR, CTRL_REG4_A, prev); } /* Returns current device's mode */ inline AccDataOutputRate AccGetOutputRate(const byte & port) { byte read; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, ACC_ADR, CTRL_REG1_A, read); return (read & CTRL_REG1_A_DATA_RATE_MASK); } /* Sets new data output rate for the accelerometer. Return: error code (see CommLSCheckStatusType for more information) */ inline char AccSetOutputRate(const byte & port, const AccDataOutputRate & rate) { byte prev; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif char out = ReadI2CRegister(port, ACC_ADR, CTRL_REG1_A, prev); if (out != 0) return out; prev &= (~CTRL_REG1_A_DATA_RATE_MASK); prev |= rate; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif return WriteI2CRegister(port, ACC_ADR, CTRL_REG1_A, prev); } /* Returns current device's mode */ inline AccMode AccGetMode(const byte & port) { byte read; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, ACC_ADR, CTRL_REG1_A, read); return (read & CTRL_REG1_A_POWER_MODE_MASK); } /* Sets new power mode for the accelerometer. Return: error code (see CommLSCheckStatusType for more information) */ inline char AccSetMode(const byte & port, const AccMode & mode) { byte prev; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif char out = ReadI2CRegister(port, ACC_ADR, CTRL_REG1_A, prev); if (out != 0) return out; prev &= (~CTRL_REG1_A_POWER_MODE_MASK); prev |= mode; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif return WriteI2CRegister(port, ACC_ADR, CTRL_REG1_A, prev); } /* Checks if there is a new set of readings available for reading */ inline bool AccIsReady(const byte & port) { byte read; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, ACC_ADR, STATUS_REG_A, read); return (read & STATUS_REG_A_XYZ_NEW_DATA_MASK); } /* Checks if previous reading on the device was overran by a new one before it was read */ inline bool AccOverrun(const byte & port) { byte read; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, ACC_ADR, STATUS_REG_A, read); return (read & STATUS_REG_A_XYZ_OVERRUN_MASK); } inline VectorType AccRawToScaled(const Vector & raw, const AccScale & scale) { VectorType ret; switch (scale) { case ACC_SCALE_2G: ret.X = 1.0*raw.X/1000.0; ret.Y = 1.0*raw.Y/1000.0; ret.Z = 1.0*raw.Z/1000.0; break; case ACC_SCALE_4G: ret.X = 1.0*raw.X/500.0; ret.Y = 1.0*raw.Y/500.0; ret.Z = 1.0*raw.Z/500.0; break; case ACC_SCALE_8G: ret.X = 1.0*raw.X/256.41; ret.Y = 1.0*raw.Y/256.41; ret.Z = 1.0*raw.Z/256.41; break; } return ret; } /* Reads raw values from the sensor */ inline Vector ReadSensorAccRaw(const byte & port) { Vector result; u16 num; byte out; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, ACC_ADR, OUT_X_H_A, out); num = out<<4; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, ACC_ADR, OUT_X_L_A, out); num |= (out>>4); result.X = (num & 0x800 ? num - 0x1000 : num); #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, ACC_ADR, OUT_Y_H_A, out); num = out<<4; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, ACC_ADR, OUT_Y_L_A, out); num |= (out>>4); result.Y = (num & 0x800 ? num - 0x1000 : num); #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, ACC_ADR, OUT_Z_H_A, out); num = out<<4; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, ACC_ADR, OUT_Z_L_A, out); num |= out>>4; result.Z = (num & 0x800 ? num - 0x1000 : num); return result; } /* Reads scaled values from the sensor provided that you know the scale in which the device is operating. Look at AccScale. Using this function reduces the communication to the sensor. */ VectorType ReadSensorAccScaledEx(const byte & port, const AccScale & scale) { VectorType v = AccRawToScaled(ReadSensorAccRaw(port), scale); return v; } /* Reads scaled values from the sensor. Unit: 1g */ VectorType ReadSensorAccScaled(const byte & port) { VectorType v = AccRawToScaled(ReadSensorAccRaw(port), AccGetScale(port)); return v; } ////////////////////////// MAG & ACC INTERFACE //////////////////////////////// inline bool SetSensorAccMag(const byte & port) { SetSensorMagnetic(port); SetSensorAcceleration(port); } inline int GetHeading(const VectorType & accReading, const VectorType & magReading, const VectorType & front) { VectorType east, north; VectorCross(magReading, accReading, east); VectorCross(accReading, east, north); return atan2(VectorDot(east, front), VectorDot(north, front)) * 180.0 / PI; } inline void ReadSensorAccMagSteady(const byte & port, const long & timeMs, VectorType & accReading, VectorType & magReading) { long count = 0, starttime = CurrentTick(), mX=0, mY=0, mZ=0, aX=0, aY=0, aZ=0; Vector M, A, reading; while (CurrentTick() - starttime < timeMs) { while (!MagIsReady(port)); reading = ReadSensorMagRaw(port); mX += reading.X; mY += reading.Y; mZ += reading.Z; reading = ReadSensorAccRaw(port); aX += reading.X; aY += reading.Y; aZ += reading.Z; count++; } A.X = aX/count; A.Y = aY/count; A.Z = aZ/count; M.X = mX/count; M.Y = mY/count; M.Z = mZ/count; accReading = AccRawToScaled(A, AccGetScale(port)); magReading = MagRawToScaled(M, MagGetGain(port)); } #endif

Show/hide all the code

DOWNLOAD

Comments

Hi, Sir. Where can I learn the tilt compensation and every stuff about control the compass and accelerometer? thank you.

Posted by: mike | 2016-04-19

Koszt całości to około 140 zł. Można zrobić sam akcelerometr. Wystarczy jakikolwiek moduł obsługujący magistralę I2C (lub TWI)

Posted by: Admin | 2012-04-13

Ile dokładnie kosztuje? Czy można by zrobić sam akcelerometr?

Posted by: Lego mindstorms | 2012-04-13

Post your comment