Ceated new branch for baro
This commit is contained in:
Jonas Holmberg
parent
7da69bb6c2
commit
db48b1235c
26
UAV-ControlSystem/inc/drivers/barometer.h
Normal file
26
UAV-ControlSystem/inc/drivers/barometer.h
Normal file
@ -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_ */
|
||||
@ -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){}
|
||||
|
||||
244
UAV-ControlSystem/src/drivers/barometer.c
Normal file
244
UAV-ControlSystem/src/drivers/barometer.c
Normal file
@ -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()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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)
|
||||
|
||||
Reference in New Issue
Block a user