Ceated new branch for baro

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_ */

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){}

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()
+{
+
+}
+
+

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

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)