Merge pull request #1 from openppg/dev/lps22hb

Barometer support (LPS22HB)

Inc/baro_LPS22HB.h

@@ -1,8 +1,6 @@
 /*
  * baro_LPS22HB.h
  *
- *  Created on: Feb 12, 2019
- *      Author: MiguelFAlvarez
  */
 
 #ifndef BARO_LPS22HB_H_
@@ -16,10 +14,34 @@ typedef enum
 {
 	REG_WHOAMI = 0x0F,
 
+	REG_CTRL_REG1=0x10,
+	REG_CTRL_REG2=0x11,
+	REG_CTRL_REG3=0x12,
+
+	REG_STATUS=0x27,
+
+	REG_PRESS_OUT_XL=0x28,
+	REG_PRESS_OUT_L=0x29,
+	REG_PRESS_OUT_H=0x2A,
+
+	REG_TEMP_OUT_L=0x2B,
+	REG_TEMP_OUT_H=0x2C,
+
 }BARO_REGS;
 
+
+typedef enum
+{
+	ST_TOR=0x20,
+	ST_POR=0x10,
+
+	ST_TDA=0x02,
+	ST_PDA=0x01,
+}BARO_STATUS;
+
 uint8_t baroInit();
 void baroWriteReg(uint8_t regAddr, uint8_t data);
 uint8_t baroReadReg(uint8_t regAddr);
+uint32_t baroReadPressTemp(int16_t *temperature);
 
 #endif /* BARO_LPS22HB_H_ */

Inc/baro_proc.h

@@ -0,0 +1,15 @@
+/*
+ * baro_proc.h
+ *
+ *  Created on: Oct 23, 2019
+ */
+
+#ifndef BARO_PROC_H_
+#define BARO_PROC_H_
+
+void baroAvgInit();
+void baroSample();
+uint32_t baroGetAvg();
+int16_t baroGetTempAvg();
+
+#endif /* BARO_PROC_H_ */

Inc/controllerInterface.h

@@ -12,7 +12,7 @@
 
 //#define INTERFACE_CONTROLLER
 #define INTERFACE_HUB
-#define CTRL_VER 0x00
+#define CTRL_VER 0x01
 #define CTRL2HUB_ID	0x10
 #define HUB2CTRL_ID 0x20
 
@@ -23,7 +23,7 @@ typedef struct
 	uint8_t id;
 	uint8_t length;
 	uint8_t armed;
-	uint16_t throttlePercent;	//0 to 1000
+	uint16_t throttlePercent; //0 to 1000
 	uint16_t crc;
 }STR_CTRL2HUB_MSG;
 
@@ -39,6 +39,8 @@ typedef struct
 	uint16_t avgRpm;
 	uint8_t avgCapTemp;
 	uint8_t avgFetTemp;
+	int16_t baroTemp; // degrees c
+	uint32_t baroPressure; // hpa
 	uint16_t crc;
 }STR_HUB2CTRL_MSG;
 

Src/baro_LPS22HB.c

@@ -1,26 +1,80 @@
 /*
  * baro_LPS22HB.c
  *
- *  Created on: Feb 12, 2019
- *      Author: MiguelFAlvarez
  */
 
 #include "main.h"
 #include "baro_LPS22HB.h"
 #include "spi.h"
 
+static uint8_t baroInitOk=0;
+
 uint8_t baroInit()
 {
 	if(baroReadReg(REG_WHOAMI) != WHO_AM_I_LPS22HB)
+	{
+		baroInitOk=0;
 		return 0;
+	}
 
-	//baroWriteReg()
-
+	baroInitOk=1;
 	return 1;
 }
 
+uint32_t baroReadPressTemp(int16_t *temperature)
+{
+	if(!baroInitOk)
+	{
+		return 0xffffffff;
+	}
+
+	baroWriteReg(REG_CTRL_REG2,0x01);// start one shot conversion
+
+	HAL_Delay(15); // wait 15 ms
+
+	int status=baroReadReg(REG_STATUS);
+
+	while(!((status&ST_TDA)&&(status&ST_PDA))) // wait for data ready flag
+	{
+		HAL_Delay(5); // delay more times
+		status=baroReadReg(REG_STATUS);
+	}
+
+	// read back all value registers (total 5)
+
+	uint8_t dataRx[6];
+
+	for(int i=0;i<5;i++)
+	{
+		dataRx[i+1]=baroReadReg(i+REG_PRESS_OUT_XL);
+	}
+
+
+	int32_t unscaled_pressure=dataRx[1]|(dataRx[2]<<8)|(dataRx[3]<<16);
+
+	/* convert the 2's complement 24 bit to 2's complement 32 bit */
+	if(unscaled_pressure & 0x00800000)
+	{
+		unscaled_pressure |= 0xFF000000;
+	}
+
+
+	int16_t unscaled_temperature=dataRx[4]|(dataRx[5]<<8);
+	*temperature=unscaled_temperature;
+
+	return unscaled_pressure*100/4096;
+}
+
+
 void baroWriteReg(uint8_t regAddr, uint8_t data)
 {
+	uint8_t dataTx[2];
+
+	dataTx[0] = (regAddr & 0x7F);
+	dataTx[1] = data;
+	SPI_BARO_SELECT;
+	SPIx_WriteData(dataTx, sizeof(dataTx));
+	SPI_BARO_DESELECT;
 
 }
 

Src/baro_proc.c

@@ -0,0 +1,70 @@
+#include "main.h"
+#include "baro_LPS22HB.h"
+
+#define BARO_AVG_COUNT 4
+
+static uint32_t baro_value[BARO_AVG_COUNT];
+static uint32_t baro_temp_value[BARO_AVG_COUNT];
+static int32_t baro_value_idx;
+
+/**
+  * @brief Initialize baro (LPS22HB)
+  * @retval None
+  */
+void baroAvgInit()
+{
+	int16_t init_temp_value;
+	uint32_t init_value= baroReadPressTemp(&init_temp_value);
+
+	// set all buf to the inital value
+	for(int i=0;i<BARO_AVG_COUNT;i++)
+	{
+		baro_value[i]=init_value;
+		baro_temp_value[i]=init_temp_value;
+	}
+
+	baro_value_idx=0;
+}
+
+void baroSample()
+{
+	int16_t baro_temp_reading;
+	uint32_t baro_reading= baroReadPressTemp(&baro_temp_reading);
+
+	if(baro_reading!=0xffffffff)
+	{// save the value if it is valid
+		baro_value[baro_value_idx]=baro_reading;
+		baro_temp_value[baro_value_idx]=baro_temp_reading;
+		baro_value_idx++;
+
+		if(baro_value_idx>=BARO_AVG_COUNT)
+		{
+			baro_value_idx=0;
+		}
+	}
+
+}
+
+uint32_t baroGetAvg()
+{
+	uint32_t total=0;
+
+	for(int i=0;i<BARO_AVG_COUNT;i++)
+	{
+		total+=baro_value[i];
+	}
+
+	return total/BARO_AVG_COUNT;
+}
+
+int16_t baroGetTempAvg()
+{
+	int32_t total=0;
+
+	for(int i=0;i<BARO_AVG_COUNT;i++)
+	{
+		total+=baro_temp_value[i];
+	}
+
+	return total/BARO_AVG_COUNT;
+}

Src/controllerInterface.c

@@ -11,6 +11,8 @@
 #include "tim.h"
 #include "escTelemetry.h"
 
+#include "baro_proc.h"
+
 static STR_CTRL2HUB_MSG controlData;
 static STR_HUB2CTRL_MSG hubData;
 static uint8_t armed = 0;
@@ -28,10 +30,7 @@ void sendControlData(uint8_t armed, uint16_t throttlePercent)
 	UART_RS485_BeginTransmit(HUB_UART, HUB_TX_DMA, HUB_TX_DMA_CH, (uint8_t*)&controlData, sizeof(STR_CTRL2HUB_MSG));
 }
 
-void receiveHubData(uint8_t *buf, uint32_t size)
-{
-
-}
+void receiveHubData(uint8_t *buf, uint32_t size){}
 #endif
 
 #ifdef INTERFACE_HUB
@@ -73,6 +72,10 @@ void sendHubData()
 	hubData.avgRpm = escGetAvgRpm();
 	hubData.avgCapTemp = escGetAvgCapTemp();
 	hubData.avgFetTemp = escGetAvgFetTemp();
+
+	hubData.baroTemp = baroGetTempAvg();
+	hubData.baroPressure = baroGetAvg();
+
 	hubData.crc = crc16((uint8_t*)&hubData, sizeof(STR_HUB2CTRL_MSG) - 2);
 
 	sendToController((uint8_t*)&hubData, sizeof(STR_HUB2CTRL_MSG));

Src/escTelemetry.c

@@ -20,7 +20,7 @@ void escInit()
 		escData[i].timeStamp = 0;
 		escData[i].packetNum = 0;
 	}
-	//Future esc's might require some sort of telemetry initialization data exchange
+	// Future esc's might require some sort of telemetry initialization data exchange
 }
 
 ESC_STATUS escParseData(uint8_t *data, uint8_t size, uint8_t escIndex)
@@ -29,7 +29,6 @@ ESC_STATUS escParseData(uint8_t *data, uint8_t size, uint8_t escIndex)
 	//if(size > ESC_HW_XROTOR_PACKET_LENGTH || escIndex > NUM_ESCS)
 	//	return ESC_STATUS_INVALID_DATA;
 
-	//memcpy((uint8_t*)&escPacket, data, size);
 	uint32_t currentTime = HAL_GetTick();
 	static uint32_t lastTime[NUM_ESCS];
 	static uint32_t mahRaw[NUM_ESCS];

Src/imu_LSM9DS1.c

@@ -14,9 +14,6 @@ uint8_t imuInit()
 	if(imuReadReg(REG_IMU_WHOAMI) != WHO_AM_I_LSM9DS1_IMU)
 		return 0;
 
-
-	//imuWriteReg()
-
 	return 1;
 }
 
@@ -44,8 +41,6 @@ uint8_t magInit()
 	if(magReadReg(REG_MAG_WHOAMI) != WHO_AM_I_LSM9DS1_MAG)
 		return 0;
 
-	//magWriteReg()
-
 	return 1;
 }
 

Src/main.c

@@ -65,6 +65,7 @@
 #include "flash.h"
 #include "controllerInterface.h"
 #include "baro_LPS22HB.h"
+#include "baro_proc.h"
 #include "imu_LSM9DS1.h"
 /* USER CODE END Includes */
 
@@ -181,6 +182,8 @@ int main(void)
   IWDG_SetPrescaler(IWDG_PRESCALER_32);
   WDT_RESET;
 
+  baroAvgInit();
+
   HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);
   HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_2);
   HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_3);
@@ -233,7 +236,7 @@ int main(void)
 
     /* USER CODE BEGIN 3 */
 	  WDT_RESET;
-	uint32_t currentTime = HAL_GetTick();
+	  uint32_t currentTime = HAL_GetTick();
 
 	  static uint32_t lastTime = 0;
 	  static uint32_t blinkTime = 510;
@@ -255,10 +258,19 @@ int main(void)
 		  else
 			  blinkTime = 500;
 
+	  }
+
+	  static const uint32_t baroSampleInterval = 250;
+	  static uint32_t lastBaroSampleTime = 0;
 
+	  if(currentTime - lastBaroSampleTime > baroSampleInterval)
+	  {
+		  lastBaroSampleTime=currentTime;
+
+		  baroSample();
 	  }
 
-	  //Log data in CSV format
+	  // Log data in CSV format
 	  if(logStatus == FR_OK && armedState)
 	  {
 		  uint32_t startWriteTimeAll = HAL_GetTick();
@@ -268,7 +280,7 @@ int main(void)
 		  memset(logData, 0, sizeof(logData));
 		  for(uint8_t i = 0; i<NUM_ESCS; i++)
 		  {
-			  //Dont log the same data twice
+			  // Dont log the same data twice
 			  if(escData[i].timeStamp > 0 && lastPacketTime[i] != escData[i].timeStamp)
 			  {
 				  sprintf(logData+strlen(logData), "%u,%u,%u,%u,%u,%u,%u,%u,%u,%u\n", escData[i].timeStamp, i, escData[i].packetNum, escData[i].throttleInput, escData[i].rpm, escData[i].voltage, escData[i].current, escData[i].mah, escData[i].capTemp, escData[i].fetTemp);
@@ -277,6 +289,10 @@ int main(void)
 			  //logStatus = logWriteData(fileName, (uint8_t*)logData, (i+1)*256, bytesWritten);
 			  //debugTime[i] = HAL_GetTick() - startWriteTime;
 		  }
+
+		  // write pressure and temp data to log
+		  sprintf(logData+strlen(logData), "pressure %u  temp %d\n", baroGetAvg(), baroGetTempAvg());
+
 		  if(strlen(logData)>1)
 		  {
 			  logStatus = logWriteData((uint8_t*)logData, strlen(logData), bytesWritten);