diff --git a/UAV-ControlSystem/inc/drivers/I2C.h b/UAV-ControlSystem/inc/drivers/I2C.h index 44fea89..0cd1530 100644 --- a/UAV-ControlSystem/inc/drivers/I2C.h +++ b/UAV-ControlSystem/inc/drivers/I2C.h @@ -54,6 +54,18 @@ bool i2c_send(I2C_HandleTypeDef* profile, uint32_t length); + +/****************************************************************************** +* BRIEF: Configure the I2C bus to be used * +* INFORMATION: This function only implements I2C1 or I2C2 DMA which are * +* available on the REVO board * +******************************************************************************/ +bool i2c_configure_DMA(I2C_TypeDef *i2c, + I2C_HandleTypeDef *out_profile, + DMA_HandleTypeDef *out_rxDMA_profile, + DMA_HandleTypeDef *out_txDMA_profile, + uint32_t my_address); + #endif /* DRIVERS_I2C_H_ */ diff --git a/UAV-ControlSystem/inc/drivers/failsafe_toggles.h b/UAV-ControlSystem/inc/drivers/failsafe_toggles.h index aa2fe29..0a0d3bc 100644 --- a/UAV-ControlSystem/inc/drivers/failsafe_toggles.h +++ b/UAV-ControlSystem/inc/drivers/failsafe_toggles.h @@ -28,7 +28,7 @@ #define getFlagMaskValue(x) (1 << x) #define failSafe_vals_offset 0 //offset for the fail safe values in the bitfield -#define boolean_vals_offset 3 //offset for the booleans values in the bitfield +#define boolean_vals_offset 3 //offset for the booleans values in the bitfield. Equals the amount of failsafe ids /*If a new value is added to the bitfield these IDs must be reviewed and checkd so that they still are correct*/ //failsafe values @@ -45,6 +45,8 @@ #define systemFlags_mixerfullscale_id 5 + boolean_vals_offset #define systemFlags_mixerlowscale_id 6 + boolean_vals_offset #define systemFlags_flightMode_3_id 7 + boolean_vals_offset +#define systemFlags_barometerIsCalibrated_id 8 + boolean_vals_offset +#define systemFlags_AcceleromterIsCalibrated_id 9 + boolean_vals_offset /*Mask values for each of the flag values*/ @@ -62,6 +64,8 @@ #define systemFlags_mixerfullscale_mask getFlagMaskValue(systemFlags_mixerfullscale_id) #define systemFlags_mixerlowscale_mask getFlagMaskValue(systemFlags_mixerlowscale_id) #define systemFlags_flightMode_3_mask getFlagMaskValue(systemFlags_flightMode_3_id) +#define systemFlags_barometerIsCalibrated_mask getFlagMaskValue(systemFlags_barometerIsCalibrated_id) +#define systemFlags_AcceleromterIsCalibrated_mask getFlagMaskValue(systemFlags_AcceleromterIsCalibrated_id) @@ -74,18 +78,20 @@ typedef union bitSetRegion struct { //fail-safe booleans - booleanValue_t rcChannelInRange : 1; - booleanValue_t noRcReceived : 1; + booleanValue_t rcChannelInRange : 1; + booleanValue_t noRcReceived : 1; //Flag boleans that are not fail-safe - booleanValue_t armed : 1; - booleanValue_t acceleromter : 1; - booleanValue_t barometer : 1; - booleanValue_t compass : 1; - booleanValue_t gps : 1; - booleanValue_t mixerfullscale : 1; - booleanValue_t mixerlowscale : 1; - booleanValue_t flightMode_3 : 1; + booleanValue_t armed : 1; + booleanValue_t acceleromter : 1; + booleanValue_t barometer : 1; + booleanValue_t compass : 1; + booleanValue_t gps : 1; + booleanValue_t mixerfullscale : 1; + booleanValue_t mixerlowscale : 1; + booleanValue_t flightMode_3 : 1; + booleanValue_t barometerIsCalibrated : 1; + booleanValue_t AcceleromterIsCalibrated : 1; }bitField; uint64_t intRepresentation; }boolFlags_t; diff --git a/UAV-ControlSystem/inc/drivers/i2c_soft.h b/UAV-ControlSystem/inc/drivers/i2c_soft.h new file mode 100644 index 0000000..7d4dcd3 --- /dev/null +++ b/UAV-ControlSystem/inc/drivers/i2c_soft.h @@ -0,0 +1,28 @@ +/* + * i2c_soft.h + * + * Created on: 27 okt. 2016 + * Author: holmis + */ + +#ifndef DRIVERS_I2C_SOFT_H_ +#define DRIVERS_I2C_SOFT_H_ + +#include "stm32f4xx.h" + +typedef struct +{ + GPIO_TypeDef * i2c_Port; + uint16_t i2c_scl_pin; + uint16_t i2c_sda_pin; +}I2C_SOFT_handle_t; + + +void i2c_soft_Init(I2C_TypeDef *i2c, I2C_SOFT_handle_t *out_profile); + +bool i2c_soft_Write(I2C_SOFT_handle_t *handle, uint8_t addr, uint8_t reg, uint8_t data); + +bool i2c_soft_Read(I2C_SOFT_handle_t *handle, uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf); + + +#endif /* DRIVERS_I2C_SOFT_H_ */ diff --git a/UAV-ControlSystem/inc/stm32f4xx_revo.h b/UAV-ControlSystem/inc/stm32f4xx_revo.h index 1d71484..90f7cf2 100644 --- a/UAV-ControlSystem/inc/stm32f4xx_revo.h +++ b/UAV-ControlSystem/inc/stm32f4xx_revo.h @@ -137,6 +137,8 @@ /* Baro */ #define BARO +#define BARO_USE_I2C_SOFT +//#define BARO_USE_I2C_HARD //Dont work with DMA right now if fixed should be faster. Otherwise software is faster than hardware I2C #define MPU6000_NSS_PIN GPIO_PIN_4 #define MPU6000_NSS_PORT GPIOA diff --git a/UAV-ControlSystem/src/Scheduler/tasks.c b/UAV-ControlSystem/src/Scheduler/tasks.c index 46c2a42..aa17518 100644 --- a/UAV-ControlSystem/src/Scheduler/tasks.c +++ b/UAV-ControlSystem/src/Scheduler/tasks.c @@ -105,7 +105,7 @@ task_t SystemTasks[TASK_COUNT] = { .taskName = "BAROMETER", .taskFunction = systemTaskBaro, - .desiredPeriod = GetUpdateRateHz(20), //20 hz update rate (50 ms) + .desiredPeriod = GetUpdateRateHz(200), //200 hz update rate (5 ms) .staticPriority = PRIORITY_LOW, }, #endif diff --git a/UAV-ControlSystem/src/drivers/I2C.c b/UAV-ControlSystem/src/drivers/I2C.c index 9fb8d48..f7d204f 100644 --- a/UAV-ControlSystem/src/drivers/I2C.c +++ b/UAV-ControlSystem/src/drivers/I2C.c @@ -61,12 +61,13 @@ bool i2c_configure(I2C_TypeDef *i2c, GPIO_InitStruct.Alternate = i2c_af; HAL_GPIO_Init(i2c_port, &GPIO_InitStruct); + HAL_Delay(10); if(__HAL_RCC_I2C1_IS_CLK_DISABLED()) __HAL_RCC_I2C1_CLK_ENABLE(); //Initialize I2C communication out_profile->Instance = i2c; - out_profile->Init.ClockSpeed = 100000; + out_profile->Init.ClockSpeed = 400000; out_profile->Init.DutyCycle = I2C_DUTYCYCLE_2/*I2C_DUTYCYCLE_2*/; out_profile->Init.OwnAddress1 = my_address; out_profile->Init.OwnAddress2 = 0; @@ -118,6 +119,7 @@ bool i2c_send(I2C_HandleTypeDef* profile, uint8_t* data, uint32_t length) { + //TODO: Fix this function 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 @@ -131,3 +133,141 @@ bool i2c_send(I2C_HandleTypeDef* profile, return (i < 10); } + +uint8_t dma_baro_rx_buffer[3]; +uint8_t dma_baro_tx_buffer[1]; + +bool i2c_configure_DMA(I2C_TypeDef *i2c, + I2C_HandleTypeDef *out_profile, + DMA_HandleTypeDef *out_rxDMA_profile, + DMA_HandleTypeDef *out_txDMA_profile, + uint32_t my_address) +{ + uint8_t i2c_af; + uint16_t sda_pin, scl_pin; + GPIO_TypeDef *i2c_port; + DMA_Stream_TypeDef *dma_rx_instance, *dma_tx_instance; + uint32_t channel; + + // get the correct alternate function and pins for the selected I2C + // Only I2C1 and I2C2 is available on the REVO board + if(i2c == I2C1) + { + i2c_af = GPIO_AF4_I2C1; + i2c_port = I2C1_PORT; + sda_pin = I2C1_SDA_PIN; + scl_pin = I2C1_SCL_PIN; + dma_rx_instance = DMA1_Stream5; + dma_tx_instance = DMA1_Stream6; + channel = DMA_CHANNEL_1; +// if(__HAL_RCC_I2C1_IS_CLK_DISABLED()) +// __HAL_RCC_I2C1_CLK_ENABLE(); + } + else if(i2c == I2C2) + { + i2c_af = GPIO_AF4_I2C2; + i2c_port = I2C2_PORT; + sda_pin = I2C2_SDA_PIN; + scl_pin = I2C2_SCL_PIN; + if(__HAL_RCC_I2C2_IS_CLK_DISABLED()) + __HAL_RCC_I2C2_CLK_ENABLE(); + } + else + return false; // The provided I2C is not correct + + if(__HAL_RCC_GPIOB_IS_CLK_DISABLED()) + __HAL_RCC_GPIOB_CLK_ENABLE(); + + + //Initialize pins for SCL and SDA + GPIO_InitTypeDef GPIO_InitStruct; + GPIO_InitStruct.Pin = sda_pin | scl_pin; + GPIO_InitStruct.Mode = GPIO_MODE_AF_OD; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_HIGH; + GPIO_InitStruct.Alternate = i2c_af; + HAL_GPIO_Init(i2c_port, &GPIO_InitStruct); + + HAL_Delay(10); + if(__HAL_RCC_I2C1_IS_CLK_DISABLED()) + __HAL_RCC_I2C1_CLK_ENABLE(); + if(__HAL_RCC_DMA1_IS_CLK_DISABLED()) + __HAL_RCC_DMA1_CLK_ENABLE(); + //Initialize I2C communication + out_profile->Instance = i2c; + out_profile->Init.ClockSpeed = 400000; + out_profile->Init.DutyCycle = I2C_DUTYCYCLE_2/*I2C_DUTYCYCLE_2*/; + out_profile->Init.OwnAddress1 = my_address; + out_profile->Init.OwnAddress2 = 0; + out_profile->Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; + out_profile->Init.DualAddressMode = I2C_DUALADDRESS_DISABLE; + out_profile->Init.GeneralCallMode = I2C_GENERALCALL_DISABLE; + out_profile->Init.NoStretchMode = I2C_NOSTRETCH_DISABLED; + + + if(HAL_I2C_Init(out_profile) != HAL_OK) + return false; + + // Enable the DMA on the i2c register level + out_profile->Instance->CR2 |= (1 << 11); + + /* Create the DMAs */ + DMA_HandleTypeDef out_rxDMA_profile2; + out_rxDMA_profile2.Instance = dma_rx_instance; + out_rxDMA_profile2.Init.Channel = channel; + out_rxDMA_profile2.Init.Direction = DMA_PERIPH_TO_MEMORY; + out_rxDMA_profile2.Init.PeriphInc = DMA_PINC_DISABLE; + out_rxDMA_profile2.Init.MemInc = DMA_MINC_ENABLE; + out_rxDMA_profile2.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; + out_rxDMA_profile2.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; + out_rxDMA_profile2.Init.Mode = DMA_CIRCULAR; + out_rxDMA_profile2.Init.Priority = DMA_PRIORITY_VERY_HIGH; + if(HAL_DMA_Init(&out_rxDMA_profile2) != HAL_OK) + return false; + + __HAL_LINKDMA(out_profile ,hdmarx, out_rxDMA_profile2); + + DMA_HandleTypeDef out_txDMA_profile2; + out_txDMA_profile2.Instance = dma_tx_instance; + out_txDMA_profile2.Init.Channel = channel; + out_txDMA_profile2.Init.Direction = DMA_MEMORY_TO_PERIPH; + out_txDMA_profile2.Init.PeriphInc = DMA_PINC_DISABLE; + out_txDMA_profile2.Init.MemInc = DMA_MINC_ENABLE; + out_txDMA_profile2.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; + out_txDMA_profile2.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; + out_txDMA_profile2.Init.Mode = DMA_CIRCULAR; + out_txDMA_profile2.Init.Priority = DMA_PRIORITY_VERY_HIGH; + if(HAL_DMA_Init(&out_txDMA_profile2) != HAL_OK) + return false; + + __HAL_LINKDMA(out_profile ,hdmatx, out_txDMA_profile2); + + + //Setup DMA buffers + + // Disable the DMA, must be done before writing to the addresses below + dma_rx_instance->CR &= ~(DMA_SxCR_EN); + + dma_rx_instance->NDTR = 3; // Set the buffer size + dma_rx_instance->PAR = (uint32_t)&i2c->DR; // Set the address to the USART data register + dma_rx_instance->M0AR = (uint32_t)dma_baro_rx_buffer; // Set the address to the first DMA buffer + dma_rx_instance->CR &= ~(0xF << 11); // Set the data size to 8 bit values + + //Enable the DMA again to start receiving data from the USART + dma_rx_instance->CR |= DMA_SxCR_EN; + + + dma_tx_instance->CR &= ~(DMA_SxCR_EN); + + dma_tx_instance->NDTR = 1; + dma_tx_instance->PAR = (uint32_t)&i2c->DR; + dma_tx_instance->M0AR = (uint32_t)dma_baro_tx_buffer; + dma_tx_instance->CR &= ~(0xF << 11); + + + dma_tx_instance->CR |= DMA_SxCR_EN; + + + return true; +} + diff --git a/UAV-ControlSystem/src/drivers/barometer.c b/UAV-ControlSystem/src/drivers/barometer.c index 93590fe..b3fa766 100644 --- a/UAV-ControlSystem/src/drivers/barometer.c +++ b/UAV-ControlSystem/src/drivers/barometer.c @@ -10,6 +10,10 @@ #include "stm32f4xx_revo.h" #include "drivers/system_clock.h" #include "math.h" +#include "drivers/i2c_soft.h" +#include "drivers/failsafe_toggles.h" + +#define Device_address_1 0x56 #define ADDR_WRITE 0xEE // Module address write mode #define ADDR_READ 0xEF // Module address read mode @@ -30,345 +34,117 @@ #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)) -#define IOCFG_OUT_OD IO_CONFIG(GPIO_Mode_Out_OD, GPIO_Speed_2MHz) +#define CALIBRATION_VAL_AMOUNT 30 I2C_HandleTypeDef baroI2C_handle; +DMA_HandleTypeDef baroI2C_Rx_DMA_handle; +DMA_HandleTypeDef baroI2C_Tx_DMA_handle; +I2C_SOFT_handle_t baroI2C_soft_handle; + +uint8_t sampleAmount; 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]; +float altitudeCalibrationValue = 0; //Value used as calibration value +float calibrationSamples[CALIBRATION_VAL_AMOUNT]; //array of stored values to be used for calibration, only samples calibration values when machine is not armed +int calibrationSamplesCount = 0; +int calibrationSamplesIterator = 0; + +//TODO: remove when not used for testing any more +uint32_t tempTestCounterStart = 0; + +uint8_t cobuf[3] = {0}; /* 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 -void IOHi(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin) +void barometer_addCalibrationSample() { - HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_SET); + //fisrt check if the amount of samples is greater than the array + if (!(calibrationSamplesCount >= CALIBRATION_VAL_AMOUNT)) + calibrationSamplesCount++; //if not increase the counter + + //Check if the iterator should restart from the beginning because of overflow + if (calibrationSamplesIterator >= CALIBRATION_VAL_AMOUNT) + calibrationSamplesIterator = 0; //if it is set it to zero + + //Add the lates calculated altitude value to the samples + calibrationSamples[calibrationSamplesIterator] = baro_Altitude; + + //increase the iterator + calibrationSamplesIterator ++; } -void IOLo(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin) +bool barometer_Calibrate() { - HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_RESET); -} + //Check if any calibration values exist + if (calibrationSamplesCount <= 0) + return false; -bool IORead(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin) -{ - return !! (GPIOx->IDR & GPIO_Pin); -} + float sampled = 0; -static void TEST_I2C_delay(void) -{ - volatile int i = 1; - while (i) { - i--; - } -} - -void TEST_i2cInit() -{ - -// scl = IOGetByTag(IO_TAG(SOFT_I2C_SCL)); -// sda = IOGetByTag(IO_TAG(SOFT_I2C_SDA)); -// -// IOConfigGPIO(scl, IOCFG_OUT_OD); -// IOConfigGPIO(sda, IOCFG_OUT_OD); -} - -static bool TEST_I2C_Start(void) -{ - IOHi(I2C1_PORT, I2C1_SDA_PIN); - IOHi(I2C1_PORT, I2C1_SCL_PIN); - asm ("nop"); // TEST_I2C_delay(); - if (!IORead(I2C1_PORT, I2C1_SDA_PIN)) { - return false; - } - IOLo(I2C1_PORT, I2C1_SDA_PIN); - asm ("nop"); // TEST_I2C_delay(); - if (IORead(I2C1_PORT, I2C1_SDA_PIN)) { - return false; - } - IOLo(I2C1_PORT, I2C1_SDA_PIN); - asm ("nop"); // TEST_I2C_delay(); - return true; -} - -static void TEST_I2C_Stop(void) -{ - IOLo(I2C1_PORT, I2C1_SCL_PIN); - asm ("nop"); // TEST_I2C_delay(); - IOLo(I2C1_PORT, I2C1_SDA_PIN); - asm ("nop"); // TEST_I2C_delay(); - IOHi(I2C1_PORT, I2C1_SCL_PIN); - asm ("nop"); // TEST_I2C_delay(); - IOHi(I2C1_PORT, I2C1_SDA_PIN); - asm ("nop"); // TEST_I2C_delay(); -} - -static void TEST_I2C_Ack(void) -{ - IOLo(I2C1_PORT, I2C1_SCL_PIN); - asm ("nop"); // TEST_I2C_delay(); - IOLo(I2C1_PORT, I2C1_SDA_PIN); - asm ("nop"); // TEST_I2C_delay(); - IOHi(I2C1_PORT, I2C1_SCL_PIN); - asm ("nop"); // TEST_I2C_delay(); - IOLo(I2C1_PORT, I2C1_SCL_PIN); - asm ("nop"); // TEST_I2C_delay(); -} - -static void TEST_I2C_NoAck(void) -{ - IOLo(I2C1_PORT, I2C1_SCL_PIN); - asm ("nop"); // TEST_I2C_delay(); - IOHi(I2C1_PORT, I2C1_SDA_PIN); - asm ("nop"); // TEST_I2C_delay(); - IOHi(I2C1_PORT, I2C1_SCL_PIN); - asm ("nop"); // TEST_I2C_delay(); - IOLo(I2C1_PORT, I2C1_SCL_PIN); - asm ("nop"); // TEST_I2C_delay(); -} - -static bool TEST_I2C_WaitAck(void) -{ - IOLo(I2C1_PORT, I2C1_SCL_PIN); - asm ("nop"); // TEST_I2C_delay(); - IOHi(I2C1_PORT, I2C1_SDA_PIN); - asm ("nop"); // TEST_I2C_delay(); - IOHi(I2C1_PORT, I2C1_SCL_PIN); - asm ("nop"); // TEST_I2C_delay(); - if (IORead(I2C1_PORT, I2C1_SDA_PIN)) { - IOLo(I2C1_PORT, I2C1_SCL_PIN); - return false; - } - IOLo(I2C1_PORT, I2C1_SCL_PIN); - return true; -} - -static void TEST_I2C_SendByte(uint8_t byte) -{ - uint8_t i = 8; - while (i--) { - IOLo(I2C1_PORT, I2C1_SCL_PIN); - asm ("nop"); // TEST_I2C_delay(); - if (byte & 0x80) { - IOHi(I2C1_PORT, I2C1_SDA_PIN); - } - else { - IOLo(I2C1_PORT, I2C1_SDA_PIN); - } - byte <<= 1; - asm ("nop"); // TEST_I2C_delay(); - IOHi(I2C1_PORT, I2C1_SCL_PIN); - asm ("nop"); // TEST_I2C_delay(); - } - IOLo(I2C1_PORT, I2C1_SCL_PIN); -} - -static uint8_t TEST_I2C_ReceiveByte(void) -{ - uint8_t i = 8; - uint8_t byte = 0; - - IOHi(I2C1_PORT, I2C1_SDA_PIN); - while (i--) { - byte <<= 1; - IOLo(I2C1_PORT, I2C1_SCL_PIN); - asm ("nop"); // TEST_I2C_delay(); - IOHi(I2C1_PORT, I2C1_SCL_PIN); - asm ("nop"); // TEST_I2C_delay(); - if (IORead(I2C1_PORT, I2C1_SDA_PIN)) { - byte |= 0x01; - } - } - IOLo(I2C1_PORT, I2C1_SCL_PIN); - return byte; -} - -bool TEST_i2cRead(I2C_HandleTypeDef *hi2c, uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf) -{ - //just send the addres 0x77 - //write = 0, read = 1 - - if (!TEST_I2C_Start()) { - return false; - } - - TEST_I2C_SendByte(addr << 1 | 0); - if (!TEST_I2C_WaitAck()) { - TEST_I2C_Stop(); - //i2cErrorCount++; - return false; - } - TEST_I2C_SendByte(reg); - TEST_I2C_WaitAck(); - TEST_I2C_Start(); - TEST_I2C_SendByte(addr << 1 | 1); - TEST_I2C_WaitAck(); - while (len) { - *buf = TEST_I2C_ReceiveByte(); - if (len == 1) { - TEST_I2C_NoAck(); - } - else { - TEST_I2C_Ack(); - } - buf++; - len--; - } - TEST_I2C_Stop(); - return true; -} - -bool TEST_i2cWrite(I2C_HandleTypeDef *hi2c, uint8_t addr, uint8_t reg, uint8_t data) -{ - //just send the addres 0x77 - //write = 0, read = 1 - - if (!TEST_I2C_Start()) { - return false; - } - - TEST_I2C_SendByte(addr << 1 | 0); - if (!TEST_I2C_WaitAck()) { - TEST_I2C_Stop(); - // i2cErrorCount++; - return false; - } - TEST_I2C_SendByte(reg); - TEST_I2C_WaitAck(); - TEST_I2C_SendByte(data); - TEST_I2C_WaitAck(); - TEST_I2C_Stop(); - return true; -} - - - - - - - -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)) + //loop through all the calibration samples + for (int i = 0; i < calibrationSamplesCount; i++) { - //m_i2c_stop(); - } - else - { - ret = i2c_send(&baroI2C_handle, ADDRESS_BARO, cmd, 1); - //m_i2c_stop(); + sampled += calibrationSamples[i]; } + + //calculate the new calibration value based on the average of all the samples + altitudeCalibrationValue = sampled / calibrationSamplesCount; + + //rest all the values associated with the calibration samples + calibrationSamplesCount = 0; + calibrationSamplesIterator = 0; + + //Set the calibration flag to true + flags_Set_ID(systemFlags_barometerIsCalibrated_id); + + //Calibration went ok + return true; } bool barometer_init() { - //Initialize pins for SCL and SDA - GPIO_InitTypeDef GPIO_InitStruct; - GPIO_InitStruct.Pin = I2C1_SCL_PIN | I2C1_SDA_PIN; - GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD; - GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Speed = GPIO_SPEED_HIGH; - HAL_GPIO_Init(I2C1_PORT, &GPIO_InitStruct); + //Set the sample rate of the data that will be calculated on the barometer peripheral + sampleAmount = CMD_ADC_2048; + //Initialize pins for SCL and SDA +#ifdef BARO_USE_I2C_SOFT + i2c_soft_Init(I2C1, &baroI2C_soft_handle); +#endif +#ifdef BARO_USE_I2C_HARD + +#endif return true; } bool barometer_reset() { - bool ack = false; - uint8_t sig; + /* Send a reset command to the baromter + * Afterwards read in all the coefficient values that are stored on the PROM of the barometer */ - // ack = TEST_i2cRead(I2C1, ADDRESS_BARO, CMD_PROM_RD, 1, &sig); - // if (!ack) - // return false; - - - TEST_i2cWrite(I2C1, ADDRESS_BARO, CMD_RESET, 1); +#ifdef BARO_USE_I2C_SOFT + i2c_soft_Write(&baroI2C_soft_handle, ADDRESS_BARO, CMD_RESET, 1); HAL_Delay(2800); for (int i = 0; i < 8; i++) { uint8_t rxbuf[2] = { 0, 0 }; - TEST_i2cRead(I2C1, ADDRESS_BARO, CMD_PROM_RD + i * 2, 2, rxbuf); // send PROM READ command + i2c_soft_Read(&baroI2C_soft_handle, ADDRESS_BARO, CMD_PROM_RD + i * 2, 2, rxbuf); // send PROM READ command coefficients_arr[i] = rxbuf[0] << 8 | rxbuf[1]; } +#endif +#ifdef BARO_USE_I2C_HARD uint8_t cobuf2[3] = {0}; - cobuf2[0] = CMD_ADC_CONV + (CMD_ADC_D2 + CMD_ADC_4096); + /* Change to hardware polling mode */ + cobuf2[0] = CMD_ADC_CONV + (CMD_ADC_D2 + sampleAmount); HAL_GPIO_DeInit(I2C1_PORT, I2C1_SCL_PIN); HAL_GPIO_DeInit(I2C1_PORT, I2C1_SDA_PIN); i2c_configure(I2C1, &baroI2C_handle, 0x0); @@ -379,15 +155,48 @@ bool barometer_reset() i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf2, 1); i2c_receive(&baroI2C_handle, ADDRESS_BARO, cobuf2, 3); - /* Read values and get a calibration value for height */ + + + /* Hardware DMA test */ + // cobuf2[0] = CMD_ADC_CONV + (CMD_ADC_D2 + sampleAmount); + // HAL_GPIO_DeInit(I2C1_PORT, I2C1_SCL_PIN); + // HAL_GPIO_DeInit(I2C1_PORT, I2C1_SDA_PIN); + // i2c_configure_DMA(I2C1, &baroI2C_handle, &baroI2C_Rx_DMA_handle, &baroI2C_Tx_DMA_handle, 0x0); + // bool isSent2 = HAL_I2C_Master_Transmit_DMA(&baroI2C_handle, ADDRESS_BARO, cobuf2, 1); + // //bool isSent2 = i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf2, 1); + // HAL_Delay(20); + // + // + // cobuf2[0] = CMD_ADC_READ; + // isSent2 = HAL_I2C_Master_Transmit_DMA(&baroI2C_handle, ADDRESS_BARO, cobuf2, 1); + // //isSent2 = i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf2, 1); + // + // HAL_Delay(20); + // HAL_I2C_Master_Receive_DMA(&baroI2C_handle, ADDRESS_BARO, cobuf2, 3); + // //i2c_receive(&baroI2C_handle, ADDRESS_BARO, cobuf2, 3); + // while(HAL_DMA_GetState(&baroI2C_handle) != HAL_DMA_STATE_READY ) + // { + // + // } + // HAL_Delay(20); + +#endif + + + /* Produce an initial calibration value */ /* 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 ++) { barometer_CaclulateValues(); HAL_Delay(10); } - altitudeCalibrationValue = (((float)1 - (pow(((float)baro_Preassure / (float)SEA_PRESS), (float)0.190284))) * (float)145366.45) * FTMETERS; + /* Set the inital calibration value */ + barometer_Calibrate(); + //force bakc the iscalibrated status to false + flags_Clear_ID(systemFlags_barometerIsCalibrated_id); + + tempTestCounterStart = clock_get_ms(); return true; } @@ -432,7 +241,10 @@ 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; + /* Calculate the altitude */ + float feet = ((float)1 - (pow(((float)baro_Preassure / (float)SEA_PRESS), (float)0.190284))) * (float)145366.45; + baro_Altitude = (flags_IsSet_ID(systemFlags_barometerIsCalibrated_id)) ? (feet * FTMETERS) - altitudeCalibrationValue : (feet * FTMETERS); } void barometer_CaclulateValues() @@ -451,42 +263,102 @@ void barometer_CaclulateValues() uint32_t startTime; uint32_t endTime; + + //If the machine is armed and not calibrated we perform a calibraton + if (!flags_IsSet_ID(systemFlags_barometerIsCalibrated_id)) + { + if (flags_IsSet_ID(systemFlags_armed_id)) + { + barometer_Calibrate(); + } + } + + + switch (currentCalculationState) { case CALCSTATE_D2_CALCULATION: - //TEST_i2cWrite(&baroI2C_handle, ADDRESS_BARO, CMD_ADC_CONV + (CMD_ADC_D2 + CMD_ADC_4096),1); // send conversion command - cobuf[0] = CMD_ADC_CONV + (CMD_ADC_D2 + CMD_ADC_4096); + //Set the message to be sent to the barometer + cobuf[0] = CMD_ADC_CONV + (CMD_ADC_D2 + sampleAmount); + + //Send the message to the barometer + #ifdef BARO_USE_I2C_SOFT + i2c_soft_Write(&baroI2C_soft_handle, ADDRESS_BARO, CMD_ADC_CONV + (CMD_ADC_D2 + sampleAmount),1); // send conversion command + #endif + #ifdef BARO_USE_I2C_HARD i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf, 1); - currentCalculationState = CALCSTATE_D2_READ; //change the state so we will go to D2 read next time function is called + #endif + + //change the state so we will go to D2 read next time function is called + currentCalculationState = CALCSTATE_D2_READ; break; case CALCSTATE_D2_READ: - startTime = clock_get_us(); - //TEST_i2cRead(I2C1, ADDRESS_BARO, CMD_ADC_READ, 3, cobuf); // send PROM READ command + //Set the message to be sent to the barometer cobuf[0] = CMD_ADC_READ; + + //Read the message from the barometer + #ifdef BARO_USE_I2C_SOFT + i2c_soft_Read(&baroI2C_soft_handle, ADDRESS_BARO, CMD_ADC_READ, 3, cobuf); // send PROM READ command + #endif + #ifdef BARO_USE_I2C_HARD i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf, 1); i2c_receive(&baroI2C_handle, ADDRESS_BARO, cobuf, 3); - 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 + #endif + + //Shift the values to the correct position for the 24 bit D2 value + D2 = (cobuf[0] << 16) + (cobuf[1] << 8) + cobuf[2]; + + //change the state so we will go to D2 read next time function is called currentCalculationState = CALCSTATE_D1_CALCULATION; break; case CALCSTATE_D1_CALCULATION: - //TEST_i2cWrite(&baroI2C_handle, ADDRESS_BARO, CMD_ADC_CONV + (CMD_ADC_D1 + CMD_ADC_4096),1); // send conversion command - cobuf[0] = CMD_ADC_CONV + (CMD_ADC_D1 + CMD_ADC_4096); + //Set the message to be sent to the barometer + cobuf[0] = CMD_ADC_CONV + (CMD_ADC_D1 + sampleAmount); + + //Send the message to the barometer + #ifdef BARO_USE_I2C_SOFT + i2c_soft_Write(&baroI2C_soft_handle, ADDRESS_BARO, CMD_ADC_CONV + (CMD_ADC_D1 + sampleAmount),1); // send conversion command + #endif + #ifdef BARO_USE_I2C_HARD i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf, 1); - currentCalculationState = CALCSTATE_D1_READ; //change the state so we will go to D1 read next time function is called + #endif + + //change the state so we will go to D1 read next time function is called + currentCalculationState = CALCSTATE_D1_READ; break; case CALCSTATE_D1_READ: - //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 + //Set the message to be sent to the barometer cobuf[0] = CMD_ADC_READ; + + //Read the message from the baromter + #ifdef BARO_USE_I2C_SOFT + i2c_soft_Read(&baroI2C_soft_handle, ADDRESS_BARO, CMD_ADC_READ, 3, cobuf); // send PROM READ command + #endif + #ifdef BARO_USE_I2C_HARD i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf, 1); i2c_receive(&baroI2C_handle, ADDRESS_BARO, cobuf, 3); - D1 = (cobuf[0] << 16) + (cobuf[1] << 8) + cobuf[2]; //Shift the values to the correct position for the 24 bit D2 value + #endif + + //Shift the values to the correct position for the 24 bit D2 value + D1 = (cobuf[0] << 16) + (cobuf[1] << 8) + cobuf[2]; + + //Change the state for the next time the function is called currentCalculationState = CALCSTATE_CALCULATE_PTA; break; case CALCSTATE_CALCULATE_PTA: + startTime = clock_get_us(); + //Calculate the Pressure, temperature and altitude barometer_CalculatePTA(D1, D2); + + //only calculate new calibration values if we are not armed + if (!flags_IsSet_ID(systemFlags_armed_id)) + { + barometer_addCalibrationSample(); //add new calibration value + flags_Clear_ID(systemFlags_barometerIsCalibrated_id); //Clear the flag for barometer calibration + } + + endTime = clock_get_us() - startTime; + //Change the state currentCalculationState = CALCSTATE_D2_CALCULATION; break; } @@ -505,8 +377,7 @@ double barometer_GetCurrentTemperature() float barometer_GetCurrentAltitudeBasedOnSeaLevel() { - float feet = ((float)1 - (pow(((float)baro_Preassure / (float)SEA_PRESS), (float)0.190284))) * (float)145366.45; - return (feet * FTMETERS) - altitudeCalibrationValue; + return baro_Altitude; } diff --git a/UAV-ControlSystem/src/drivers/i2c_soft.c b/UAV-ControlSystem/src/drivers/i2c_soft.c new file mode 100644 index 0000000..34bf083 --- /dev/null +++ b/UAV-ControlSystem/src/drivers/i2c_soft.c @@ -0,0 +1,241 @@ +/* + * i2c_soft.c + * + * Created on: 27 okt. 2016 + * Author: holmis + */ + +#include "drivers/i2c_soft.h" +#include "stm32f4xx_revo.h" + +#define WRITE_INDICATOR 0 +#define READ_INDICATOR 1 + +static void IOHi(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin) +{ + HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_SET); +} + +static void IOLo(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin) +{ + HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_RESET); +} + +static bool IORead(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin) +{ + return !! (GPIOx->IDR & GPIO_Pin); +} + +static void i2c_soft_delay(void) +{ + volatile int i = 1; + while (i) { + i--; + } +} + +void i2c_soft_Init(I2C_TypeDef *i2c, I2C_SOFT_handle_t *out_profile) +{ + uint16_t sda_pin, scl_pin; + GPIO_TypeDef *i2c_port; + + //Check what i2c should be used + if(i2c == I2C1) + { + i2c_port = I2C1_PORT; + sda_pin = I2C1_SDA_PIN; + scl_pin = I2C1_SCL_PIN; + } + else if(i2c == I2C2) + { + i2c_port = I2C2_PORT; + sda_pin = I2C2_SDA_PIN; + scl_pin = I2C2_SCL_PIN; + } + + //Init the GPIO pins + GPIO_InitTypeDef GPIO_InitStruct; + GPIO_InitStruct.Pin = scl_pin | sda_pin; + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_HIGH; + HAL_GPIO_Init(i2c_port, &GPIO_InitStruct); + + //Assign the values to the out struct + out_profile->i2c_Port = i2c_port; + out_profile->i2c_scl_pin = scl_pin; + out_profile->i2c_sda_pin = sda_pin; +} + +static bool i2c_soft_Start(I2C_SOFT_handle_t *handle) +{ + IOHi(handle->i2c_Port, handle->i2c_sda_pin); + IOHi(handle->i2c_Port, handle->i2c_scl_pin); + asm ("nop"); // i2c_soft_delay(); + if (!IORead(handle->i2c_Port, handle->i2c_sda_pin)) { + return false; + } + IOLo(handle->i2c_Port, handle->i2c_sda_pin); + asm ("nop"); // i2c_soft_delay(); + if (IORead(handle->i2c_Port, handle->i2c_sda_pin)) { + return false; + } + IOLo(handle->i2c_Port, handle->i2c_sda_pin); + asm ("nop"); // i2c_soft_delay(); + return true; +} + +static void i2c_soft_Stop(I2C_SOFT_handle_t *handle) +{ + IOLo(handle->i2c_Port, handle->i2c_scl_pin); + asm ("nop"); // i2c_soft_delay(); + IOLo(handle->i2c_Port, handle->i2c_sda_pin); + asm ("nop"); // i2c_soft_delay(); + IOHi(handle->i2c_Port, handle->i2c_scl_pin); + asm ("nop"); // i2c_soft_delay(); + IOHi(handle->i2c_Port, handle->i2c_sda_pin); + asm ("nop"); // i2c_soft_delay(); +} + +static void i2c_soft_Ack(I2C_SOFT_handle_t *handle) +{ + IOLo(handle->i2c_Port, handle->i2c_scl_pin); + asm ("nop"); // i2c_soft_delay(); + IOLo(handle->i2c_Port, handle->i2c_sda_pin); + asm ("nop"); // i2c_soft_delay(); + IOHi(handle->i2c_Port, handle->i2c_scl_pin); + asm ("nop"); // i2c_soft_delay(); + IOLo(handle->i2c_Port, handle->i2c_scl_pin); + asm ("nop"); // i2c_soft_delay(); +} + +static void i2c_soft_NoAck(I2C_SOFT_handle_t *handle) +{ + IOLo(handle->i2c_Port, handle->i2c_scl_pin); + asm ("nop"); // i2c_soft_delay(); + IOHi(handle->i2c_Port, handle->i2c_sda_pin); + asm ("nop"); // i2c_soft_delay(); + IOHi(handle->i2c_Port, handle->i2c_scl_pin); + asm ("nop"); // i2c_soft_delay(); + IOLo(handle->i2c_Port, handle->i2c_scl_pin); + asm ("nop"); // i2c_soft_delay(); +} + +static bool i2c_soft_WaitAck(I2C_SOFT_handle_t *handle) +{ + IOLo(handle->i2c_Port, handle->i2c_scl_pin); + asm ("nop"); // i2c_soft_delay(); + IOHi(handle->i2c_Port, handle->i2c_sda_pin); + asm ("nop"); // i2c_soft_delay(); + IOHi(handle->i2c_Port, handle->i2c_scl_pin); + asm ("nop"); // i2c_soft_delay(); + if (IORead(handle->i2c_Port, handle->i2c_sda_pin)) { + IOLo(handle->i2c_Port, handle->i2c_scl_pin); + return false; + } + IOLo(handle->i2c_Port, handle->i2c_scl_pin); + return true; +} + +static void i2c_soft_SendByte(I2C_SOFT_handle_t *handle, uint8_t byte) +{ + uint8_t i = 8; + while (i--) { + IOLo(handle->i2c_Port, handle->i2c_scl_pin); + asm ("nop"); // i2c_soft_delay(); + if (byte & 0x80) { + IOHi(handle->i2c_Port, handle->i2c_sda_pin); + } + else { + IOLo(handle->i2c_Port, handle->i2c_sda_pin); + } + byte <<= 1; + asm ("nop"); // i2c_soft_delay(); + IOHi(handle->i2c_Port, handle->i2c_scl_pin); + asm ("nop"); // i2c_soft_delay(); + } + IOLo(handle->i2c_Port, handle->i2c_scl_pin); +} + +static uint8_t i2c_soft_ReceiveByte(I2C_SOFT_handle_t *handle) +{ + uint8_t i = 8; + uint8_t byte = 0; + + IOHi(handle->i2c_Port, handle->i2c_sda_pin); + while (i--) { + byte <<= 1; + IOLo(handle->i2c_Port, handle->i2c_scl_pin); + asm ("nop"); // i2c_soft_delay(); + IOHi(handle->i2c_Port, handle->i2c_scl_pin); + asm ("nop"); // i2c_soft_delay(); + if (IORead(handle->i2c_Port, handle->i2c_sda_pin)) { + byte |= 0x01; + } + } + IOLo(handle->i2c_Port, handle->i2c_scl_pin); + return byte; +} + +bool i2c_soft_Read(I2C_SOFT_handle_t *handle, uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf) +{ + //just send the addres 0x77 + //write = 0, read = 1 + + if (!i2c_soft_Start(handle)) { + return false; + } + + i2c_soft_SendByte(handle, addr << 1 | 0); + if (!i2c_soft_WaitAck(handle)) { + i2c_soft_Stop(handle); + //i2cErrorCount++; + return false; + } + i2c_soft_SendByte(handle, reg); + i2c_soft_WaitAck(handle); + i2c_soft_Start(handle); + i2c_soft_SendByte(handle, addr << 1 | 1); + i2c_soft_WaitAck(handle); + while (len) { + *buf = i2c_soft_ReceiveByte(handle); + if (len == 1) { + i2c_soft_NoAck(handle); + } + else { + i2c_soft_Ack(handle); + } + buf++; + len--; + } + i2c_soft_Stop(handle); + return true; +} + +bool i2c_soft_Write(I2C_SOFT_handle_t *handle, uint8_t addr, uint8_t reg, uint8_t data) +{ + //just send the addres 0x77 + //write = 0, read = 1 + + //Start the i2c, if it cant return + if (!i2c_soft_Start(handle)) { + return false; + } + + //Send the address + i2c_soft_SendByte(handle, addr << 1 | WRITE_INDICATOR); + if (!i2c_soft_WaitAck(handle)) { + i2c_soft_Stop(handle); + // i2cErrorCount++; + return false; + } + + //send the data + i2c_soft_SendByte(handle, reg); + i2c_soft_WaitAck(handle); + i2c_soft_SendByte(handle, data); + i2c_soft_WaitAck(handle); + i2c_soft_Stop(handle); + return true; +} + diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index 1d42d07..3c71f7e 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -47,6 +47,7 @@ void init_system() //Configure the clock system_clock_config(); + //Initializes all the pids that are used in the system. This part will also init the gyro and accelerometer. pidInit(); /* read saved variables from eeprom, in most cases eeprom should be read after a lot of the initializes */ @@ -72,8 +73,8 @@ void init_system() #endif #ifdef BARO - barometer_init(); - barometer_reset(); + //barometer_init(); + //barometer_reset(); #endif #ifdef COMPASS @@ -114,7 +115,6 @@ int main(void) //Initialize the scheduler, add all the tasks that should run to the ready queue of the scheduler initScheduler(); - while (1) { //Run the scheduler, responsible for distributing all the work of the running system diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index 56f2554..00b15d7 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -184,64 +184,7 @@ void systemTaskBattery(void) void systemTaskBaro(void) { - 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, address, 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); + //barometer_CaclulateValues(); } void systemTaskCompass(void) @@ -263,14 +206,12 @@ 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 pressure = barometer_GetCurrentPreassure(); + //float altitute = barometer_GetCurrentAltitudeBasedOnSeaLevel(); - double temperature = barometer_GetCurrentTemperature(); - double pressure = barometer_GetCurrentPreassure(); - float altitute = barometer_GetCurrentAltitudeBasedOnSeaLevel(); - - char buffer[50]; - sprintf(buffer, "Temperature: %8.2f \n\r", altitute); - usart_transmit(&cliUsart, buffer, 50, 1000000000); + //pid run, should probably be moved to systemTaskAltitude + pidRun(PID_ID_BAROMETER); } void systemTaskBeeper(void) diff --git a/revolution_hal_lib/HAL_Driver/Src/stm32f4xx_hal_i2c.c b/revolution_hal_lib/HAL_Driver/Src/stm32f4xx_hal_i2c.c index 008158e..477c3e6 100644 --- a/revolution_hal_lib/HAL_Driver/Src/stm32f4xx_hal_i2c.c +++ b/revolution_hal_lib/HAL_Driver/Src/stm32f4xx_hal_i2c.c @@ -376,6 +376,8 @@ HAL_StatusTypeDef HAL_I2C_Init(I2C_HandleTypeDef *hi2c) /* Get PCLK1 frequency */ pclk1 = HAL_RCC_GetPCLK1Freq(); + //this is temp test + // pclk1 = 50000000; /* Calculate frequency range */ freqrange = I2C_FREQRANGE(pclk1);