diff --git a/UAV-ControlSystem/inc/drivers/barometer.h b/UAV-ControlSystem/inc/drivers/barometer.h index de18b4a..2c47476 100644 --- a/UAV-ControlSystem/inc/drivers/barometer.h +++ b/UAV-ControlSystem/inc/drivers/barometer.h @@ -19,7 +19,7 @@ double barometer_GetCurrentPreassure(); double barometer_GetCurrentTemperature(); -double barometer_GetCurrentAltitude(); +float barometer_GetCurrentAltitudeBasedOnSeaLevel(); diff --git a/UAV-ControlSystem/src/drivers/I2C.c b/UAV-ControlSystem/src/drivers/I2C.c index 092916f..737946c 100644 --- a/UAV-ControlSystem/src/drivers/I2C.c +++ b/UAV-ControlSystem/src/drivers/I2C.c @@ -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); diff --git a/UAV-ControlSystem/src/drivers/barometer.c b/UAV-ControlSystem/src/drivers/barometer.c index 932cec7..e7f1d5a 100644 --- a/UAV-ControlSystem/src/drivers/barometer.c +++ b/UAV-ControlSystem/src/drivers/barometer.c @@ -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; } diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index 03bc4b0..56f2554 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -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)