Working software ic2 barometer
Can read from baro and get temp, preassure and alt. Will look into optimizations.
This commit is contained in:
Jonas Holmberg
parent
f35617c92d
commit
ec395d11e0
@ -19,7 +19,7 @@ double barometer_GetCurrentPreassure();
|
||||
|
||||
double barometer_GetCurrentTemperature();
|
||||
|
||||
double barometer_GetCurrentAltitude();
|
||||
float barometer_GetCurrentAltitudeBasedOnSeaLevel();
|
||||
|
||||
|
||||
|
||||
|
||||
@ -55,9 +55,9 @@ bool i2c_configure(I2C_TypeDef *i2c,
|
||||
//Initialize pins for SCL and SDA
|
||||
GPIO_InitTypeDef GPIO_InitStruct;
|
||||
GPIO_InitStruct.Pin = sda_pin | scl_pin;
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
|
||||
GPIO_InitStruct.Pull = GPIO_PULLUP;
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_FAST;
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD/*GPIO_MODE_AF_PP*/;
|
||||
GPIO_InitStruct.Pull = GPIO_NOPULL/*GPIO_PULLUP*/;
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
|
||||
GPIO_InitStruct.Alternate = i2c_af;
|
||||
HAL_GPIO_Init(i2c_port, &GPIO_InitStruct);
|
||||
|
||||
|
||||
@ -8,6 +8,8 @@
|
||||
#include "drivers/barometer.h"
|
||||
#include "drivers/I2C.h"
|
||||
#include "stm32f4xx_revo.h"
|
||||
#include "drivers/system_clock.h"
|
||||
#include "math.h"
|
||||
|
||||
#define ADDR_WRITE 0xEE // Module address write mode
|
||||
#define ADDR_READ 0xEF // Module address read mode
|
||||
@ -25,6 +27,9 @@
|
||||
#define CMD_ADC_4096 0x08 // ADC OSR=4096
|
||||
#define CMD_PROM_RD 0xA0 // Prom read command
|
||||
|
||||
#define SEA_PRESS 1013.25 //default sea level pressure level in mb
|
||||
#define FTMETERS 0.3048 //convert feet to meters
|
||||
|
||||
#define Device_address_1 0x56
|
||||
|
||||
#define IO_CONFIG(mode, speed) ((mode) | (speed))
|
||||
@ -36,6 +41,7 @@ double baro_Preassure; // compensated pressure value (mB)
|
||||
double baro_Temperature; // compensated temperature value (degC)
|
||||
double baro_Altitude; // altitude (ft)
|
||||
double baro_S; // sea level barometer (mB)
|
||||
float altitudeCalibrationValue = 0;
|
||||
|
||||
uint8_t cobuf[3];
|
||||
|
||||
@ -61,7 +67,7 @@ bool IORead(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin)
|
||||
|
||||
static void TEST_I2C_delay(void)
|
||||
{
|
||||
volatile int i = 7;
|
||||
volatile int i = 1;
|
||||
while (i) {
|
||||
i--;
|
||||
}
|
||||
@ -81,64 +87,64 @@ static bool TEST_I2C_Start(void)
|
||||
{
|
||||
IOHi(I2C1_PORT, I2C1_SDA_PIN);
|
||||
IOHi(I2C1_PORT, I2C1_SCL_PIN);
|
||||
TEST_I2C_delay();
|
||||
asm ("nop"); // TEST_I2C_delay();
|
||||
if (!IORead(I2C1_PORT, I2C1_SDA_PIN)) {
|
||||
return false;
|
||||
}
|
||||
IOLo(I2C1_PORT, I2C1_SDA_PIN);
|
||||
TEST_I2C_delay();
|
||||
asm ("nop"); // TEST_I2C_delay();
|
||||
if (IORead(I2C1_PORT, I2C1_SDA_PIN)) {
|
||||
return false;
|
||||
}
|
||||
IOLo(I2C1_PORT, I2C1_SDA_PIN);
|
||||
TEST_I2C_delay();
|
||||
asm ("nop"); // TEST_I2C_delay();
|
||||
return true;
|
||||
}
|
||||
|
||||
static void TEST_I2C_Stop(void)
|
||||
{
|
||||
IOLo(I2C1_PORT, I2C1_SCL_PIN);
|
||||
TEST_I2C_delay();
|
||||
asm ("nop"); // TEST_I2C_delay();
|
||||
IOLo(I2C1_PORT, I2C1_SDA_PIN);
|
||||
TEST_I2C_delay();
|
||||
asm ("nop"); // TEST_I2C_delay();
|
||||
IOHi(I2C1_PORT, I2C1_SCL_PIN);
|
||||
TEST_I2C_delay();
|
||||
asm ("nop"); // TEST_I2C_delay();
|
||||
IOHi(I2C1_PORT, I2C1_SDA_PIN);
|
||||
TEST_I2C_delay();
|
||||
asm ("nop"); // TEST_I2C_delay();
|
||||
}
|
||||
|
||||
static void TEST_I2C_Ack(void)
|
||||
{
|
||||
IOLo(I2C1_PORT, I2C1_SCL_PIN);
|
||||
TEST_I2C_delay();
|
||||
asm ("nop"); // TEST_I2C_delay();
|
||||
IOLo(I2C1_PORT, I2C1_SDA_PIN);
|
||||
TEST_I2C_delay();
|
||||
asm ("nop"); // TEST_I2C_delay();
|
||||
IOHi(I2C1_PORT, I2C1_SCL_PIN);
|
||||
TEST_I2C_delay();
|
||||
asm ("nop"); // TEST_I2C_delay();
|
||||
IOLo(I2C1_PORT, I2C1_SCL_PIN);
|
||||
TEST_I2C_delay();
|
||||
asm ("nop"); // TEST_I2C_delay();
|
||||
}
|
||||
|
||||
static void TEST_I2C_NoAck(void)
|
||||
{
|
||||
IOLo(I2C1_PORT, I2C1_SCL_PIN);
|
||||
TEST_I2C_delay();
|
||||
asm ("nop"); // TEST_I2C_delay();
|
||||
IOHi(I2C1_PORT, I2C1_SDA_PIN);
|
||||
TEST_I2C_delay();
|
||||
asm ("nop"); // TEST_I2C_delay();
|
||||
IOHi(I2C1_PORT, I2C1_SCL_PIN);
|
||||
TEST_I2C_delay();
|
||||
asm ("nop"); // TEST_I2C_delay();
|
||||
IOLo(I2C1_PORT, I2C1_SCL_PIN);
|
||||
TEST_I2C_delay();
|
||||
asm ("nop"); // TEST_I2C_delay();
|
||||
}
|
||||
|
||||
static bool TEST_I2C_WaitAck(void)
|
||||
{
|
||||
IOLo(I2C1_PORT, I2C1_SCL_PIN);
|
||||
TEST_I2C_delay();
|
||||
asm ("nop"); // TEST_I2C_delay();
|
||||
IOHi(I2C1_PORT, I2C1_SDA_PIN);
|
||||
TEST_I2C_delay();
|
||||
asm ("nop"); // TEST_I2C_delay();
|
||||
IOHi(I2C1_PORT, I2C1_SCL_PIN);
|
||||
TEST_I2C_delay();
|
||||
asm ("nop"); // TEST_I2C_delay();
|
||||
if (IORead(I2C1_PORT, I2C1_SDA_PIN)) {
|
||||
IOLo(I2C1_PORT, I2C1_SCL_PIN);
|
||||
return false;
|
||||
@ -152,7 +158,7 @@ static void TEST_I2C_SendByte(uint8_t byte)
|
||||
uint8_t i = 8;
|
||||
while (i--) {
|
||||
IOLo(I2C1_PORT, I2C1_SCL_PIN);
|
||||
TEST_I2C_delay();
|
||||
asm ("nop"); // TEST_I2C_delay();
|
||||
if (byte & 0x80) {
|
||||
IOHi(I2C1_PORT, I2C1_SDA_PIN);
|
||||
}
|
||||
@ -160,9 +166,9 @@ static void TEST_I2C_SendByte(uint8_t byte)
|
||||
IOLo(I2C1_PORT, I2C1_SDA_PIN);
|
||||
}
|
||||
byte <<= 1;
|
||||
TEST_I2C_delay();
|
||||
asm ("nop"); // TEST_I2C_delay();
|
||||
IOHi(I2C1_PORT, I2C1_SCL_PIN);
|
||||
TEST_I2C_delay();
|
||||
asm ("nop"); // TEST_I2C_delay();
|
||||
}
|
||||
IOLo(I2C1_PORT, I2C1_SCL_PIN);
|
||||
}
|
||||
@ -176,9 +182,9 @@ static uint8_t TEST_I2C_ReceiveByte(void)
|
||||
while (i--) {
|
||||
byte <<= 1;
|
||||
IOLo(I2C1_PORT, I2C1_SCL_PIN);
|
||||
TEST_I2C_delay();
|
||||
asm ("nop"); // TEST_I2C_delay();
|
||||
IOHi(I2C1_PORT, I2C1_SCL_PIN);
|
||||
TEST_I2C_delay();
|
||||
asm ("nop"); // TEST_I2C_delay();
|
||||
if (IORead(I2C1_PORT, I2C1_SDA_PIN)) {
|
||||
byte |= 0x01;
|
||||
}
|
||||
@ -222,7 +228,7 @@ bool TEST_i2cRead(I2C_HandleTypeDef *hi2c, uint8_t addr, uint8_t reg, uint8_t le
|
||||
return true;
|
||||
}
|
||||
|
||||
bool TEST_i2cWrite(I2C_HandleTypeDef *hi2c, uint8_t addr, uint8_t reg, uint8_t data, uint8_t readWrite)
|
||||
bool TEST_i2cWrite(I2C_HandleTypeDef *hi2c, uint8_t addr, uint8_t reg, uint8_t data)
|
||||
{
|
||||
//just send the addres 0x77
|
||||
//write = 0, read = 1
|
||||
@ -329,7 +335,6 @@ void m_i2c_send(uint8_t cmd)
|
||||
|
||||
bool barometer_init()
|
||||
{
|
||||
//i2c_configure(I2C1, &baroI2C_handle, Device_address_1);
|
||||
//Initialize pins for SCL and SDA
|
||||
GPIO_InitTypeDef GPIO_InitStruct;
|
||||
GPIO_InitStruct.Pin = I2C1_SCL_PIN | I2C1_SDA_PIN;
|
||||
@ -338,14 +343,23 @@ bool barometer_init()
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
|
||||
HAL_GPIO_Init(I2C1_PORT, &GPIO_InitStruct);
|
||||
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool barometer_reset()
|
||||
{
|
||||
bool ack = false;
|
||||
uint8_t sig;
|
||||
// ack = TEST_i2cRead(I2C1, ADDRESS_BARO, CMD_PROM_RD, 1, &sig);
|
||||
// if (!ack)
|
||||
// return false;
|
||||
|
||||
TEST_i2cWrite(I2C1, ADDRESS_BARO, CMD_RESET, 1, 0);
|
||||
// ack = TEST_i2cRead(I2C1, ADDRESS_BARO, CMD_PROM_RD, 1, &sig);
|
||||
// if (!ack)
|
||||
// return false;
|
||||
|
||||
|
||||
TEST_i2cWrite(I2C1, ADDRESS_BARO, CMD_RESET, 1);
|
||||
HAL_Delay(2800);
|
||||
|
||||
for (int i = 0; i < 8; i++)
|
||||
{
|
||||
uint8_t rxbuf[2] = { 0, 0 };
|
||||
@ -353,113 +367,15 @@ bool barometer_init()
|
||||
coefficients_arr[i] = rxbuf[0] << 8 | rxbuf[1];
|
||||
}
|
||||
|
||||
|
||||
HAL_GPIO_DeInit(I2C1_PORT, I2C1_SCL_PIN);
|
||||
HAL_GPIO_DeInit(I2C1_PORT, I2C1_SDA_PIN);
|
||||
|
||||
|
||||
return i2c_configure(I2C1, &baroI2C_handle, Device_address_1);
|
||||
}
|
||||
|
||||
bool barometer_reset()
|
||||
{
|
||||
|
||||
|
||||
//Change to write mode and send reset command
|
||||
cobuf[0] = CMD_RESET;
|
||||
HAL_Delay(1000);
|
||||
//ool response = i2c_send(&baroI2C_handle, ADDRESS_BARO, (uint8_t*)cobuf[0], 1);
|
||||
int i = 0;
|
||||
uint32_t TempaddressW = 0b11101110;
|
||||
uint32_t TempaddressR = 0b11101110;
|
||||
uint32_t testValueW = ((uint8_t)((TempaddressW) & (~0x00000001U)));
|
||||
uint32_t testValR = ((uint8_t)((TempaddressR) | 0x00000001U));
|
||||
|
||||
HAL_I2C_Master_Transmit(&baroI2C_handle,TempaddressW,cobuf, 1,1000);
|
||||
|
||||
// for (i = 0; i < 256; i ++)
|
||||
// {
|
||||
// if(HAL_I2C_Master_Transmit(&baroI2C_handle,i,cobuf, 1,1000) == HAL_OK)
|
||||
// {
|
||||
// uint32_t testValue = ((uint8_t)((i) & (~0x00000001U)));
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
//
|
||||
// }
|
||||
// }
|
||||
|
||||
|
||||
|
||||
|
||||
HAL_Delay(3000);
|
||||
|
||||
for (int coef_num = 0; coef_num < 8; coef_num++)
|
||||
/* Read values and get a calibration value for height */
|
||||
/* Run loop 5 times since there are 5 state, also need delay to ensure values will be read */
|
||||
for (int i = 0; i < 5; i ++)
|
||||
{
|
||||
HAL_Delay(50);
|
||||
cobuf[0] = 0;
|
||||
cobuf[1] = 0;
|
||||
cobuf[2] = 0;
|
||||
cobuf[0] = CMD_PROM_RD + (coef_num * 2);
|
||||
HAL_I2C_Master_Transmit(&baroI2C_handle, TempaddressW, cobuf, 1, 1000); // send PROM READ command
|
||||
cobuf[0] = 0;
|
||||
HAL_I2C_Master_Receive(&baroI2C_handle, TempaddressR, cobuf, 2, 1000); //Read the adc values
|
||||
uint32_t rC = (cobuf[0] << 8) | cobuf[1];
|
||||
coefficients_arr[coef_num] = rC;
|
||||
barometer_CaclulateValues();
|
||||
HAL_Delay(10);
|
||||
}
|
||||
//m_i2c_send(CMD_PROM_RD + coef_num * 2); // send PROM READ command
|
||||
altitudeCalibrationValue = (((float)1 - (pow(((float)baro_Preassure / (float)SEA_PRESS), (float)0.190284))) * (float)145366.45) * FTMETERS;
|
||||
|
||||
|
||||
//
|
||||
// while(true)
|
||||
// {
|
||||
// i++;
|
||||
// //HAL_I2C_Mem_Write()
|
||||
// I2C1->CR1 |= (1 << 8);
|
||||
// HAL_Delay(10);
|
||||
// I2C1->SR1;
|
||||
//
|
||||
// uint32_t Tempaddress = 0b11101100;
|
||||
// I2C1->DR = Tempaddress;
|
||||
// HAL_Delay(10);
|
||||
// uint32_t compVal = (I2C1->SR1 & (1 << 10));
|
||||
// if (compVal != 0)
|
||||
// {
|
||||
// I2C1->SR1 &= ~(1 << 10);
|
||||
// I2C1->CR1 |= (1 << 9);
|
||||
// HAL_Delay(100);
|
||||
//
|
||||
// }
|
||||
// else if (compVal == 0)
|
||||
// {
|
||||
// break;
|
||||
// }
|
||||
// }
|
||||
//
|
||||
//
|
||||
// I2C1->SR2;
|
||||
//
|
||||
// I2C1->DR = CMD_RESET;
|
||||
|
||||
//I2C1->CR1 |= (1 << 9);
|
||||
|
||||
|
||||
HAL_Delay(1000);
|
||||
|
||||
if(HAL_I2C_IsDeviceReady(&baroI2C_handle, ADDRESS_BARO, 10,1000) == HAL_OK)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// m_i2c_send(CMD_RESET);
|
||||
// m_i2c_send(CMD_RESET);
|
||||
// m_i2c_send(CMD_RESET);
|
||||
|
||||
//wait for the reset sequence
|
||||
HAL_Delay(5);
|
||||
loadCoefs();
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -484,7 +400,7 @@ void barometer_CalculatePTA(uint32_t D1, uint32_t D2)
|
||||
|
||||
/* Calculate TEMP: Actual temperature -40 to 85 C: (2000 + dT * C6/2^23) */
|
||||
int32_t TEMP = 2000 + (int64_t)dT * (int64_t)coefficients_arr[6] / (int64_t)(1 << 23);
|
||||
baro_Temperature = TEMP = 100.0; //Assign the calculated temp to the holding variable
|
||||
baro_Temperature = (double)TEMP / 100.0; //Assign the calculated temp to the holding variable
|
||||
|
||||
/* Improvements for different temperatures */
|
||||
if (TEMP < 2000) //if temp is lower than 20 Celsius
|
||||
@ -503,6 +419,8 @@ void barometer_CalculatePTA(uint32_t D1, uint32_t D2)
|
||||
}
|
||||
/* Calculate pressure, temperature compensated pressure 10..1200mbar: ( (D1 * SENS/2^21 - OFF)2^15 ) */
|
||||
baro_Preassure = (double)(((((int64_t)D1 * SENS ) >> 21) - OFF) / (double) (1 << 15)) / 100.0;
|
||||
|
||||
|
||||
}
|
||||
|
||||
void barometer_CaclulateValues()
|
||||
@ -518,33 +436,38 @@ void barometer_CaclulateValues()
|
||||
static uint32_t D1 = 0;
|
||||
static uint32_t D2 = 0;
|
||||
uint8_t cobuf[3] = {0};
|
||||
|
||||
|
||||
uint32_t startTime;
|
||||
uint32_t endTime;
|
||||
|
||||
switch (currentCalculationState)
|
||||
{
|
||||
case CALCSTATE_D2_CALCULATION:
|
||||
HAL_I2C_Master_Transmit(&baroI2C_handle, ADDRESS_BARO, CMD_ADC_CONV + (CMD_ADC_D2 + CMD_ADC_4096),1,1000); // send conversion command
|
||||
TEST_i2cWrite(&baroI2C_handle, ADDRESS_BARO, CMD_ADC_CONV + (CMD_ADC_D2 + CMD_ADC_4096),1); // send conversion command
|
||||
currentCalculationState = CALCSTATE_D2_READ; //change the state so we will go to D2 read next time function is called
|
||||
break;
|
||||
case CALCSTATE_D2_READ:
|
||||
HAL_I2C_Master_Transmit(&baroI2C_handle, ADDRESS_BARO, CMD_ADC_READ, 1, 1000); //Tell the sensor we want to read
|
||||
HAL_I2C_Master_Receive(&baroI2C_handle, ADDRESS_BARO, cobuf, 3, 1000); //Read the adc values
|
||||
//TEST_i2cWrite(&baroI2C_handle, ADDRESS_BARO, CMD_ADC_READ, 1); //Tell the sensor we want to read
|
||||
//TEST_i2cRead(&baroI2C_handle, ADDRESS_BARO, cobuf, 3, 1000); //Read the adc values
|
||||
startTime = clock_get_us();
|
||||
TEST_i2cRead(I2C1, ADDRESS_BARO, CMD_ADC_READ, 3, cobuf); // send PROM READ command
|
||||
endTime = clock_get_us() - startTime;
|
||||
D2 = (cobuf[0] << 16) + (cobuf[1] << 8) + cobuf[2]; //Shift the values to the correct position for the 24 bit D2 value
|
||||
currentCalculationState = CALCSTATE_D1_CALCULATION;
|
||||
break;
|
||||
case CALCSTATE_D1_CALCULATION:
|
||||
HAL_I2C_Master_Transmit(&baroI2C_handle, ADDRESS_BARO, CMD_ADC_CONV + (CMD_ADC_D1 + CMD_ADC_4096),1, 1000); // send conversion command
|
||||
TEST_i2cWrite(&baroI2C_handle, ADDRESS_BARO, CMD_ADC_CONV + (CMD_ADC_D1 + CMD_ADC_4096),1); // send conversion command
|
||||
currentCalculationState = CALCSTATE_D1_READ; //change the state so we will go to D1 read next time function is called
|
||||
break;
|
||||
case CALCSTATE_D1_READ:
|
||||
HAL_I2C_Master_Transmit(&baroI2C_handle, ADDRESS_BARO, CMD_ADC_READ, 1, 1000); //Tell the sensor we want to read
|
||||
HAL_I2C_Master_Receive(&baroI2C_handle, ADDRESS_BARO, cobuf, 3, 1000); //Read the adc values
|
||||
//TEST_i2cWrite(&baroI2C_handle, ADDRESS_BARO, CMD_ADC_READ, 1); //Tell the sensor we want to read
|
||||
//HAL_I2C_Master_Receive(&baroI2C_handle, ADDRESS_BARO, cobuf, 3, 1000); //Read the adc values
|
||||
TEST_i2cRead(I2C1, ADDRESS_BARO, CMD_ADC_READ, 3, cobuf); // send PROM READ command
|
||||
D1 = (cobuf[0] << 16) + (cobuf[1] << 8) + cobuf[2]; //Shift the values to the correct position for the 24 bit D2 value
|
||||
currentCalculationState = CALCSTATE_CALCULATE_PTA;
|
||||
break;
|
||||
case CALCSTATE_CALCULATE_PTA:
|
||||
barometer_CalculatePTA(D1, D2);
|
||||
currentCalculationState = CALCSTATE_D2_CALCULATION;
|
||||
break;
|
||||
}
|
||||
|
||||
@ -560,9 +483,10 @@ double barometer_GetCurrentTemperature()
|
||||
return baro_Temperature;
|
||||
}
|
||||
|
||||
double barometer_GetCurrentAltitude()
|
||||
float barometer_GetCurrentAltitudeBasedOnSeaLevel()
|
||||
{
|
||||
|
||||
float feet = ((float)1 - (pow(((float)baro_Preassure / (float)SEA_PRESS), (float)0.190284))) * (float)145366.45;
|
||||
return (feet * FTMETERS) - altitudeCalibrationValue;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -41,6 +41,7 @@
|
||||
#include "drivers/accel_gyro.h"
|
||||
#include "drivers/motormix.h"
|
||||
#include "Flight/pid.h"
|
||||
#include "drivers/barometer.h"
|
||||
|
||||
void systemTaskGyroPid(void)
|
||||
{
|
||||
@ -183,7 +184,7 @@ void systemTaskBattery(void)
|
||||
|
||||
void systemTaskBaro(void)
|
||||
{
|
||||
//barometer_CaclulateValues();
|
||||
barometer_CaclulateValues();
|
||||
|
||||
|
||||
|
||||
@ -261,10 +262,15 @@ void systemTaskSonar(void)
|
||||
void systemTaskAltitude(void)
|
||||
{
|
||||
//Keep track of the vehicles current altitude, based on some sensor. In this case either barometer or sonar
|
||||
/*double temperature = barometer_GetCurrentTemperature();
|
||||
|
||||
|
||||
double temperature = barometer_GetCurrentTemperature();
|
||||
double pressure = barometer_GetCurrentPreassure();
|
||||
float altitute = barometer_GetCurrentAltitudeBasedOnSeaLevel();
|
||||
|
||||
char buffer[50];
|
||||
sprintf(buffer, "Temperature: %8.2f \n\r", temperature);
|
||||
usart_transmit(&cliUsart, buffer, 50, 1000000000);*/
|
||||
sprintf(buffer, "Temperature: %8.2f \n\r", altitute);
|
||||
usart_transmit(&cliUsart, buffer, 50, 1000000000);
|
||||
}
|
||||
|
||||
void systemTaskBeeper(void)
|
||||
|
||||
Reference in New Issue
Block a user