File content as of revision 0:4dc21c013b2a:

/*

	MPU9250 Basic Example Code
	by: Kris Winer
	date: April 1, 2014
	license: Beerware - Use this code however you'd like. If you
	find it useful you can buy me a beer some time.

	Demonstrate basic MPU-9250 functionality including parameterizing the
	register addresses, initializing the sensor, getting properly scaled
	accelerometer, gyroscope, and magnetometer data out. Added display functions
	to allow display to on breadboard monitor. Addition of 9 DoF sensor fusion
	using open source Madgwick and Mahony filter algorithms. Sketch runs on
	the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.

	-----------------------------------------------------------------------

	Adapted by Marijn Jeurissen for the anyThing Connected Sensor Sticker based on Nordic nRF51822
	date: February 16, 2018


*/


// If you define DEBUG_MODE, then RTT debug prints will be enabled
#define DEBUG_MODE


#include "mbed.h"
#include "ble/BLE.h"
#include "ble/services/DeviceInformationService.h"
#include "MPU9250.h"
#include "softdevice_handler.h"

#ifdef DEBUG_MODE
#include "SEGGER_RTT.h"
#include "SEGGER_RTT.c"
#include "SEGGER_RTT_printf.c"
#endif


// Sensor config
#define SENSOR_SDA P0_8
#define SENSOR_SCL P0_9

// Declination at Delft is 1 degrees 3 minutes on 2018-02-16
#define SENSOR_DECLINATION (-1.05f)


// BLE TxPower values for advertising and connection, highest possible
// TODO: Remove this shit as it DOESN'T FUCKING DO ANYTHING!
static const int TX_POWER_ADVERTISING	= 10;
static const int TX_POWER_CONNECTION	= 10;


// Used for triggering a sensor polling, updating all sensor values
// TODO: Use multiple values/bit masking to allow fine-grained control of what
// sensor value is read/polled
// Always read sensor value at startup
static volatile bool triggerSensorPolling = true;

// Array used to keep track of sensor data, 10 floats in the format:
// Acceleration x, y, z, Gyrometer x, y, z, Magnetometer x, y, z, Temperature
static const uint16_t NUM_MEASUREMENTS = 10;
static float measurements[NUM_MEASUREMENTS] = {0.0f};


// Buffer for float printing
static const uint16_t BUFFER_SIZE = 50;
static char buffer[BUFFER_SIZE];


// Define UUIDs for service and characteristics
static const uint16_t customServiceUUID	= 0xFFFF;	// Custom UUID, FFFF is reserved for development
static const uint16_t readCharUUID		= 0xA001;
static const uint16_t writeCharUUID		= 0xA002;

// Define BLE configuration things
static const char DEVICE_NAME[]			= "AnyConSensor";
static const uint16_t uuid16List[]		= {customServiceUUID,
											GattService::UUID_DEVICE_INFORMATION_SERVICE};
static const int ADVERTISING_INTERVAL	= 1000;		// in ms

// Set Up custom Characteristics
static const uint16_t READ_CHAR_SIZE = NUM_MEASUREMENTS * sizeof(float);
static const uint16_t WRITE_CHAR_SIZE = NUM_MEASUREMENTS * sizeof(float);

static uint8_t readValue[READ_CHAR_SIZE] = {0};
ReadOnlyArrayGattCharacteristic<uint8_t, READ_CHAR_SIZE> readChar(readCharUUID, readValue);

static uint8_t writeValue[WRITE_CHAR_SIZE] = {0};
WriteOnlyArrayGattCharacteristic<uint8_t, WRITE_CHAR_SIZE> writeChar(writeCharUUID, writeValue);

// Set up custom service
GattCharacteristic  *characteristics[] = {&readChar, &writeChar};
GattService customService(customServiceUUID, characteristics, sizeof(characteristics) / sizeof(GattCharacteristic*));

DeviceInformationService  *deviceInfoService;

//Initialize tap variables
bool zTap = 0, zTapStarted = 0;
int zDiff = 0, zTapLimit = 0, zTapDelay = 0;
int tapDiff = 0, tapCount = 0, lastTapFrame = 0, tapCycleStart = 0 , tapCycleDuration = 0, now = 0, timerCycle = 0;
float zMin = 0, zMax = 0, zTapStartValue = 0;

// Tap Settings
int tapZThresh = 3000 ; //x and y axis acceleration threshold to trigger tap in mg
int tapPulseLimit = 50 ; //Not a tap if acceleration duration over this limit
int tapDelayDuration = 500; //Minimum time between taps in ms
int tapCycleLimit = 3000; //Maximum cycle duration to detect multitap from first tap in ms
int tapAction = 3; //Number of taps needed to trigger multitap action      

int toInt(float x) {
	return (int) ((x >= 0.0f) ? x + 0.5f : x - 0.5f);
}

int getTime(int counter, int shift) {
	return (int)((counter / 60000.0f) + shift);
}

void resetTap()
{
    zTapLimit = now + tapPulseLimit;
    zMax = measurements[2];
    zMin = measurements[2];
    zDiff = 0;
    zTap = 0;
    zTapStarted = 0;
    zTapStartValue = 0; 
}

void detectTap()
{
    now = t.read_ms();
    if (now < zTapLimit)
    {
        if (measurements[2] < zMin) zMin = measurements[2];
        if (measurements[2] > zMax)  zMax = measurements[2];
        tapDiff = toInt(1000*(zMax - zMin));
        if (tapDiff > zDiff) zDiff = tapDiff;
    }
    else  //Reset tap difference values every 50ms max tap detection duration time
    {
        resetTap();
    }
    
    if ((zDiff > tapZThresh) //if acceleration is above threshold
        && (now > zTapDelay)  //only register tap again after tap delay
        && (zTapStarted == 0)) //wait for acceleration to end
    {
        resetTap();
        zTapStarted = 1;
        zTapStartValue = measurements[2];
    }
    if (now < zTapLimit && zTapStarted == 1) //Check if acceleration stops within pulse window
    {
        if (measurements[2] < zTapStartValue) //if acceleration falls again within limit, tap detected
        {
            resetTap();
            zTap = 1;
            zTapDelay = now + tapDelayDuration;
            #ifdef DEBUG_MODE
				SEGGER_RTT_WriteString(0, "---------------------------------Tap detected\n"); 
			#endif
        }
    }
    else
    {
        resetTap();
    }
    // Detects taps on the x axis and resets some of the flags
    if (zTap)    // check for tap
    {
        now = t.read_ms();
        if (tapCount == 0) tapCycleStart = now;  // if first tap reset tap cycle
        tapCount++;                            // increment tap counter
        if ((now - tapCycleStart) > tapCycleLimit || tapCount >= tapAction) tapCount = 0;    //Reset tap count after cycle ends
        if (tapCount == tapAction) 
        {
            #ifdef DEBUG_MODE
				SEGGER_RTT_WriteString(0, "---------------------------------Triple tap\n"); // do we have 3 taps during cycle?
			#endif
        }
    }
}

/*
	Restart advertising when client disconnects
*/
void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *) {
	BLE::Instance(BLE::DEFAULT_INSTANCE).gap().startAdvertising();
}

/*
	Callback when a device disconnects
*/
void timeoutCallback(Gap::TimeoutSource_t source) {
	#ifdef DEBUG_MODE
	SEGGER_RTT_printf(0, "Fuck, got a timeout on BLE connection with error %d...", source);
	#endif
}
/*
	Callback function to check if we need to update sensor values
*/
void periodicCallback() {
	// Note that the periodicCallback() executes in interrupt context, so it is
	// safer to do heavy-weight sensor polling from the main thread
	triggerSensorPolling = true;
}


/*
	Handle writes to writeCharacteristic
	TODO: Remove this or find a use for it
*/
void writeCharCallback(const GattWriteCallbackParams  *params) {
	// Check to see what characteristic was written, by handle
	if (params->handle == writeChar.getValueHandle()) {
		// toggle LED if only 1 byte is written
		if (params->len == 1) {
			(params->data[0] == 0x00) ? printf("led on\n\r") : printf("led off\n\r");
		} else {
			// Print the data if more than 1 byte is written
			#ifdef DEBUG_MODE
			SEGGER_RTT_printf(0, "Data received: length = %d, data = 0x", params->len);
			#endif

			for(int x=0; x < params->len; x++) {
				printf("%x", params->data[x]);
			}

			printf("\n\r");
		}
	}
}

/*
	Initialization callback
*/
void bleInitComplete(BLE::InitializationCompleteCallbackContext  *params) {
	BLE &ble = params->ble;
	ble_error_t error = params->error;

	if (error != BLE_ERROR_NONE) {
		return;
	}

	ble.gap().onDisconnection(disconnectionCallback);
	// ble.gattServer().onDataWritten(writeCharCallback);
	ble.gap().onTimeout(timeoutCallback);

	// Setup information service
	deviceInfoService = new DeviceInformationService(ble, "ARM", "Model1", "SN1",
		"hw-rev1", "fw-rev1", "soft-rev1");

	// Setup advertising, BLE only, no classic BT
	ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
	ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
		(uint8_t *)uuid16List, sizeof(uuid16List));
	ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME,
		(uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));

	ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
	ble.gap().setAdvertisingInterval(ADVERTISING_INTERVAL);
	ble.gap().setAdvertisingTimeout(0);

	// Get current advertising power level...
	// TODO: Remove this useless crap since it doesnt do anything...
	/*
	GapAdvertisingData payload = ble.gap().getAdvertisingPayload();

	#ifdef DEBUG_MODE
	SEGGER_RTT_printf(0, "Current advertising Tx power: %d dBm\n", payload.TX_POWER_LEVEL);
	#endif

	error = ble.gap().accumulateAdvertisingPayloadTxPower(TX_POWER_ADVERTISING);

	if (error != BLE_ERROR_NONE) {
		#ifdef DEBUG_MODE
		SEGGER_RTT_printf(0, "Failed to set advertising TX power to %d dBm!\n", TX_POWER_ADVERTISING);
		#endif
	} else {
		#ifdef DEBUG_MODE
		SEGGER_RTT_printf(0, "Set advertising TX power to %d dBm!\n", TX_POWER_ADVERTISING);
		#endif
	}

	// Get current advertising power level...
	payload = ble.gap().getAdvertisingPayload();

	#ifdef DEBUG_MODE
	SEGGER_RTT_printf(0, "Current advertising Tx power: %d dBm\n", payload.TX_POWER_LEVEL);
	#endif

	error = ble.gap().setTxPower(TX_POWER_CONNECTION);

	if (error != BLE_ERROR_NONE) {
		#ifdef DEBUG_MODE
		SEGGER_RTT_printf(0, "Failed to set general TX power to %d dBm!\n", TX_POWER_CONNECTION);
		#endif
	} else {
		#ifdef DEBUG_MODE
		SEGGER_RTT_printf(0, "Set general TX power to %d dBm!\n", TX_POWER_CONNECTION);
		#endif
	}
	*/

	ble.addService(customService);
	ble.gap().startAdvertising();
}

void printPowerValues() {
	const int8_t *power_values;
	size_t count;

	BLE &ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
	ble.gap().getPermittedTxPowerValues(&power_values, &count);

	for (int i = 0; i < count; ++i) {
		#ifdef DEBUG_MODE
		SEGGER_RTT_printf(0, "Got power value: %d dBm\n", power_values[i]);
		#endif
	}
}

/*
	Copies the measurements float array into the BLE readChar.
	Make sure the destination buffer has sufficient space! Its size should be
	at least 4 times that of measurements. This function assumes that
	sizeof(float) = 4 bytes, aka 32 bits, aka 4 uint8_t values.
*/
void copyMeasurementsToBLEChar(float *measurements, uint8_t *destination, uint16_t numElements) {
	for (uint16_t i = 0; i < numElements; i++) {
		// Use a dirty union to slice the floats into 4 uint8_t's
		union {
			float f;
			uint8_t uints[4];
		} dirtyHack;

		dirtyHack.f = measurements[i];
		destination[i * 4]		= dirtyHack.uints[0];
		destination[i * 4 + 1]	= dirtyHack.uints[1];
		destination[i * 4 + 2]	= dirtyHack.uints[2];
		destination[i * 4 + 3]	= dirtyHack.uints[3];
	}
}


/*
	Returns true/false if the sensor has new data. If not, no measurements are
	done. If the sensor has new data, then the measurements array
	(sized appropriately) will be filled according to the format specified at
	the top of this file.
*/
bool measureSensor(MPU9250 *mpu9250, float *measurements) {

	if (!mpu9250->hasNewData()) {
		return false;
	}

	// Temporary variables to store results
	float ax, ay, az, gx, gy, gz, mx, my, mz, temperature;

	// Get the accleration value into actual g's
	mpu9250->readAccelData(&ax, &ay, &az);

	// Get the gyro values into actual degrees per second
	mpu9250->readGyroData(&gx, &gy, &gz);

	// Get actual magnetometer value, this depends on scale being set
	mpu9250->readMagData(&gx, &gy, &gz);

	// Temperature in degrees Celsius
	temperature = mpu9250->getTemperature();

	// Store it in the measurements array
	measurements[0] = ax;
	measurements[1] = ay;
	measurements[2] = az;
	measurements[3] = gx;
	measurements[4] = gy;
	measurements[5] = gz;
	measurements[6] = mx;
	measurements[7] = my;
	measurements[8] = mz;
	measurements[9] = temperature;
    
    detectTap();
	return true;
}


/*
	Sets up the given sensor
*/
void setupSensor(MPU9250  *mpu9250) {
	#ifdef DEBUG_MODE
	SEGGER_RTT_printf(0, "CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
	#endif

	float SelfTest[6];

	// Read the WHO_AM_I register, this is a good test of communication
	// Read WHO_AM_I register for MPU-9250
	uint8_t whoami = mpu9250->getWhoAmI();

	// WHO_AM_I should always be 0x71
	if (whoami == CORRECT_WHO_AM_I) {
		#ifdef DEBUG_MODE
		SEGGER_RTT_WriteString(0, "MPU9250 is online...\n\n");
		#endif

		wait(1);

		// Reset registers to default in preparation for device calibration
		mpu9250->resetMPU9250();

		// Start by performing self test and reporting values
		mpu9250->MPU9250SelfTest(SelfTest);

		#ifdef DEBUG_MODE
		SEGGER_RTT_printf(0, "x-axis self test: accel trim within : %d % of factory value\n",
			toInt(SelfTest[0]));
		SEGGER_RTT_printf(0, "y-axis self test: accel trim within : %d % of factory value\n",
			toInt(SelfTest[1]));
		SEGGER_RTT_printf(0, "z-axis self test: accel trim within : %d % of factory value\n",
			toInt(SelfTest[2]));
		SEGGER_RTT_printf(0, "x-axis self test: gyration trim within : %d % of factory value\n",
			toInt(SelfTest[3]));
		SEGGER_RTT_printf(0, "y-axis self test: gyration trim within : %d % of factory value\n",
			toInt(SelfTest[4]));
		SEGGER_RTT_printf(0, "z-axis self test: gyration trim within : %d % of factory value\n\n",
			toInt(SelfTest[5]));
		#endif

		// Calibrate gyro and accelerometers, load biases in bias registers
		mpu9250->calibrateMPU9250();

		#ifdef DEBUG_MODE
		SEGGER_RTT_printf(0, "x gyro bias = %d\n", toInt(mpu9250->gyroBias[0]));
		SEGGER_RTT_printf(0, "y gyro bias = %d\n", toInt(mpu9250->gyroBias[1]));
		SEGGER_RTT_printf(0, "z gyro bias = %d\n", toInt(mpu9250->gyroBias[2]));
		SEGGER_RTT_printf(0, "x accel bias = %d\n", toInt(mpu9250->accelBias[0]));
		SEGGER_RTT_printf(0, "y accel bias = %d\n", toInt(mpu9250->accelBias[1]));
		SEGGER_RTT_printf(0, "z accel bias = %d\n\n", toInt(mpu9250->accelBias[2]));
		#endif

		wait(2);

		// Initialize device for active mode read of acclerometer, gyroscope, and temperature
		mpu9250->initMPU9250();

		#ifdef DEBUG_MODE
		SEGGER_RTT_WriteString(0, "MPU9250 initialized for active data mode....\n");
		#endif

		// Initialize device for active mode read of magnetometer
		mpu9250->initAK8963();

		#ifdef DEBUG_MODE
		SEGGER_RTT_WriteString(0, "AK8963 initialized for active data mode....\n");
		SEGGER_RTT_printf(0, "Accelerometer full-scale range = %d g\n",
			toInt(2.0f * (float) (1<<mpu9250->Ascale)));
		SEGGER_RTT_printf(0, "Gyroscope full-scale range = %d deg/s\n",
			toInt(250.0f * (float) (1<<mpu9250->Gscale)));

		if (mpu9250->Mscale == 0) {
			SEGGER_RTT_WriteString(0, "Magnetometer resolution = 14 bits\n");
		}

		if (mpu9250->Mscale == 1) {
			SEGGER_RTT_WriteString(0, "Magnetometer resolution = 16 bits\n");
		}

		if (mpu9250->Mmode == 2) {
			SEGGER_RTT_WriteString(0, "Magnetometer ODR = 8 Hz\n");
		}

		if (mpu9250->Mmode == 6) {
			SEGGER_RTT_WriteString(0, "Magnetometer ODR = 100 Hz\n");
		}

		SEGGER_RTT_WriteString(0, "\n");

		// Scale resolutions per LSB for the sensors
		SEGGER_RTT_printf(0, "Accelerometer sensitivity is %d LSB/g \n",
			toInt(1.0f / mpu9250->getAres()));
		SEGGER_RTT_printf(0, "Gyroscope sensitivity is %d LSB/deg/s \n",
			toInt(1.0f / mpu9250->getGres()));
		SEGGER_RTT_printf(0, "Magnetometer sensitivity is %d LSB/G \n",
			toInt(1.0f / mpu9250->getMres()));
		#endif

		wait(1);
	} else {
		// Loop forever if communication doesn't happen...
		#ifdef DEBUG_MODE
		SEGGER_RTT_printf(0, "Could not connect to MPU9250: 0x%x \n", whoami);
		#endif
		while(1);
	}
}



/*
	Main function
*/
int main() {
	// Setup periodicCallback ticker
	Ticker ticker;
	ticker.attach(&periodicCallback, 1);

	BLE &ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
	ble.init(bleInitComplete);

	// SpinWait for initialization to complete. This is necessary because the
	// BLE object is used in the main loop below.
	while (ble.hasInitialized() == false) { /* spin loop */ }

	#ifdef DEBUG_MODE
	SEGGER_RTT_WriteString(0, "Initialized BLE...\n");
	#endif

	printPowerValues();

	// Setup temporary array of 10 bytes so we don't have to use the heap
	uint8_t newReadValue[READ_CHAR_SIZE] = {0};

	// Setup I2C and MPU9250 sensor
	I2C i2cConnection(P0_8, P0_9);
	MPU9250 mpu9250(&i2cConnection);
	setupSensor(&mpu9250);

	Timer t;
	t.start();

	// Variables to hold latest sensor data values
	//float pitch, yaw, roll;
	bool sensorHasNewData = false;

	// Used to control display output rate
	float shift = 0.0f;
	int delt_t = 0;
	int count = 0;
	int cycle = 0;

	// Used to calculate integration interval
	int lastUpdate = 0, firstUpdate = 0, currentTime = 0;                                                         

    resetTap();
    
	while (1) {
		// If not enough measurements for the sensor have been done, or the
		// trigger has been activated, do a measurement
		if (triggerSensorPolling || !mpu9250.sufficientMeasurements) {
			triggerSensorPolling = false;

			// Check if sensor has new data
			sensorHasNewData = measureSensor(&mpu9250, measurements);
			currentTime = t.read_us();

			// Set integration time by time elapsed since last filter update
			mpu9250.deltat = (float) ((currentTime - lastUpdate) / 1000000.0f);
			lastUpdate = currentTime;

			if (lastUpdate - firstUpdate > 10000000.0f) {
				// Decrease filter gain and increase bias drift gain after stabilized
				mpu9250.beta = 0.04;
				mpu9250.zeta = 0.015;
				mpu9250.sufficientMeasurements = true;
			}

			// Pass gyro rate as rad/s
			// mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx * PI/180.0f,
			// gy * PI/180.0f, gz * PI/180.0f, my, mx, mz);
			/*mpu9250.MahonyQuaternionUpdate(measurements[0], measurements[1],
				measurements[2], DEG2RAD(measurements[3]), DEG2RAD(measurements[4]),
				DEG2RAD(measurements[5]), measurements[6], measurements[7], measurements[8]);
            */
			// Serial print 1 s rate independent of data rates
			delt_t = t.read_ms() - count;

			// Update print once per second independent of read rate
			#ifdef DEBUG_MODE
			SEGGER_RTT_printf(0, "\n\nax = %d", toInt(1000 * measurements[0]));
			SEGGER_RTT_printf(0, " ay = %d", toInt(1000 * measurements[1]));
			SEGGER_RTT_printf(0, " az = %d mg\n", toInt(1000 * measurements[2]));

			SEGGER_RTT_printf(0, "gx = %d", toInt(measurements[3]));
			SEGGER_RTT_printf(0, " gy = %d", toInt(measurements[4]));
			SEGGER_RTT_printf(0, " gz = %d deg/s\n", toInt(measurements[5]));

			SEGGER_RTT_printf(0, "mx = %d", toInt(measurements[6]));
			SEGGER_RTT_printf(0, " my = %d", toInt(measurements[7]));
			SEGGER_RTT_printf(0, " mz = %d mG\n", toInt(measurements[8]));

			SEGGER_RTT_printf(0, "Temperature = %d C\n\n", toInt(measurements[9]));
			#endif

			// Get yaw, pitch and roll
			/*mpu9250.getYawPitchRoll(&yaw, &pitch, &roll, SENSOR_DECLINATION);

			#ifdef DEBUG_MODE
			SEGGER_RTT_printf(0, "Yaw: %d Pitch: %d Roll: %d\n\n",
				toInt(yaw), toInt(pitch), toInt(roll));
			#endif*/

			count = t.read_ms();

			#ifdef DEBUG_MODE
			SEGGER_RTT_printf(0, "Time active: %d minutes\n----------------",
				getTime(count, shift));
			#endif

			if (count > 1<<21) {
				// Start the timer over again if ~30 minutes has passed
				t.stop();
				t.reset();
				t.start();
				count = 0;

				#ifdef DEBUG_MODE
				SEGGER_RTT_printf(0, "Resetting timer! t.read_ms() now gives: %d\n",
					t.read_ms());
				#endif

				mpu9250.deltat = 0;
				lastUpdate = t.read_us();
				shift = (++cycle * 34.9525f);
			}

			if (ble.getGapState().connected) {
				// Send measurements via bluetooth
				copyMeasurementsToBLEChar(measurements, readValue, NUM_MEASUREMENTS);
				ble.gattServer().write(readChar.getValueHandle(), readValue, READ_CHAR_SIZE);
			}
		} else {
			// Save power by waiting for BLE events, if we already have enough
			// measurements for sufficient sensor accuracy
			if (mpu9250.sufficientMeasurements) {
				ble.waitForEvent();
			}
		}
	}
}