diff --git a/UAV-ControlSystem/inc/drivers/barometer.h b/UAV-ControlSystem/inc/drivers/barometer.h new file mode 100644 index 0000000..de18b4a --- /dev/null +++ b/UAV-ControlSystem/inc/drivers/barometer.h @@ -0,0 +1,26 @@ +/* + * barometer.h + * + * Created on: 18 okt. 2016 + * Author: holmis + */ + +#ifndef DRIVERS_BAROMETER_H_ +#define DRIVERS_BAROMETER_H_ + + +bool barometer_init(); + +bool barometer_reset(); + +void barometer_CaclulateValues(); + +double barometer_GetCurrentPreassure(); + +double barometer_GetCurrentTemperature(); + +double barometer_GetCurrentAltitude(); + + + +#endif /* DRIVERS_BAROMETER_H_ */ diff --git a/UAV-ControlSystem/src/drivers/I2C.c b/UAV-ControlSystem/src/drivers/I2C.c index ed643b0..c0120a2 100644 --- a/UAV-ControlSystem/src/drivers/I2C.c +++ b/UAV-ControlSystem/src/drivers/I2C.c @@ -63,7 +63,7 @@ bool i2c_configure(I2C_TypeDef *i2c, //Initialize I2C communication out_profile->Instance = i2c; - out_profile->Init.ClockSpeed = 100000; + out_profile->Init.ClockSpeed = 400000; out_profile->Init.DutyCycle = I2C_DUTYCYCLE_2; out_profile->Init.OwnAddress1 = my_address; out_profile->Init.OwnAddress2 = 0; @@ -118,8 +118,10 @@ bool i2c_send(I2C_HandleTypeDef* profile, uint8_t i = 0; // try to send the data 10 times and if no acknowledge is received during these 10 messages we stop trying so that // the system don't gets stuck forever because a slave is unreachable - while(HAL_I2C_Master_Transmit(profile,(slave_address << 1), (uint8_t*)data, length, 1000) != HAL_OK && i++ < 10) - {} +// while(HAL_I2C_Master_Transmit(profile,(slave_address << 1), (uint8_t*)data, length, 1000) != HAL_OK && i++ < 10) +// {} + while(HAL_I2C_Master_Transmit(profile, slave_address, (uint8_t*)data, length, 5000) != HAL_OK && i++ < 10) + {} //Wait til the I2C bus is done with all sending while (HAL_I2C_GetState(profile) != HAL_I2C_STATE_READY){} diff --git a/UAV-ControlSystem/src/drivers/barometer.c b/UAV-ControlSystem/src/drivers/barometer.c new file mode 100644 index 0000000..bd63463 --- /dev/null +++ b/UAV-ControlSystem/src/drivers/barometer.c @@ -0,0 +1,244 @@ +/* + * barometer.c + * + * Created on: 18 okt. 2016 + * Author: holmis + */ + +#include "drivers/barometer.h" +#include "drivers/I2C.h" +#include "stm32f4xx_revo.h" + +#define ADDR_WRITE 0xEE // Module address write mode +#define ADDR_READ 0xEF // Module address read mode +#define ADDRESS_BARO 0x76 //0x77 + +#define CMD_RESET 0x1E // ADC reset command +#define CMD_ADC_READ 0x00 // ADC read command +#define CMD_ADC_CONV 0x40 // ADC conversion command +#define CMD_ADC_D1 0x00 // ADC D1 conversion +#define CMD_ADC_D2 0x10 // ADC D2 conversion +#define CMD_ADC_256 0x00 // ADC OSR=256 +#define CMD_ADC_512 0x02 // ADC OSR=512 +#define CMD_ADC_1024 0x04 // ADC OSR=1024 +#define CMD_ADC_2048 0x06 // ADC OSR=2048 +#define CMD_ADC_4096 0x08 // ADC OSR=4096 +#define CMD_PROM_RD 0xA0 // Prom read command + +#define Device_address_1 0x56 + +I2C_HandleTypeDef baroI2C_handle; + +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) + +uint8_t cobuf[3]; + +/* address: 0 = factory data and the setup + * address: 1-6 = calibration coefficients + * address: 7 = serial code and CRC */ +uint32_t coefficients_arr[8]; //coefficient storage + + +uint8_t crc4(uint32_t n_prom[]) { + uint32_t n_rem; + uint32_t crc_read; + uint8_t n_bit; + n_rem = 0x00; + crc_read = n_prom[7]; + n_prom[7]=(0xFF00 & (n_prom[7])); + for (int cnt = 0; cnt < 16; cnt++) { + if (cnt%2 == 1) { + n_rem ^= (uint16_t) ((n_prom[cnt>>1]) & 0x00FF); + } else { + n_rem ^= (uint16_t) (n_prom[cnt>>1]>>8); + } + for (n_bit = 8; n_bit > 0; n_bit--) { + if (n_rem & (0x8000)) { + n_rem = (n_rem << 1) ^ 0x3000; + } else { + n_rem = (n_rem << 1); + } + } + } + n_rem= (0x000F & (n_rem >> 12)); + n_prom[7]=crc_read; + return (n_rem ^ 0x0); +} + +unsigned int barometer_cmd_prom(char coef_num) +{ + uint8_t cobuf[2]; + unsigned int rC = 0; + cobuf[0] = 0; + cobuf[1] = 0; + //i2c_send(&baroI2C_handle, ADDRESS_BARO, CMD_PROM_RD + coef_num * 2, 1); // send PROM READ command + m_i2c_send(CMD_PROM_RD + coef_num * 2); // send PROM READ command + i2c_receive(&baroI2C_handle, ADDRESS_BARO, cobuf, 3); //Read the adc values + rC = cobuf[0] * 256 + cobuf[1]; + return rC; +} + +void loadCoefs() { + for (int i = 0; i < 8; i++){ + HAL_Delay(50); + coefficients_arr[i] = barometer_cmd_prom(i); + } + uint8_t n_crc = crc4(coefficients_arr); +} + + +int32_t m_i2c_start(bool readMode) { + int32_t twst; + if(readMode == true) + { + twst = i2c_send(&baroI2C_handle, ADDRESS_BARO, ADDR_READ, 1); + } + else + { + twst = i2c_send(&baroI2C_handle, ADDRESS_BARO, ADDR_WRITE, 1); + } + return(true); +} + +void m_i2c_send(uint8_t cmd) +{ + uint8_t ret; + ret = m_i2c_start(false); + if(!(ret)) + { + //m_i2c_stop(); + } + else + { + ret = i2c_send(&baroI2C_handle, ADDRESS_BARO, cmd, 1); + //m_i2c_stop(); + } +} + +bool barometer_init() +{ + 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); + bool response = i2c_send(&baroI2C_handle, ADDR_WRITE, (uint8_t*)cobuf[0], 1); + 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; +} + +typedef enum { + CALCSTATE_D2_CALCULATION = 0, //Tell the sensor that we want to read D2 + CALCSTATE_D2_READ, //Read D2 from the sensor + CALCSTATE_D1_CALCULATION, //Tell the sensor that we want to read D1 + CALCSTATE_D1_READ, //Read D1 from the sensor + CALCSTATE_CALCULATE_PTA //preassure, temp, altidute calc +}calculationState; + +void barometer_CalculatePTA(uint32_t D1, uint32_t D2) +{ + /* calculate dT, difference between actual and reference temp: (D2 - C5 * 2^8) */ + int64_t dT = D2 - ((uint64_t)coefficients_arr[5] << 8); + + /* Calculate OFF, offset at actual temp: (C2 * 2^16 + (C4 * dT)/2^7) */ + int64_t OFF = (((uint32_t)coefficients_arr[2]) << 16) + ((coefficients_arr[4] * dT) >> 7); + + /* Calculate SENS, sensitivity at actual temperature: (C1 * 2^15 + (C3 * dT)/2^8) */ + int64_t SENS = ((uint32_t)coefficients_arr[1] << 15) + ((coefficients_arr[3] * dT) >> 8 ); + + /* 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 + + /* Improvements for different temperatures */ + if (TEMP < 2000) //if temp is lower than 20 Celsius + { + int64_t T1 = ((int64_t)TEMP - 2000) * ((int64_t)TEMP - 2000); + int64_t OFF1 = (5 * T1) >> 1; + int64_t SENS1 = (5 * T1) >> 2; + if(TEMP < -1500) //if temp is lower than -15 + { + T1 = ((int64_t)TEMP + 1500) * ((int64_t)TEMP + 1500); + OFF1 += 7 * T1; + SENS1 += 11 * T1 >> 1; + } + OFF -= OFF1; + SENS -= SENS1; + } + /* 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() +{ + /*the calculation is in need of different states. This is because the + * a wait time is needed when talking to the sensor. Because we cant + * use a delay wait we need to do parts of the calculation every time + * the function is called. The "delay" is handled by the period of + * the task that handles the calculation. It cant have a period faster + * that 10 ms, or the wait will not be enough in some cases according + * to the datasheet of the sensor http://www.amsys.info/sheets/amsys.en.ms5611_01ba03.pdf*/ + static uint8_t currentCalculationState = CALCSTATE_D2_CALCULATION; + static uint32_t D1 = 0; + static uint32_t D2 = 0; + uint8_t cobuf[3] = {0}; + + + + switch (currentCalculationState) + { + case CALCSTATE_D2_CALCULATION: + i2c_send(&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: + i2c_send(&baroI2C_handle, ADDRESS_BARO, CMD_ADC_READ, 1); //Tell the sensor we want to read + i2c_receive(&baroI2C_handle, ADDRESS_BARO, cobuf, 3); //Read the adc values + 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: + i2c_send(&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: + i2c_send(&baroI2C_handle, ADDRESS_BARO, CMD_ADC_READ, 1); //Tell the sensor we want to read + i2c_receive(&baroI2C_handle, ADDRESS_BARO, cobuf, 3); //Read the adc values + 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); + break; + } + +} + +double barometer_GetCurrentPreassure() +{ + return baro_Preassure; +} + +double barometer_GetCurrentTemperature() +{ + return baro_Temperature; +} + +double barometer_GetCurrentAltitude() +{ + +} + + diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index b69cd1d..1d42d07 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -28,6 +28,7 @@ #include "drivers/motormix.h" #include "drivers/motors.h" #include "Flight/pid.h" +#include "drivers/barometer.h" /************************************************************************** @@ -52,7 +53,7 @@ void init_system() readEEPROM(); //initialize the CLI NOTE: Cant use the same usart as anything else or there will be some big trouble - cliInit(USART6); + cliInit(USART3); //init sbus, using USART1 sbus_init(); @@ -71,7 +72,8 @@ void init_system() #endif #ifdef BARO - + barometer_init(); + barometer_reset(); #endif #ifdef COMPASS diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index 64b57ad..8984962 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -183,7 +183,64 @@ void systemTaskBattery(void) void systemTaskBaro(void) { - pidRun(PID_ID_BAROMETER); + barometer_CaclulateValues(); + + + +// //read the barometer +// I2C_HandleTypeDef i2c_profile; +// +// //---- COMPAS WORKING ---- +// i2c_configure(I2C1, &i2c_profile, 0x56); +// +// uint32_t address = 0b00011110; +// uint8_t start_request_1[2] = { 0b00000000, 0b01110000 }; +// uint8_t start_request_2[2] = { 0b00000001, 0b10100000 }; +// uint8_t start_request_3[2] = { 0b00000010, 0b00000000 }; +// +// uint8_t request_data[1] = { 0b00000110 }; +// uint8_t reset_pointer_data[1] = { 0b00000011 }; +// uint8_t response_data[6] = { 0x0 }; +// +// i2c_send(&i2c_profile, address, start_request_1, 2); +// i2c_send(&i2c_profile, address, start_request_2, 2); +// i2c_send(&i2c_profile, address, start_request_3, 2); +// +// // Delay for at least 6 ms for system startup to finish +// HAL_Delay(10); +// +// while (1) +// { +// i2c_send(&i2c_profile, 0x3D, request_data, 1); +// i2c_receive(&i2c_profile, address, response_data, 6); +// i2c_send(&i2c_profile, address, reset_pointer_data, 1); +// +// // HAL_Delay(100); +// if(response_data[0] != 0) +// response_data[0] = 0; +// +// +// int16_t x = (~(*(int16_t*)(response_data+0)))+1; +// int16_t z = (~(*(int16_t*)(response_data+2)))+1; +// int16_t y = (~(*(int16_t*)(response_data+4)))+1; +// int stop = 5 *2; +// } + + + + + + + + + + + + + + + //pid run + //pidRun(PID_ID_BAROMETER); } void systemTaskCompass(void) @@ -204,6 +261,10 @@ 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(); + char buffer[50]; + sprintf(buffer, "Temperature: %8.2f \n\r", temperature); + usart_transmit(&cliUsart, buffer, 50, 1000000000); } void systemTaskBeeper(void)