#include "MicroBit.h" /** * Constructor. * Create a compass representation with the given ID. * @param id the event ID of the compass object. * @param address the default address for the compass register * * Example: * @code * compass(MICROBIT_ID_COMPASS, MAG3110_DEFAULT_ADDR); * @endcode * * Possible Events for the compass are as follows: * @code * MICROBIT_COMPASS_EVT_CAL_REQUIRED // triggered when no magnetometer data is available in persistent storage * MICROBIT_COMPASS_EVT_CAL_START // triggered when calibration has begun * MICROBIT_COMPASS_EVT_CAL_END // triggered when calibration has finished. * @endcode */ MicroBitCompass::MicroBitCompass(uint16_t id, uint16_t address) : average(), sample(), int1(MICROBIT_PIN_COMPASS_DATA_READY) { this->id = id; this->address = address; // We presume the device calibrated until the average values are read. this->status = 0x01; // Select 10Hz update rate, with oversampling, and enable the device. this->samplePeriod = 100; this->configure(); // Assume that we have no calibraiton information. status &= ~MICROBIT_COMPASS_STATUS_CALIBRATED; // Indicate that we're up and running. uBit.flags |= MICROBIT_FLAG_COMPASS_RUNNING; } /** * Issues a standard, 2 byte I2C command write to the magnetometer. * Blocks the calling thread until complete. * * @param reg The address of the register to write to. * @param value The value to write. * @return MICROBIT_OK on success, MICROBIT_I2C_ERROR if the magnetometer could not be accessed. */ int MicroBitCompass::writeCommand(uint8_t reg, uint8_t value) { uint8_t command[2]; command[0] = reg; command[1] = value; return uBit.i2c.write(address, (const char *)command, 2); } /** * Issues a read command into the specified buffer. * Blocks the calling thread until complete. * * @param reg The address of the register to access. * @param buffer Memory area to read the data into. * @param length The number of bytes to read. */ int MicroBitCompass::readCommand(uint8_t reg, uint8_t* buffer, int length) { int result; if (buffer == NULL || length <= 0) return MICROBIT_INVALID_PARAMETER; result = uBit.i2c.write(address, (const char *)®, 1, true); if (result !=0) return MICROBIT_I2C_ERROR; result = uBit.i2c.read(address, (char *)buffer, length); if (result !=0) return MICROBIT_I2C_ERROR; return MICROBIT_OK; } /** * Issues a read of a given address, and returns the value. * Blocks the calling thread until complete. * * @param reg The based address of the 16 bit register to access. * @return The register value, interpreted as a 16 but signed value, or MICROBIT_I2C_ERROR if the magnetometer could not be accessed. */ int MicroBitCompass::read16(uint8_t reg) { uint8_t cmd[2]; int result; cmd[0] = reg; result = uBit.i2c.write(address, (const char *)cmd, 1); if (result !=0) return MICROBIT_I2C_ERROR; cmd[0] = 0x00; cmd[1] = 0x00; result = uBit.i2c.read(address, (char *)cmd, 2); if (result !=0) return MICROBIT_I2C_ERROR; return (int16_t) ((cmd[1] | (cmd[0] << 8))); //concatenate the MSB and LSB } /** * Issues a read of a given address, and returns the value. * Blocks the calling thread until complete. * * @param reg The based address of the 16 bit register to access. * @return The register value, interpreted as a 8 bit unsigned value, or MICROBIT_I2C_ERROR if the magnetometer could not be accessed. */ int MicroBitCompass::read8(uint8_t reg) { uint8_t data; int result; data = 0; result = readCommand(reg, (uint8_t*) &data, 1); if (result != MICROBIT_OK) return MICROBIT_I2C_ERROR; return data; } /** * Gets the current heading of the device, relative to magnetic north. * If the compass is not calibrated, it will raise the MICROBIT_COMPASS_EVT_CALIBRATE event. * Users wishing to implement their own calibration algorithms should listen for this event, * using MESSAGE_BUS_LISTENER_IMMEDIATE model. This ensures that calibration is complete before * the user program continues. * * @return the current heading, in degrees. Or MICROBIT_CALIBRATION_IN_PROGRESS if the compass is calibrating. * * Example: * @code * uBit.compass.heading(); * @endcode */ int MicroBitCompass::heading() { float bearing; if(status & MICROBIT_COMPASS_STATUS_CALIBRATING) return MICROBIT_CALIBRATION_IN_PROGRESS; if(!(status & MICROBIT_COMPASS_STATUS_CALIBRATED)) calibrate(); // Precompute the tilt compensation parameters to improve readability. float phi = uBit.accelerometer.getRollRadians(); float theta = uBit.accelerometer.getPitchRadians(); float x = (float) getX(NORTH_EAST_DOWN); float y = (float) getY(NORTH_EAST_DOWN); float z = (float) getZ(NORTH_EAST_DOWN); // Precompute cos and sin of pitch and roll angles to make the calculation a little more efficient. float sinPhi = sin(phi); float cosPhi = cos(phi); float sinTheta = sin(theta); float cosTheta = cos(theta); bearing = (360*atan2(z*sinPhi - y*cosPhi, x*cosTheta + y*sinTheta*sinPhi + z*sinTheta*cosPhi)) / (2*PI); if (bearing < 0) bearing += 360.0; return (int) bearing; } /** * Periodic callback from MicroBit clock. * Check if any data is ready for reading by checking the interrupt. */ void MicroBitCompass::idleTick() { // Poll interrupt line from accelerometer (Active HI). // Interrupt is cleared on data read of MAG_OUT_X_MSB. if(int1) { sample.x = MAG3110_NORMALIZE_SAMPLE((int) read16(MAG_OUT_X_MSB)); sample.y = MAG3110_NORMALIZE_SAMPLE((int) read16(MAG_OUT_Y_MSB)); sample.z = MAG3110_NORMALIZE_SAMPLE((int) read16(MAG_OUT_Z_MSB)); // Indicate that a new sample is available MicroBitEvent e(id, MICROBIT_COMPASS_EVT_DATA_UPDATE); } } /** * Reads the X axis value of the latest update from the compass. * @return The magnetic force measured in the X axis, in nano teslas. * * Example: * @code * uBit.compass.getX(); * @endcode */ int MicroBitCompass::getX(MicroBitCoordinateSystem system) { switch (system) { case SIMPLE_CARTESIAN: return sample.x - average.x; case NORTH_EAST_DOWN: return -(sample.y - average.y); case RAW: default: return sample.x; } } /** * Reads the Y axis value of the latest update from the compass. * @return The magnetic force measured in the Y axis, in nano teslas. * * Example: * @code * uBit.compass.getY(); * @endcode */ int MicroBitCompass::getY(MicroBitCoordinateSystem system) { switch (system) { case SIMPLE_CARTESIAN: return -(sample.y - average.y); case NORTH_EAST_DOWN: return (sample.x - average.x); case RAW: default: return sample.y; } } /** * Reads the Z axis value of the latest update from the compass. * @return The magnetic force measured in the Z axis, in nano teslas. * * Example: * @code * uBit.compass.getZ(); * @endcode */ int MicroBitCompass::getZ(MicroBitCoordinateSystem system) { switch (system) { case SIMPLE_CARTESIAN: case NORTH_EAST_DOWN: return -(sample.z - average.z); case RAW: default: return sample.z; } } /** * Determines the overall magnetic field strength based on the latest update from the compass. * @return The magnetic force measured across all axes, in nano teslas. * * Example: * @code * uBit.compass.getFieldStrength(); * @endcode */ int MicroBitCompass::getFieldStrength() { double x = getX(); double y = getY(); double z = getZ(); return (int) sqrt(x*x + y*y + z*z); } /** * Configures the compass for the sample rate defined * in this object. The nearest values are chosen to those defined * that are supported by the hardware. The instance variables are then * updated to reflect reality. * @return MICROBIT_OK or MICROBIT_I2C_ERROR if the magnetometer could not be configured. */ int MicroBitCompass::configure() { const MAG3110SampleRateConfig *actualSampleRate; int result; // First, take the device offline, so it can be configured. result = writeCommand(MAG_CTRL_REG1, 0x00); if (result != MICROBIT_OK) return MICROBIT_I2C_ERROR; // Wait for the part to enter standby mode... while(1) { // Read the status of the part... // If we can't communicate with it over I2C, pass on the error. result = this->read8(MAG_SYSMOD); if (result == MICROBIT_I2C_ERROR) return MICROBIT_I2C_ERROR; // if the part in in standby, we're good to carry on. if((result & 0x03) == 0) break; // Perform a power efficient sleep... uBit.sleep(100); } // Find the nearest sample rate to that specified. actualSampleRate = &MAG3110SampleRate[MAG3110_SAMPLE_RATES-1]; for (int i=MAG3110_SAMPLE_RATES-1; i>=0; i--) { if(MAG3110SampleRate[i].sample_period < this->samplePeriod * 1000) break; actualSampleRate = &MAG3110SampleRate[i]; } // OK, we have the correct data. Update our local state. this->samplePeriod = actualSampleRate->sample_period / 1000; // Enable automatic reset after each sample; result = writeCommand(MAG_CTRL_REG2, 0xA0); if (result != MICROBIT_OK) return MICROBIT_I2C_ERROR; // Bring the device online, with the requested sample frequency. result = writeCommand(MAG_CTRL_REG1, actualSampleRate->ctrl_reg1 | 0x01); if (result != MICROBIT_OK) return MICROBIT_I2C_ERROR; return MICROBIT_OK; } /** * Attempts to set the sample rate of the compass to the specified value (in ms). * n.b. the requested rate may not be possible on the hardware. In this case, the * nearest lower rate is chosen. * @param period the requested time between samples, in milliseconds. * @return MICROBIT_OK or MICROBIT_I2C_ERROR if the magnetometer could not be updated. */ int MicroBitCompass::setPeriod(int period) { this->samplePeriod = period; return this->configure(); } /** * Reads the currently configured sample rate of the compass. * @return The time between samples, in milliseconds. */ int MicroBitCompass::getPeriod() { return (int)samplePeriod; } /** * Attempts to determine the 8 bit ID from the magnetometer. * @return the id of the compass (magnetometer), or MICROBIT_I2C_ERROR if the magnetometer could not be updated. * * Example: * @code * uBit.compass.whoAmI(); * @endcode */ int MicroBitCompass::whoAmI() { uint8_t data; int result; result = readCommand(MAG_WHOAMI, &data, 1); if (result != MICROBIT_OK) return MICROBIT_I2C_ERROR; return (int)data; } /** * Reads the current die temperature of the compass. * @return the temperature in degrees celsius, or MICROBIT_I2C_ERROR if the magnetometer could not be updated. */ int MicroBitCompass::readTemperature() { int8_t temperature; int result; result = readCommand(MAG_DIE_TEMP, (uint8_t *)&temperature, 1); if (result != MICROBIT_OK) return MICROBIT_I2C_ERROR; return temperature; } /** * Perform a calibration of the compass. * * This method will be called automatically if a user attempts to read a compass value when * the compass is uncalibrated. It can also be called at any time by the user. * * Any old calibration data is deleted. * The method will only return once the compass has been calibrated. * * @return MICROBIT_OK, MICROBIT_I2C_ERROR if the magnetometer could not be accessed, * or MICROBIT_CALIBRATION_REQUIRED if the calibration algorithm failed to complete succesfully. * @note THIS MUST BE CALLED TO GAIN RELIABLE VALUES FROM THE COMPASS */ int MicroBitCompass::calibrate() { // Only perform one calibration process at a time. if(isCalibrating()) return MICROBIT_CALIBRATION_IN_PROGRESS; // Delete old calibration data clearCalibration(); // Record that we've started calibrating. status |= MICROBIT_COMPASS_STATUS_CALIBRATING; // Launch any registred calibration alogrithm visialisation MicroBitEvent(id, MICROBIT_COMPASS_EVT_CALIBRATE); // Record that we've finished calibrating. status &= ~MICROBIT_COMPASS_STATUS_CALIBRATING; // If there are no changes to our sample data, we either have no calibration algorithm, or it couldn't complete succesfully. if(!(status & MICROBIT_COMPASS_STATUS_CALIBRATED)) return MICROBIT_CALIBRATION_REQUIRED; return MICROBIT_OK; } /** * Perform a calibration of the compass. * This will fire MICROBIT_COMPASS_EVT_CAL_START. * @return MICROBIT_OK, or MICROBIT_I2C_ERROR if the magnetometer could not be accessed. * * @note *** THIS FUNCTION IS NOW DEPRECATED AND WILL BE REMOVED IN THE NEXT MAJOR RELEASE *** * @note *** PLEASE USE THE calibrate() FUNCTION INSTEAD *** */ int MicroBitCompass::calibrateStart() { return calibrate(); } /** * Perform the asynchronous calibration of the compass. * This will fire MICROBIT_COMPASS_EVT_CAL_START and MICROBIT_COMPASS_EVT_CAL_END when finished. * * @note *** THIS FUNCITON IS NOW DEPRECATED AND WILL BE REMOVED IN THE NEXT MAJOR RELEASE *** * @note *** PLEASE USE THE calibrate() FUNCTION INSTEAD *** */ void MicroBitCompass::calibrateAsync() { calibrate(); } /** * Complete the calibration of the compass. * This will fire MICROBIT_COMPASS_EVT_CAL_END. * * @note *** THIS FUNCTION IS NOW DEPRECATED AND WILL BE REMOVED IN THE NEXT MAJOR RELEASE *** */ void MicroBitCompass::calibrateEnd() { } /** * Configure the compass to use the given calibration data. * Calibration data is comprised of the perceived zero offset of each axis of the compass. * After calibration this should now take into account trimming errors in the magnetometer, * and any "hard iron" offsets on the device. * * @param The x, y and z zero offsets to use as calibration data. */ void MicroBitCompass::setCalibration(CompassSample calibration) { average = calibration; status |= MICROBIT_COMPASS_STATUS_CALIBRATED; } /** * Provides the calibration data currently in use by the compass. * More specifically, the x, y and z zero offsets of the compass. * * @return The x, y and z xero offsets of the compass. */ CompassSample MicroBitCompass::getCalibration() { return average; } /** * Returns 0 or 1. 1 indicates that the compass is calibrated, zero means the compass requires calibration. */ int MicroBitCompass::isCalibrated() { return status & MICROBIT_COMPASS_STATUS_CALIBRATED; } /** * Returns 0 or 1. 1 indicates that the compass is calibrating, zero means the compass is not currently calibrating. */ int MicroBitCompass::isCalibrating() { return status & MICROBIT_COMPASS_STATUS_CALIBRATING; } /** * Clears the calibration held in persistent storage, and sets the calibrated flag to zero. */ void MicroBitCompass::clearCalibration() { status &= ~MICROBIT_COMPASS_STATUS_CALIBRATED; } /** * Returns 0 or 1. 1 indicates data is waiting to be read, zero means data is not ready to be read. */ int MicroBitCompass::isIdleCallbackNeeded() { // The MAG3110 raises an interrupt line when data is ready, which we sample here. // The interrupt line is active HI, so simply return the state of the pin. return int1; } /** * Destructor for MicroBitMessageBus, so that we deregister ourselves as an idleComponent */ MicroBitCompass::~MicroBitCompass() { uBit.removeIdleComponent(this); } const MAG3110SampleRateConfig MAG3110SampleRate[MAG3110_SAMPLE_RATES] = { {12500, 0x00}, // 80 Hz {25000, 0x20}, // 40 Hz {50000, 0x40}, // 20 Hz {100000, 0x60}, // 10 hz {200000, 0x80}, // 5 hz {400000, 0x88}, // 2.5 hz {800000, 0x90}, // 1.25 hz {1600000, 0xb0}, // 0.63 hz {3200000, 0xd0}, // 0.31 hz {6400000, 0xf0}, // 0.16 hz {12800000, 0xf8} // 0.08 hz };