From 50a860a3806c2b144cefbcde8d8509ff19c2bdf8 Mon Sep 17 00:00:00 2001
From: Matthew Kennedy <matthewkennedy@outlook.com>
Date: Sat, 6 Feb 2021 21:59:44 -0800
Subject: [PATCH] implement lps25 (#2301)

* bb

* implement

* tweaks

* s

* comment, format

* guard

* implement lps25

* headers and comment

* sensor works

* headers

* guard

Co-authored-by: Matthew Kennedy <makenne@microsoft.com>
---
 firmware/hw_layer/sensors/lps25.cpp  | 103 +++++++++++++++++++++++++++
 firmware/hw_layer/sensors/lps25.h    |  29 ++++++++
 firmware/hw_layer/sensors/sensors.mk |   1 +
 3 files changed, 133 insertions(+)
 create mode 100644 firmware/hw_layer/sensors/lps25.cpp
 create mode 100644 firmware/hw_layer/sensors/lps25.h

diff --git a/firmware/hw_layer/sensors/lps25.cpp b/firmware/hw_layer/sensors/lps25.cpp
new file mode 100644
index 0000000000..678ab3ac23
--- /dev/null
+++ b/firmware/hw_layer/sensors/lps25.cpp
@@ -0,0 +1,103 @@
+/**
+ * @file        lps25.cpp
+ * @brief       Driver for the ST LPS25HB pressure sensor
+ *
+ * @date February 6, 2020
+ * @author Matthew Kennedy, (c) 2020
+ */
+
+#if EFI_PROD_CODE
+
+#include "lps25.h"
+
+static constexpr uint8_t addr = 0x5C;
+static constexpr uint8_t expectedWhoAmI = 0xBD;
+
+// Control register 1
+#define LPS_CR1_PD (1 << 7)
+#define LPS_CR1_ODR_25hz (4 << 4)
+#define LPS_CR1_BDU (1 << 2)
+
+// Status register flags
+#define LPS_SR_P_DA (1 << 1)	// Pressure data available
+
+#define REG_WhoAmI 0x0F
+#define REG_Cr1 0x20
+#define REG_Status 0x27
+#define REG_PressureOutXl 0x28
+#define REG_PressureOutL 0x29
+#define REG_PressureOutH 0x2A
+
+bool Lps25::init(brain_pin_e scl, brain_pin_e sda) {
+	m_i2c.init(scl, sda);
+
+	// Read ident register
+	auto whoAmI = m_i2c.readRegister(addr, REG_WhoAmI);
+	if (whoAmI != expectedWhoAmI) {
+		return false;
+	}
+
+	// Set the control registers
+	m_i2c.writeRegister(addr, REG_Cr1,
+		LPS_CR1_PD |		// Set to active mode
+		LPS_CR1_ODR_25hz |	// 25hz update rate
+
+		// TODO: should bdu be set?
+		LPS_CR1_BDU			// Output registers update only when read
+	);
+
+	m_hasInit = true;
+	return true;
+}
+
+expected<float> Lps25::readPressureKpa() {
+	// First read the status reg to check if there are data available
+	uint8_t sr = m_i2c.readRegister(addr, REG_Status);
+
+	bool hasPressure = sr & LPS_SR_P_DA;
+
+	if (!hasPressure) {
+		return unexpected;
+	}
+
+	/*
+	TODO: why doesn't this work?
+
+		// Sequential multi-byte reads need to set the high bit of the
+		// register address to enable multi-byte read
+		constexpr uint8_t readAddr = REG_PressureOutXl | 0x80;
+
+		uint8_t buffer[3];
+		m_i2c.writeRead(addr, &readAddr, 1, buffer, 3);
+
+		// Glue the 3 bytes back in to a 24 bit integer
+		uint32_t counts = buffer[2] << 16 | buffer[1] << 8 | buffer[0];
+	*/
+
+	auto xl = m_i2c.readRegister(addr, REG_PressureOutXl);
+	auto l = m_i2c.readRegister(addr, REG_PressureOutL);
+	auto h = m_i2c.readRegister(addr, REG_PressureOutH);
+
+	// Glue the 3 bytes back in to a 24 bit integer
+	uint32_t counts = h << 16 | l << 8 | xl;
+
+	// 4096 counts per hectopascal
+	// = 40960 counts per kilopascal
+	constexpr float ratio = 1 / 40960.0f;
+	
+	float kilopascal = counts * ratio;
+
+	// Sensor limits are 26-126 kPa
+	// The highest ever barometric pressure measured was only 108.3 kPa
+	// The pressure at the highest altitude road (Khardung La, India/Tibet) is at 5600 meters,
+	// which should have a nominal barometric pressure of around 50 kPa
+	// Anything outside that range is not a place we expect your engine to run, so we assume
+	// some sensing problem (sealed ECU case and high temperature?)
+	if (kilopascal > 120 || kilopascal < 50) {
+		return unexpected;
+	}
+
+	return kilopascal;
+}
+
+#endif // EFI_PROD_CODE
diff --git a/firmware/hw_layer/sensors/lps25.h b/firmware/hw_layer/sensors/lps25.h
new file mode 100644
index 0000000000..34efbe0542
--- /dev/null
+++ b/firmware/hw_layer/sensors/lps25.h
@@ -0,0 +1,29 @@
+/**
+ * @file        lps25.h
+ * @brief       Driver for the ST LPS25HB pressure sensor
+ *
+ * @date February 6, 2020
+ * @author Matthew Kennedy, (c) 2020
+ */
+
+#pragma once
+
+#if EFI_PROD_CODE
+
+#include "i2c_bb.h"
+
+#include "expected.h"
+
+class Lps25 {
+public:
+	// Returns true if the sensor was initialized successfully.
+	bool init(brain_pin_e scl, brain_pin_e sda);
+
+	expected<float> readPressureKpa();
+
+private:
+	BitbangI2c m_i2c;
+	bool m_hasInit = false;
+};
+
+#endif // EFI_PROD_CODE
diff --git a/firmware/hw_layer/sensors/sensors.mk b/firmware/hw_layer/sensors/sensors.mk
index 0a1a55975e..6c007ac4cd 100644
--- a/firmware/hw_layer/sensors/sensors.mk
+++ b/firmware/hw_layer/sensors/sensors.mk
@@ -8,4 +8,5 @@ HW_SENSORS_SRC = \
 	$(HW_SENSORS_DIR)/yaw_rate_sensor.cpp \
 	$(HW_SENSORS_DIR)/accelerometer.cpp \
 	$(HW_SENSORS_DIR)/joystick.cpp \
+	$(HW_SENSORS_DIR)/lps25.cpp \
 	
\ No newline at end of file