
// ****************************************************************************
//
//                                 Main code
//
// ****************************************************************************
/* Example code to talk to a MPU6050 MEMS accelerometer and gyroscope

   This is taking to simple approach of simply reading registers. It's perfectly
   possible to link up an interrupt line and set things up to read from the
   inbuilt FIFO to make it more useful.

   NOTE: Ensure the device is capable of being driven at 3.3v NOT 5v. The Pico
   GPIO (and therefor I2C) cannot be used at 5v.

   You will need to use a level shifter on the I2C lines if you want to run the
   board at 5v.

   Connections on Raspberry Pi Pico board, other boards may vary.

   GPIO 4 (pin 6)-> SDA on MPU6050 board
   GPIO 5 (pin 7)-> SCL on MPU6050 board
   3.3v (pin 36) -> VCC on MPU6050 board
   GND (pin 38)  -> GND on MPU6050 board
*/

#include "include.h"

// By default these devices  are on bus address 0x68
static int addr = 0x68;

#define I2C_PORT i2c0

static void mpu6050_reset()
{
	// Two byte reset. First byte register, second byte data
	// There are a load more options to set up the device in different ways that could be added here
	uint8_t buf[] = {0x6B, 0x00};
	i2c_write_blocking(I2C_PORT, addr, buf, 2, false);
}

static void mpu6050_read_raw(int16_t accel[3], int16_t gyro[3], int16_t *temp)
{
	// For this particular device, we send the device the register we want to read
	// first, then subsequently read from the device. The register is auto incrementing
	// so we don't need to keep sending the register we want, just the first.

	uint8_t buffer[6];

	// Start reading acceleration registers from register 0x3B for 6 bytes
	uint8_t val = 0x3B;
	i2c_write_blocking(I2C_PORT, addr, &val, 1, true); // true to keep master control of bus
	i2c_read_blocking(I2C_PORT, addr, buffer, 6, false);

	for (int i = 0; i < 3; i++)
	{
		accel[i] = (buffer[i * 2] << 8 | buffer[(i * 2) + 1]);
	}

	// Now gyro data from reg 0x43 for 6 bytes
	// The register is auto incrementing on each read
	val = 0x43;
	i2c_write_blocking(I2C_PORT, addr, &val, 1, true);
	i2c_read_blocking(I2C_PORT, addr, buffer, 6, false);  // False - finished with bus

	for (int i = 0; i < 3; i++)
	{
		gyro[i] = (buffer[i * 2] << 8 | buffer[(i * 2) + 1]);;
	}

	// Now temperature from reg 0x41 for 2 bytes
	// The register is auto incrementing on each read
	val = 0x41;
	i2c_write_blocking(I2C_PORT, addr, &val, 1, true);
	i2c_read_blocking(I2C_PORT, addr, buffer, 2, false);  // False - finished with bus

	*temp = buffer[0] << 8 | buffer[1];
}

int main()
{
	stdio_init_all();

	printf("Hello, MPU6050! Reading raw data from registers...\n");

	// This example will use I2C0 on GPIO4 (SDA) and GPIO5 (SCL) running at 400kHz.
	i2c_init(I2C_PORT, 400 * 1000);
	gpio_set_function(4, GPIO_FUNC_I2C);
	gpio_set_function(5, GPIO_FUNC_I2C);
	gpio_pull_up(4);
	gpio_pull_up(5);

	mpu6050_reset();

	int16_t acceleration[3], gyro[3], temp;

	while (1)
	{
		mpu6050_read_raw(acceleration, gyro, &temp);

		// These are the raw numbers from the chip, so will need tweaking to be really useful.
		// See the datasheet for more information
		printf("Acc. X = %d, Y = %d, Z = %d\n", acceleration[0], acceleration[1], acceleration[2]);
		printf("Gyro. X = %d, Y = %d, Z = %d\n", gyro[0], gyro[1], gyro[2]);
		// Temperature is simple so use the datasheet calculation to get deg C.
		// Note this is chip temperature.
		printf("Temp. = %f\n", (temp / 340.0) + 36.53);

		sleep_ms(100);
	}

	return 0;
}
