max86150.c

/***************************************************
  Arduino library written for the Maxim MAX86150 ECG and PPG integrated sensor

	Written by Ashwin Whitchurch, ProtoCentral Electronics (www.protocentral.com)

  Based on code written by Peter Jansen and Nathan Seidle (SparkFun) for the MAX30105 sensor
  BSD license, all text above must be included in any redistribution.
 *****************************************************/

#include "max86150.h"

#include "tmr_utils.h"
#include "i2c.h"

#include <stdint.h>
#include <stdbool.h>
#include <string.h>

typedef uint8_t byte;

static const uint8_t MAX86150_INTSTAT1   = 0x00;
static const uint8_t MAX86150_INTSTAT2   = 0x01;
static const uint8_t MAX86150_INTENABLE1 = 0x02;
static const uint8_t MAX86150_INTENABLE2 = 0x03;

static const uint8_t MAX86150_FIFOWRITEPTR = 0x04;
static const uint8_t MAX86150_FIFOOVERFLOW = 0x05;
static const uint8_t MAX86150_FIFOREADPTR  = 0x06;
static const uint8_t MAX86150_FIFODATA     = 0x07;

static const uint8_t MAX86150_FIFOCONFIG   = 0x08;
static const uint8_t MAX86150_FIFOCONTROL1 = 0x09;
static const uint8_t MAX86150_FIFOCONTROL2 = 0x0A;

static const uint8_t MAX86150_SYSCONTROL   = 0x0D;
static const uint8_t MAX86150_PPGCONFIG1   = 0x0E;
static const uint8_t MAX86150_PPGCONFIG2   = 0x0F;
static const uint8_t MAX86150_LED_PROX_AMP = 0x10;

static const uint8_t MAX86150_LED1_PULSEAMP = 0x11;
static const uint8_t MAX86150_LED2_PULSEAMP = 0x12;
static const uint8_t MAX86150_LED_RANGE     = 0x14;
static const uint8_t MAX86150_LED_PILOT_PA  = 0x15;

static const uint8_t MAX86150_ECG_CONFIG1   = 0x3C;
static const uint8_t MAX86150_ECG_CONFIG3   = 0x3E;
static const uint8_t MAX86150_PROXINTTHRESH = 0x10;

static const uint8_t MAX86150_PARTID = 0xFF;

// MAX86150 Commands
static const uint8_t MAX86150_INT_A_FULL_MASK    = (byte)~0b10000000;
static const uint8_t MAX86150_INT_A_FULL_ENABLE  = 0x80;
static const uint8_t MAX86150_INT_A_FULL_DISABLE = 0x00;

static const uint8_t MAX86150_INT_DATA_RDY_MASK    = (byte)~0b01000000;
static const uint8_t MAX86150_INT_DATA_RDY_ENABLE  = 0x40;
static const uint8_t MAX86150_INT_DATA_RDY_DISABLE = 0x00;

static const uint8_t MAX86150_INT_ALC_OVF_MASK    = (byte)~0b00100000;
static const uint8_t MAX86150_INT_ALC_OVF_ENABLE  = 0x20;
static const uint8_t MAX86150_INT_ALC_OVF_DISABLE = 0x00;

static const uint8_t MAX86150_INT_PROX_INT_MASK    = (byte)~0b00010000;
static const uint8_t MAX86150_INT_PROX_INT_ENABLE  = 0x10;
static const uint8_t MAX86150_INT_PROX_INT_DISABLE = 0x00;

static const uint8_t MAX86150_SAMPLEAVG_MASK = (byte)~0b00000111;

static const uint8_t MAX86150_ROLLOVER_MASK    = (byte)~0b00010000;
static const uint8_t MAX86150_ROLLOVER_ENABLE  = 0b00010000;
static const uint8_t MAX86150_ROLLOVER_DISABLE = 0b00000000;

static const uint8_t MAX86150_ALMOST_FULL_CLEAR_MASK    = (byte)~0b01000000;
static const uint8_t MAX86150_ALMOST_FULL_CLEAR_ENABLE  = 0b01000000;
static const uint8_t MAX86150_ALMOST_FULL_CLEAR_DISABLE = 0b00000000;

static const uint8_t MAX86150_ALMOST_FULL_REPEAT_MASK    = (byte)~0b00100000;
static const uint8_t MAX86150_ALMOST_FULL_REPEAT_ENABLE  = 0b00100000;
static const uint8_t MAX86150_ALMOST_FULL_REPEAT_DISABLE = 0b00000000;

static const uint8_t MAX86150_A_FULL_MASK = (byte)~0b00001111;

static const uint8_t MAX86150_SHUTDOWN_MASK = (byte)~0b00000010;
static const uint8_t MAX86150_SHUTDOWN      = 0b10;
static const uint8_t MAX86150_WAKEUP        = 0b00;

static const uint8_t MAX86150_FIFO_ENABLE_MASK = (byte)~0b00000100;
static const uint8_t MAX86150_FIFO_ENABLE      = 0b100;
static const uint8_t MAX86150_FIFO_DISABLE     = 0b000;

static const uint8_t MAX86150_RESET_MASK = (byte)~0b00000001;
static const uint8_t MAX86150_RESET      = 0b1;

static const uint8_t MAX86150_ADCRANGE_MASK = (byte)~0b11000000;

static const uint8_t MAX86150_PPG_SAMPLERATE_MASK = (byte)~0b00111100;

static const uint8_t MAX86150_PPG_PULSEWIDTH_MASK = (byte)~0b00000011;

static const uint8_t MAX86150_SLOT1_MASK = 0xF0;
static const uint8_t MAX86150_SLOT2_MASK = 0x0F;
static const uint8_t MAX86150_SLOT3_MASK = 0xF0;
static const uint8_t MAX86150_SLOT4_MASK = 0x0F;

static const uint8_t MAX86150_LED1_RANGE_MASK = (byte)~0b00000011;
static const uint8_t MAX86150_LED2_RANGE_MASK = (byte)~0b00001100;

static const uint8_t MAX86150_ECG_SAMPLERATE_MASK = (byte)~0b00000111;

static const uint8_t MAX86150_ECG_PGA_GAIN_MASK = (byte)~0b00001100;

static const uint8_t MAX86150_ECG_IA_GAIN_MASK = (byte)~0b00000011;

static const uint8_t MAX86150_EXPECTEDPARTID = 0x1E;

static byte activeDevices =
	3; //Gets set during max86150_setup. Allows max86150_check() to calculate how many bytes to read from FIFO

#define STORAGE_SIZE                                                           \
	128 //Each long is 4 bytes so limit this to fit on your micro
typedef struct Record {
	uint32_t red[STORAGE_SIZE];
	uint32_t IR[STORAGE_SIZE];
	int32_t ecg[STORAGE_SIZE];
	byte head;
	byte tail;
} sense_struct; //This is our circular buffer of readings from the sensor

static sense_struct sense;

static void delay(int ms)
{
	TMR_Delay(MXC_TMR0, MSEC(ms), 0);
}

bool max86150_begin(void)
{
	// Step 1: Initial Communication and Verification
	// Check that a MAX86150 is connected
	if (max86150_read_part_id() != MAX86150_EXPECTEDPARTID) {
		// Error -- Part ID read from MAX86150 does not match expected part ID.
		// This may mean there is a physical connectivity problem (broken wire, unpowered, etc).
		return false;
	}
	return true;
}

//
// Configuration
//

//Begin Interrupt configuration
uint8_t max86150_get_int1(void)
{
	return (max86150_read_register(MAX86150_ADDRESS, MAX86150_INTSTAT1));
}
uint8_t max86150_get_int2(void)
{
	return (max86150_read_register(MAX86150_ADDRESS, MAX86150_INTSTAT2));
}

void max86150_set_int_full(bool enabled)
{
	if (enabled) {
		max86150_bit_mask(
			MAX86150_INTENABLE1,
			MAX86150_INT_A_FULL_MASK,
			MAX86150_INT_A_FULL_ENABLE
		);
	} else {
		max86150_bit_mask(
			MAX86150_INTENABLE1,
			MAX86150_INT_A_FULL_MASK,
			MAX86150_INT_A_FULL_DISABLE
		);
	}
}

void max86150_set_int_datardy(bool enabled)
{
	if (enabled) {
		max86150_bit_mask(
			MAX86150_INTENABLE1,
			MAX86150_INT_DATA_RDY_MASK,
			MAX86150_INT_DATA_RDY_ENABLE
		);
	} else {
		max86150_bit_mask(
			MAX86150_INTENABLE1,
			MAX86150_INT_DATA_RDY_MASK,
			MAX86150_INT_DATA_RDY_DISABLE
		);
	}
}

void max86150_set_int_ambient_light_overflow(bool enabled)
{
	if (enabled) {
		max86150_bit_mask(
			MAX86150_INTENABLE1,
			MAX86150_INT_ALC_OVF_MASK,
			MAX86150_INT_ALC_OVF_ENABLE
		);
	} else {
		max86150_bit_mask(
			MAX86150_INTENABLE1,
			MAX86150_INT_ALC_OVF_MASK,
			MAX86150_INT_ALC_OVF_DISABLE
		);
	}
}

void max86150_set_int_proximity(bool enabled)
{
	if (enabled) {
		max86150_bit_mask(
			MAX86150_INTENABLE1,
			MAX86150_INT_PROX_INT_MASK,
			MAX86150_INT_PROX_INT_ENABLE
		);
	} else {
		max86150_bit_mask(
			MAX86150_INTENABLE1,
			MAX86150_INT_PROX_INT_MASK,
			MAX86150_INT_PROX_INT_DISABLE
		);
	}
}
//End Interrupt configuration

void max86150_soft_reset(void)
{
	max86150_bit_mask(
		MAX86150_SYSCONTROL, MAX86150_RESET_MASK, MAX86150_RESET
	);

	// Poll for bit to clear, reset is then complete
	// Timeout after 100 tries
	uint8_t tries = 0;
	while (tries < 100) {
		uint8_t response = max86150_read_register(
			MAX86150_ADDRESS, MAX86150_SYSCONTROL
		);
		if ((response & MAX86150_RESET) == 0)
			break; //We're done!
		tries++;
		delay(1); //Let's not over burden the I2C bus
	}
}

void max86150_shut_down(void)
{
	// Put IC into low power mode (datasheet pg. 19)
	// During shutdown the IC will continue to respond to I2C commands but will
	// not update with or take new readings (such as temperature)
	max86150_bit_mask(
		MAX86150_SYSCONTROL, MAX86150_SHUTDOWN_MASK, MAX86150_SHUTDOWN
	);
}

void max86150_wake_up(void)
{
	// Pull IC out of low power mode (datasheet pg. 19)
	max86150_bit_mask(
		MAX86150_SYSCONTROL, MAX86150_SHUTDOWN_MASK, MAX86150_WAKEUP
	);
}

void max86150_set_fifo_enable(bool enabled)
{
	if (enabled) {
		max86150_bit_mask(
			MAX86150_SYSCONTROL,
			MAX86150_FIFO_ENABLE_MASK,
			MAX86150_FIFO_ENABLE
		);
	} else {
		max86150_bit_mask(
			MAX86150_SYSCONTROL,
			MAX86150_FIFO_ENABLE_MASK,
			MAX86150_FIFO_DISABLE
		);
	}
}

void max86150_set_ppg_adc_range(uint8_t adcRange)
{
	// adcRange: one of MAX86150_ADCRANGE_*
	max86150_bit_mask(
		MAX86150_PPGCONFIG1, MAX86150_ADCRANGE_MASK, adcRange
	);
}

void max86150_set_ppg_sample_rate(uint8_t sampleRate)
{
	// sampleRate: one of MAX86150_PPG_SAMPLERATE_*
	max86150_bit_mask(
		MAX86150_PPGCONFIG1, MAX86150_PPG_SAMPLERATE_MASK, sampleRate
	);
}

void max86150_set_ppg_pulse_width(uint8_t pulseWidth)
{
	// pulseWidth: one of MAX86150_PPG_PULSEWIDTH_*
	max86150_bit_mask(
		MAX86150_PPGCONFIG1, MAX86150_PPG_PULSEWIDTH_MASK, pulseWidth
	);
}

// NOTE: Amplitude values: 0x00 = 0mA, 0x7F = 25.4mA, 0xFF = 50mA (typical)
// See datasheet, page 21
void max86150_set_led_red_amplitude(uint8_t amplitude)
{
	max86150_write_register(
		MAX86150_ADDRESS, MAX86150_LED2_PULSEAMP, amplitude
	);
	max86150_bit_mask(
		MAX86150_LED_RANGE,
		MAX86150_LED2_RANGE_MASK,
		MAX86150_LED2_RANGE_50
	);
}

void max86150_set_led_ir_amplitude(uint8_t amplitude)
{
	max86150_write_register(
		MAX86150_ADDRESS, MAX86150_LED1_PULSEAMP, amplitude
	);
	max86150_bit_mask(
		MAX86150_LED_RANGE,
		MAX86150_LED1_RANGE_MASK,
		MAX86150_LED1_RANGE_50
	);
}

void max86150_set_led_proximity_amplitude(uint8_t amplitude)
{
	max86150_write_register(
		MAX86150_ADDRESS, MAX86150_LED_PROX_AMP, amplitude
	);
}

void max86150_set_proximity_threshold(uint8_t threshMSB)
{
	// The threshMSB signifies only the 8 most significant-bits of the ADC count.
	max86150_write_register(
		MAX86150_ADDRESS, MAX86150_PROXINTTHRESH, threshMSB
	);
}

//Given a slot number assign a thing to it
//Devices are SLOT_RED_LED or SLOT_RED_PILOT (proximity)
//Assigning a SLOT_RED_LED will pulse LED
//Assigning a SLOT_RED_PILOT will ??
void max86150_fifo_enable_slot(uint8_t slotNumber, uint8_t device)
{
	switch (slotNumber) {
	case (1):
		max86150_bit_mask(
			MAX86150_FIFOCONTROL1, MAX86150_SLOT1_MASK, device
		);
		break;
	case (2):
		max86150_bit_mask(
			MAX86150_FIFOCONTROL1,
			MAX86150_SLOT2_MASK,
			device << 4
		);
		break;
	case (3):
		max86150_bit_mask(
			MAX86150_FIFOCONTROL2, MAX86150_SLOT3_MASK, device
		);
		break;
	case (4):
		max86150_bit_mask(
			MAX86150_FIFOCONTROL2,
			MAX86150_SLOT4_MASK,
			device << 4
		);
		break;
	default:
		//Shouldn't be here!
		break;
	}
}

//Clears all slot assignments
void max86150_disableSlots(void)
{
	max86150_write_register(MAX86150_ADDRESS, MAX86150_FIFOCONTROL1, 0);
	max86150_write_register(MAX86150_ADDRESS, MAX86150_FIFOCONTROL2, 0);
}

//
// FIFO Configuration
//

void max86150_set_ppg_averaging(uint8_t numberOfSamples)
{
	max86150_bit_mask(
		MAX86150_FIFOCONFIG, MAX86150_SAMPLEAVG_MASK, numberOfSamples
	);
}

//Resets all points to start in a known state
void max86150_clear_fifo(void)
{
	max86150_write_register(MAX86150_ADDRESS, MAX86150_FIFOWRITEPTR, 0);
	max86150_write_register(MAX86150_ADDRESS, MAX86150_FIFOOVERFLOW, 0);
	max86150_write_register(MAX86150_ADDRESS, MAX86150_FIFOREADPTR, 0);
}

//Enable roll over if FIFO over flows
void max86150_set_fifo_rollover(bool enabled)
{
	if (enabled) {
		max86150_bit_mask(
			MAX86150_FIFOCONFIG,
			MAX86150_ROLLOVER_MASK,
			MAX86150_ROLLOVER_ENABLE
		);
	} else {
		max86150_bit_mask(
			MAX86150_FIFOCONFIG,
			MAX86150_ROLLOVER_MASK,
			MAX86150_ROLLOVER_DISABLE
		);
	}
}

//Enable fifo almost full flag clear on data read
void max86150_set_fifo_almost_full_clear(bool enabled)
{
	if (enabled) {
		max86150_bit_mask(
			MAX86150_FIFOCONFIG,
			MAX86150_ALMOST_FULL_CLEAR_MASK,
			MAX86150_ALMOST_FULL_CLEAR_ENABLE
		);
	} else {
		max86150_bit_mask(
			MAX86150_FIFOCONFIG,
			MAX86150_ALMOST_FULL_CLEAR_MASK,
			MAX86150_ALMOST_FULL_CLEAR_DISABLE
		);
	}
}

//Enable fifo almost full flag repeated assertion
void max86150_set_fifo_almost_full_repeat(bool enabled)
{
	if (enabled) {
		max86150_bit_mask(
			MAX86150_FIFOCONFIG,
			MAX86150_ALMOST_FULL_REPEAT_MASK,
			MAX86150_ALMOST_FULL_REPEAT_ENABLE
		);
	} else {
		max86150_bit_mask(
			MAX86150_FIFOCONFIG,
			MAX86150_ALMOST_FULL_REPEAT_MASK,
			MAX86150_ALMOST_FULL_REPEAT_DISABLE
		);
	}
}

//Power on default is 32 samples
//Note it is reverse: 0x00 is 32 samples, 0x0F is 17 samples
void max86150_set_fifo_almost_full(uint8_t numberOfSamples)
{
	max86150_bit_mask(
		MAX86150_FIFOCONFIG, MAX86150_A_FULL_MASK, numberOfSamples
	);
}

//Read the FIFO Write Pointer
uint8_t max86150_get_fifo_write_pointer(void)
{
	return (max86150_read_register(MAX86150_ADDRESS, MAX86150_FIFOWRITEPTR));
}

//Read the FIFO Read Pointer
uint8_t max86150_get_fifo_read_pointer(void)
{
	return (max86150_read_register(MAX86150_ADDRESS, MAX86150_FIFOREADPTR));
}

//
// Device ID and Revision
//
uint8_t max86150_read_part_id()
{
	return max86150_read_register(MAX86150_ADDRESS, MAX86150_PARTID);
}

//Set ecg sample rate
void max86150_set_ecg_sample_rate(uint8_t sampleRate)
{
	// sampleRate: one of MAX86150_ECG_SAMPLERATE_*
	max86150_bit_mask(
		MAX86150_ECG_CONFIG1, MAX86150_ECG_SAMPLERATE_MASK, sampleRate
	);
}

//Set ecg pga gain
void max86150_set_ecg_pga_gain(uint8_t gain)
{
	// sampleRate: one of MAX86150_ECG_PGA_GAIN_*
	max86150_bit_mask(
		MAX86150_ECG_CONFIG3, MAX86150_ECG_PGA_GAIN_MASK, gain
	);
}

//Set ecg pga gain
void max86150_set_ecg_instrumentation_amplifier_gain(uint8_t gain)
{
	// sampleRate: one of MAX86150_ECG_IA_GAIN_*
	max86150_bit_mask(
		MAX86150_ECG_CONFIG3, MAX86150_ECG_IA_GAIN_MASK, gain
	);
}

//Setup the sensor
//The MAX86150 has many settings.
//Use the default max86150_setup if you are just getting started with the MAX86150 sensor
void max86150_setup(const uint8_t ppg_sample_rate)
{
	max86150_soft_reset();
	max86150_set_ppg_averaging(MAX86150_SAMPLEAVG_4);
	max86150_set_fifo_rollover(true);
	max86150_set_fifo_almost_full(8);
	max86150_set_fifo_almost_full_clear(true);
	max86150_set_fifo_almost_full_repeat(true);

	max86150_fifo_enable_slot(1, MAX86150_SLOT_RED_LED);
	max86150_fifo_enable_slot(2, MAX86150_SLOT_IR_LED);
	max86150_fifo_enable_slot(3, MAX86150_SLOT_ECG);
	//max86150_fifo_enable_slot(4, MAX86150_SLOT_NONE);

	max86150_set_ppg_adc_range(MAX86150_ADCRANGE_16384);
	max86150_set_ppg_sample_rate(ppg_sample_rate);
	max86150_set_ppg_pulse_width(MAX86150_PPG_PULSEWIDTH_100);

	max86150_set_led_ir_amplitude(0x66);
	max86150_set_led_red_amplitude(0x66);

	max86150_set_ecg_sample_rate(MAX86150_ECG_SAMPLERATE_200);
	max86150_set_ecg_pga_gain(MAX86150_ECG_PGA_GAIN_8);
	max86150_set_ecg_instrumentation_amplifier_gain(
		MAX86150_ECG_IA_GAIN_9_5
	);

	max86150_set_int_datardy(false);
	max86150_set_int_full(true);

	max86150_clear_fifo(); //Reset the FIFO before we begin checking the sensor

	max86150_set_fifo_enable(true);
}

//Tell caller how many samples are max86150_available
uint8_t max86150_available(void)
{
	int8_t numberOfSamples = sense.head - sense.tail;
	if (numberOfSamples < 0)
		numberOfSamples += STORAGE_SIZE;

	return (numberOfSamples);
}

//Report the most recent red value
uint32_t max86150_get_red(void)
{
	//Check the sensor for new data for 250ms
	if (max86150_safeCheck(250))
		return (sense.red[sense.head]);
	else
		return (0); //Sensor failed to find new data
}

//Report the most recent IR value
uint32_t max86150_get_ir(void)
{
	//Check the sensor for new data for 250ms
	if (max86150_safeCheck(250))
		return (sense.IR[sense.head]);
	else
		return (0); //Sensor failed to find new data
}

//Report the most recent Green value
int32_t max86150_get_ecg(void)
{
	//Check the sensor for new data for 250ms
	if (max86150_safeCheck(250))
		return (sense.ecg[sense.head]);
	else
		return (0); //Sensor failed to find new data
}

//Report the next Red value in the FIFO
uint32_t max86150_get_fifo_red(void)
{
	return (sense.red[sense.tail]);
}

//Report the next IR value in the FIFO
uint32_t max86150_get_fifo_ir(void)
{
	return (sense.IR[sense.tail]);
}

//Report the next Green value in the FIFO
int32_t max86150_get_fifo_ecg(void)
{
	return (sense.ecg[sense.tail]);
}

//Advance the tail
void max86150_next_sample(void)
{
	if (max86150_available()) { //Only advance the tail if new data is max86150_available
		sense.tail++;
		sense.tail %= STORAGE_SIZE; //Wrap condition
	}
}

//Polls the sensor for new data
//Call regularly
//If new data is max86150_available, it updates the head and tail in the main struct
//Returns number of new samples obtained
uint8_t max86150_get_sample(uint32_t *red, uint32_t *ir, int32_t *ecg)
{
	//Read register FIDO_DATA in (3-byte * number of active LED) chunks
	//Until FIFO_RD_PTR = FIFO_WR_PTR

	byte readPointer  = max86150_get_fifo_read_pointer();
	byte writePointer = max86150_get_fifo_write_pointer();

	int numberOfSamples = 0;

	//Do we have new data?
	if (readPointer != writePointer) {
		//Calculate the number of readings we need to get from sensor
		numberOfSamples = writePointer - readPointer;
		if (numberOfSamples < 0)
			numberOfSamples += 32; //Wrap condition

		//Get ready to read a burst of data from the FIFO register
		uint8_t command[] = { MAX86150_FIFODATA };

		// Important! true is for repeated start (since we are not reading complete fifo)
		// See https://os.mbed.com/users/laserdad/code/MAX86150_ECG_PPG//file/3c728f3d1f10/main.cpp/
		I2C_MasterWrite(
			MXC_I2C1_BUS0, MAX86150_ADDRESS << 1, command, 1, true
		);

		if (numberOfSamples > 0) {
			uint8_t data[3 * 3];
			I2C_MasterRead(
				MXC_I2C1_BUS0,
				MAX86150_ADDRESS << 1,
				data,
				3 * 3,
				0
			);

			*red = (data[0] << 16) | (data[1] << 8) | (data[2]);
			*ir  = (data[3] << 16) | (data[4] << 8) | (data[5]);
			*ecg = (data[6] << 16) | (data[7] << 8) | (data[8]);
		}

	}                         //End readPtr != writePtr
	return (numberOfSamples); //Let the world know how much new data we found
}
//Polls the sensor for new data
//Call regularly
//If new data is max86150_available, it updates the head and tail in the main struct
//Returns number of new samples obtained
uint16_t max86150_check(void)
{
	//Read register FIDO_DATA in (3-byte * number of active LED) chunks
	//Until FIFO_RD_PTR = FIFO_WR_PTR

	byte readPointer  = max86150_get_fifo_read_pointer();
	byte writePointer = max86150_get_fifo_write_pointer();

	int numberOfSamples = 0;

	//Do we have new data?
	if (readPointer != writePointer) {
		//Calculate the number of readings we need to get from sensor
		numberOfSamples = writePointer - readPointer;
		if (numberOfSamples < 0)
			numberOfSamples += 32; //Wrap condition

		//We now have the number of readings, now calc bytes to read
		//For this example we are just doing Red and IR (3 bytes each)
		int bytesLeftToRead = numberOfSamples * activeDevices * 3;

		//Get ready to read a burst of data from the FIFO register
		uint8_t command[] = { MAX86150_FIFODATA };
		I2C_MasterWrite(
			MXC_I2C1_BUS0, MAX86150_ADDRESS << 1, command, 1, 0
		);

		//We may need to read as many as 288 bytes so we read in blocks no larger than I2C_BUFFER_LENGTH
		//I2C_BUFFER_LENGTH changes based on the platform. 64 bytes for SAMD21, 32 bytes for Uno.
		//Wire.requestFrom() is limited to BUFFER_LENGTH which is 32 on the Uno
		while (bytesLeftToRead > 0) {
			int toGet = bytesLeftToRead;
			if (toGet > I2C_BUFFER_LENGTH) {
				//If toGet is 32 this is bad because we read 9 bytes (Red+IR+ECG * 3 = 9) at a time
				//32 % 9 = 5 left over. We don't want to request 32 bytes, we want to request 27.
				//32 % 9 (Red+IR+GREEN) = 5 left over. We want to request 27.

				toGet = I2C_BUFFER_LENGTH -
					(I2C_BUFFER_LENGTH %
					 (activeDevices *
					  3)); //Trim toGet to be a multiple of the samples we need to read
			}

			bytesLeftToRead -= toGet;

			//Request toGet number of bytes from sensor
			//_i2cPort->requestFrom(MAX86150_ADDRESS, toGet);
			uint8_t data[bytesLeftToRead];
			uint8_t *p = data;
			I2C_MasterRead(
				MXC_I2C1_BUS0,
				MAX86150_ADDRESS << 1,
				data,
				bytesLeftToRead,
				0
			);

			while (bytesLeftToRead > 0) {
				sense.head++; //Advance the head of the storage struct
				sense.head %= STORAGE_SIZE; //Wrap condition

				byte temp[sizeof(
					uint32_t)]; //Array of 4 bytes that we will convert into long
				uint32_t tempLong;

				//Burst read three bytes - RED
				temp[3] = 0;
				temp[2] = *p++;
				temp[1] = *p++;
				temp[0] = *p++;

				//Convert array to long
				memcpy(&tempLong, temp, sizeof(tempLong));

				tempLong &= 0x7FFFF; //Zero out all but 18 bits

				sense.red[sense.head] =
					tempLong; //Store this reading into the sense array

				if (activeDevices > 1) {
					//Burst read three more bytes - IR
					temp[3] = 0;
					temp[2] = *p++;
					temp[1] = *p++;
					temp[0] = *p++;

					//Convert array to long
					memcpy(&tempLong,
					       temp,
					       sizeof(tempLong));
					//Serial.println(tempLong);
					tempLong &=
						0x7FFFF; //Zero out all but 18 bits

					sense.IR[sense.head] = tempLong;
				}

				if (activeDevices > 2) {
					//Burst read three more bytes - ECG
					int32_t tempLongSigned;
					temp[3] = 0;
					temp[2] = *p++;
					temp[1] = *p++;
					temp[0] = *p++;

					//Serial.println(tempLong);
					//Convert array to long
					memcpy(&tempLongSigned,
					       temp,
					       sizeof(tempLongSigned));
					//tempLong &= 0x3FFFF; //Zero out all but 18 bits
					sense.ecg[sense.head] = tempLongSigned;
				}

				bytesLeftToRead -= activeDevices * 3;
			}
		}                 //End while (bytesLeftToRead > 0)
	}                         //End readPtr != writePtr
	return (numberOfSamples); //Let the world know how much new data we found
}

//Check for new data but give up after a certain amount of time
//Returns true if new data was found
//Returns false if new data was not found
bool max86150_safe_check(uint8_t max_tries)
{
	uint8_t tries = 0;

	while (tries < max_tries) {
		if (max86150_check() == true) { //We found new data!
			return (true);
		}

		tries++;
		delay(1);
	}
	return false;
}

//Given a register, read it, mask it, and then set the thing
void max86150_bit_mask(uint8_t reg, uint8_t mask, uint8_t thing)
{
	// Grab current register context
	uint8_t originalContents =
		max86150_read_register(MAX86150_ADDRESS, reg);

	// Zero-out the portions of the register we're interested in
	originalContents = originalContents & mask;

	// Change contents
	max86150_write_register(
		MAX86150_ADDRESS, reg, originalContents | thing
	);
}

uint8_t max86150_read_register(uint8_t address, uint8_t reg)
{
	uint8_t tempData  = 0;
	uint8_t command[] = { reg };
	// Important! true is for repeated start (since we are not reading complete fifo)
	// See https://os.mbed.com/users/laserdad/code/MAX86150_ECG_PPG//file/3c728f3d1f10/main.cpp/
	I2C_MasterWrite(MXC_I2C1_BUS0, address << 1, command, 1, true);

	I2C_MasterRead(MXC_I2C1_BUS0, address << 1, &tempData, 1, 0);

	return tempData;
}

void max86150_write_register(uint8_t address, uint8_t reg, uint8_t value)
{
	uint8_t command[] = { reg, value };
	I2C_MasterWrite(MXC_I2C1_BUS0, address << 1, command, 2, 0);
}