From db48b1235cc210c66fdd1cfc5d986f14b709e612 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Thu, 20 Oct 2016 08:59:30 +0200 Subject: [PATCH 01/25] Ceated new branch for baro --- UAV-ControlSystem/inc/drivers/barometer.h | 26 +++ UAV-ControlSystem/src/drivers/I2C.c | 8 +- UAV-ControlSystem/src/drivers/barometer.c | 244 ++++++++++++++++++++++ UAV-ControlSystem/src/main.c | 6 +- UAV-ControlSystem/src/tasks_main.c | 63 +++++- 5 files changed, 341 insertions(+), 6 deletions(-) create mode 100644 UAV-ControlSystem/inc/drivers/barometer.h create mode 100644 UAV-ControlSystem/src/drivers/barometer.c 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) From e75e8a70893f5e9c25a3391b8fb7cb3b1709bc92 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Thu, 20 Oct 2016 11:09:35 +0200 Subject: [PATCH 02/25] started calibration fixing --- UAV-ControlSystem/src/config/cli.c | 85 +++++++++++++++---- UAV-ControlSystem/src/drivers/accel_gyro.c | 97 +++++++++++++++++----- UAV-ControlSystem/src/tasks_main.c | 6 +- 3 files changed, 151 insertions(+), 37 deletions(-) diff --git a/UAV-ControlSystem/src/config/cli.c b/UAV-ControlSystem/src/config/cli.c index 7f96636..f53a4e8 100644 --- a/UAV-ControlSystem/src/config/cli.c +++ b/UAV-ControlSystem/src/config/cli.c @@ -73,6 +73,9 @@ typedef enum ACTION_RESET, //Resets the entire eeprom ACTION_STATS, //gives statistics on the system ACTION_MOTORCALIBRATE, + ACTION_GYROCALIBRATE, + ACTION_ACCELEROMTETERCALIBRATE, + ACTION_COMPASSCALIBRATE, //The number of actions ACTION_COUNT, //Count of the number of actions @@ -94,7 +97,10 @@ const typeString commandAction_Strings[ACTION_COUNT] = { "reboot", "reset", "stats", - "motorcalibrate" + "calibrate_motors", + "calibrate_gyro", + "calibrate_accelerometer", + "calibrate_compass" }; /* String values descrbing information of a certain action */ @@ -110,8 +116,11 @@ const typeString commandActionInformation_Strings[ACTION_COUNT] = { "| exit - Exits the CLI mode.\n\r", "| reboot - Exit CLI and reboots the system.\n\r", "| reset - Restore all the values to its default values.\n\r", - "| stats - Gives some current stats of the system and tasks.\n\r" - "| motorcalibrate - Calibrates all motors." + "| stats - Gives some current stats of the system and tasks.\n\r", + "| calibrate_motors - Calibrates all motors.", + "| calibrate_gyro - Calibrates the gyro.", + "| calibrate_accelerometer - Calibrates the accelerometer.", + "| calibrate_compass - Calibrates the compass." }; /* String array containing all the signature examples for each action */ @@ -128,7 +137,10 @@ const typeString commandActionSignature_Strings[ACTION_COUNT] = { " reboot", " reset", " stats", - " motorcalibrate" + " calibrate_motors", + " calibrate_gyro", + " calibrate_accelerometer", + " calibrate_compass" }; /* Size values for the values a command will require */ @@ -1398,17 +1410,17 @@ void cliRun() break; case commandMask(commandSize_1, ACTION_RESET): //resets all the values in the eeprom to the default values - TransmitBack("- All values will be deleted and system rebooted. Continue? (y/n)... \n\n\r"); + TransmitBack("- All values will be deleted and system rebooted. Continue? (y/n)... \n\n\r"); - //read until a y or n is found - if (ReceiveBinaryDecision(121, 110)) - { - defaultEEPROM(); - } - else - { - TransmitBack("- Values unchanged...\n\n\r"); - } + //read until a y or n is found + if (ReceiveBinaryDecision(121, 110)) + { + defaultEEPROM(); + } + else + { + TransmitBack("- Values unchanged...\n\n\r"); + } break; case commandMask(commandSize_1, ACTION_STATS): TransmitSystemStats(); @@ -1422,13 +1434,56 @@ void cliRun() { TransmitBack("Starting calibration, BE CAREFUL!!! \n\n\r"); calibrateMotors(); + TransmitBack("Wait until the beeping have ceased... \n\n\r"); } else { TransmitBack("Exiting the calibration, BE CAREFUL!!! \n\n\r"); } - break; + break; + case commandMask(commandSize_1, ACTION_GYROCALIBRATE): + TransmitBack("Do you really want to calibrate the gyro? (y/n)\n\n\r"); + if (ReceiveBinaryDecision(121,110)) + { + TransmitBack("Starting calibration! \n\n\r"); + TransmitBack("NOT IMPLEMENTED! \n\n\r"); + TransmitBack("Calibration complete! \n\n\r"); + } + else + { + TransmitBack("Exiting the calibration! \n\n\r"); + } + + break; + case commandMask(commandSize_1, ACTION_ACCELEROMTETERCALIBRATE): + TransmitBack("Do you really want to calibrate the accelerometer? (y/n)\n\n\r"); + if (ReceiveBinaryDecision(121,110)) + { + TransmitBack("Starting calibration! \n\n\r"); + TransmitBack("NOT IMPLEMENTED! \n\n\r"); + TransmitBack("Calibration complete! \n\n\r"); + } + else + { + TransmitBack("Exiting the calibration! \n\n\r"); + } + + break; + case commandMask(commandSize_1, ACTION_COMPASSCALIBRATE): + TransmitBack("Do you really want to calibrate the compass? (y/n)\n\n\r"); + if (ReceiveBinaryDecision(121,110)) + { + TransmitBack("Starting calibration! \n\n\r"); + TransmitBack("NOT IMPLEMENTED! \n\n\r"); + TransmitBack("Calibration complete! \n\n\r"); + } + else + { + TransmitBack("Exiting the calibration! \n\n\r"); + } + + break; default: if (actionId != ACTION_NOACTION) TransmitCommandInstruction(actionId); diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c index 95ae8d0..a40e668 100644 --- a/UAV-ControlSystem/src/drivers/accel_gyro.c +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -44,7 +44,7 @@ bool mpu6000_transmit_receive(uint8_t reg, uint8_t* data, uint8_t length, uint32 /*********************************************************************** * BRIEF: mpu6000_read_offset reads and returns the offset of the * - * gyroscope and accelerometer * + * gyroscope * * INFORMATION: * * Is automatically called when mpu6000_init is called * * The flight controller needs to be stationary when this function is * @@ -52,46 +52,104 @@ bool mpu6000_transmit_receive(uint8_t reg, uint8_t* data, uint8_t length, uint32 * When the UAV is finished this data could be saved so that the * * offset doesn't need to be read every time * ***********************************************************************/ -HAL_StatusTypeDef mpu6000_read_offset(gyro_t* gyro, accel_t* accel) +HAL_StatusTypeDef mpu6000_read_gyro_offset(gyro_t* gyro) { uint8_t dataG[6]; /* Temporary data variable used to receive gyroscope data */ - uint8_t dataA[6]; /* Temporary data variable used to receive accelerometer data */ + int16_t dataGCheck[6] = {0}; /* checkval */ + uint8_t maxDrift = 5; + bool calibrate_go = true; + bool calibrate_ok = false; + int countCalibrate = 0; - if(!mpu6000_transmit_receive(MPU_RA_ACCEL_XOUT_H, dataA, 6, 10)) - return HAL_ERROR; + //small delay so that we know the gyro should give some values + HAL_Delay(100); + for (int i = 0; i < 100; i++) + { + //assume that we should calibrate at the start of each loop + calibrate_go = true; + + //get new values + if(!mpu6000_transmit_receive(MPU_RA_GYRO_XOUT_H, dataG, 6, 10)) + return HAL_ERROR; + + //cheack the new values and see if they are within acceptable range compared to the previous values, so that it is realativelly still + for(int j = 0; j < 6; j ++) + { + int16_t currentDrift = (int16_t)abs(dataGCheck[j] - dataG[j]); + if (!(currentDrift < maxDrift)) + calibrate_go = false; + + dataGCheck[j] = dataG[j]; + } + + //if true, we have good values exit loop + if (calibrate_go) + { + countCalibrate++; + } + if (countCalibrate > 4) + { + calibrate_ok = true; + break; + } + //wait for a little bit so the checks are not to fast + HAL_Delay(200); + } - if(!mpu6000_transmit_receive(MPU_RA_GYRO_XOUT_H, dataG, 6, 10)) - return HAL_ERROR; #ifdef YAW_ROT_0 gyro->offsetX = -(((int16_t)dataG[0] << 8) | dataG[1]); gyro->offsetY = -(((int16_t)dataG[2] << 8) | dataG[3]); gyro->offsetZ = (((int16_t)dataG[4] << 8) | dataG[5]); - accel->offsetX = -((int16_t)dataA[0] << 8) | dataA[1]; - accel->offsetY = -((int16_t)dataA[2] << 8) | dataA[3]; - accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); #elif defined(YAW_ROT_90) gyro->offsetX = -(((int16_t)dataG[2] << 8) | dataG[3]); gyro->offsetY = (((int16_t)dataG[0] << 8) | dataG[1]); gyro->offsetZ = (((int16_t)dataG[4] << 8) | dataG[5]); - - accel->offsetX = -((int16_t)dataA[2] << 8) | dataA[3]; - accel->offsetY = ((int16_t)dataA[0] << 8) | dataA[1]; - accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); #elif defined(YAW_ROT_180) gyro->offsetX = (((int16_t)dataG[0] << 8) | dataG[1]); gyro->offsetY = (((int16_t)dataG[2] << 8) | dataG[3]); gyro->offsetZ = (((int16_t)dataG[4] << 8) | dataG[5]); - - accel->offsetX = ((int16_t)dataA[0] << 8) | dataA[1]; - accel->offsetY = ((int16_t)dataA[2] << 8) | dataA[3]; - accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); #elif defined(YAW_ROT_270) gyro->offsetX = (((int16_t)dataG[2] << 8) | dataG[3]); gyro->offsetY = -(((int16_t)dataG[0] << 8) | dataG[1]); gyro->offsetZ = (((int16_t)dataG[4] << 8) | dataG[5]); +#endif + return HAL_OK; +} + +/*********************************************************************** + * BRIEF: mpu6000_read_offset reads and returns the offset of the * + * accelerometer * + * INFORMATION: * + * Is automatically called when mpu6000_init is called * + * The flight controller needs to be stationary when this function is * + * called * + * When the UAV is finished this data could be saved so that the * + * offset doesn't need to be read every time * + ***********************************************************************/ +HAL_StatusTypeDef mpu6000_read_acc_offset(accel_t* accel) +{ + uint8_t dataA[6]; /* Temporary data variable used to receive accelerometer data */ + + if(!mpu6000_transmit_receive(MPU_RA_ACCEL_XOUT_H, dataA, 6, 10)) + return HAL_ERROR; + + +#ifdef YAW_ROT_0 + accel->offsetX = -((int16_t)dataA[0] << 8) | dataA[1]; + accel->offsetY = -((int16_t)dataA[2] << 8) | dataA[3]; + accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); +#elif defined(YAW_ROT_90) + accel->offsetX = -((int16_t)dataA[2] << 8) | dataA[3]; + accel->offsetY = ((int16_t)dataA[0] << 8) | dataA[1]; + accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); +#elif defined(YAW_ROT_180) + accel->offsetX = ((int16_t)dataA[0] << 8) | dataA[1]; + accel->offsetY = ((int16_t)dataA[2] << 8) | dataA[3]; + accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); +#elif defined(YAW_ROT_270) accel->offsetX = ((int16_t)dataA[2] << 8) | dataA[3]; accel->offsetY = -((int16_t)dataA[0] << 8) | dataA[1]; accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); @@ -180,7 +238,8 @@ bool mpu6000_init(gyro_t* gyro, accel_t* accel) if(!mpu6000_transmit(reg, 2)) return false; - mpu6000_read_offset(gyro, accel); + mpu6000_read_gyro_offset(gyro); + mpu6000_read_acc_offset(accel); return true; } diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index 8984962..ad801a9 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -183,7 +183,7 @@ void systemTaskBattery(void) void systemTaskBaro(void) { - barometer_CaclulateValues(); + //barometer_CaclulateValues(); @@ -261,10 +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(); + /*double temperature = barometer_GetCurrentTemperature(); char buffer[50]; sprintf(buffer, "Temperature: %8.2f \n\r", temperature); - usart_transmit(&cliUsart, buffer, 50, 1000000000); + usart_transmit(&cliUsart, buffer, 50, 1000000000);*/ } void systemTaskBeeper(void) From 75dadc7484716a91c8515ca9bd817a5aa4f68832 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Tue, 25 Oct 2016 09:43:38 +0200 Subject: [PATCH 03/25] Chnaged port from usart 3 to 6 on cli --- UAV-ControlSystem/src/main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index 9722f46..b69cd1d 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -52,7 +52,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(USART3); + cliInit(USART6); //init sbus, using USART1 sbus_init(); From facfcb7023c5c1adfc461a4e55ddae26bfb2f168 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Tue, 25 Oct 2016 10:13:49 +0200 Subject: [PATCH 04/25] PID accelerometer added low pass filter --- UAV-ControlSystem/src/Flight/pid.c | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 082ef17..2ddce64 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -56,6 +56,7 @@ pidProfileBuff_t PidProfileBuff[PID_COUNT] = {0}; accel_t accelProfile; /*Struct profile for input data from sensor*/ gyro_t gyroProfile; /*Struct profile for input data from sensor*/ +pt1Filter_t accelFilter[2] = {0}; /************************************************************************** * BRIEF: Calculates angle from accelerometer * @@ -127,10 +128,19 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) mpu6000_read_accel(&accelProfile); /*Reads data from accelerometer*/ + + /*May need Low pass filter since the accelerometer may drift*/ + sensorValues[ROLL] = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); sensorValues[PITCH] = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); + //float sensorRoll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); + //sensorValues[ROLL] = pt1FilterApply4(&accelFilter[0], sensorRoll, 90, PidProfileBuff[ROLL].dT); + + //float sensorPitch = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); + //sensorValues[PITCH] = pt1FilterApply4(&accelFilter[1], sensorPitch, 90, PidProfileBuff[PITCH].dT); + break; case PID_ID_COMPASS: case PID_ID_BAROMETER: From f35617c92d4a49d12c46738a47a381efb4c60813 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Tue, 25 Oct 2016 14:23:01 +0200 Subject: [PATCH 05/25] Working communication with the BARO Not a full implementation just a test that can get conntact with the BARO and no other calculations are made. This is not using the HAL but its a software implementation that simly sets pins high and low etc... --- UAV-ControlSystem/src/drivers/I2C.c | 14 +- UAV-ControlSystem/src/drivers/barometer.c | 350 +++++++++++++++++++++- UAV-ControlSystem/src/tasks_main.c | 26 +- 3 files changed, 357 insertions(+), 33 deletions(-) diff --git a/UAV-ControlSystem/src/drivers/I2C.c b/UAV-ControlSystem/src/drivers/I2C.c index c0120a2..092916f 100644 --- a/UAV-ControlSystem/src/drivers/I2C.c +++ b/UAV-ControlSystem/src/drivers/I2C.c @@ -57,20 +57,20 @@ bool i2c_configure(I2C_TypeDef *i2c, GPIO_InitStruct.Pin = sda_pin | scl_pin; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Pull = GPIO_PULLUP; - GPIO_InitStruct.Speed = GPIO_SPEED_HIGH; + GPIO_InitStruct.Speed = GPIO_SPEED_FAST; GPIO_InitStruct.Alternate = i2c_af; HAL_GPIO_Init(i2c_port, &GPIO_InitStruct); //Initialize I2C communication out_profile->Instance = i2c; out_profile->Init.ClockSpeed = 400000; - out_profile->Init.DutyCycle = I2C_DUTYCYCLE_2; + 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_ENABLE; + out_profile->Init.NoStretchMode = I2C_NOSTRETCH_DISABLED; if(HAL_I2C_Init(out_profile) != HAL_OK) return false; @@ -118,10 +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, (uint8_t*)data, length, 5000) != HAL_OK && i++ < 10) - {} + while(HAL_I2C_Master_Transmit(profile,(slave_address << 1), (uint8_t*)data, length, 1000) != HAL_OK && i++ < 10) + {I2C1->CR1 |= (1 << 9);} +// 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 index bd63463..932cec7 100644 --- a/UAV-ControlSystem/src/drivers/barometer.c +++ b/UAV-ControlSystem/src/drivers/barometer.c @@ -11,7 +11,7 @@ #define ADDR_WRITE 0xEE // Module address write mode #define ADDR_READ 0xEF // Module address read mode -#define ADDRESS_BARO 0x76 //0x77 +#define ADDRESS_BARO 0x77 //0x77 #define CMD_RESET 0x1E // ADC reset command #define CMD_ADC_READ 0x00 // ADC read command @@ -27,6 +27,9 @@ #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) + I2C_HandleTypeDef baroI2C_handle; double baro_Preassure; // compensated pressure value (mB) @@ -41,6 +44,212 @@ uint8_t cobuf[3]; * address: 7 = serial code and CRC */ uint32_t coefficients_arr[8]; //coefficient storage +void IOHi(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin) +{ + HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_SET); +} + +void IOLo(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin) +{ + HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_RESET); +} + +bool IORead(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin) +{ + return !! (GPIOx->IDR & GPIO_Pin); +} + +static void TEST_I2C_delay(void) +{ + volatile int i = 7; + 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); + TEST_I2C_delay(); + if (!IORead(I2C1_PORT, I2C1_SDA_PIN)) { + return false; + } + IOLo(I2C1_PORT, I2C1_SDA_PIN); + TEST_I2C_delay(); + if (IORead(I2C1_PORT, I2C1_SDA_PIN)) { + return false; + } + IOLo(I2C1_PORT, I2C1_SDA_PIN); + TEST_I2C_delay(); + return true; +} + +static void TEST_I2C_Stop(void) +{ + IOLo(I2C1_PORT, I2C1_SCL_PIN); + TEST_I2C_delay(); + IOLo(I2C1_PORT, I2C1_SDA_PIN); + TEST_I2C_delay(); + IOHi(I2C1_PORT, I2C1_SCL_PIN); + TEST_I2C_delay(); + IOHi(I2C1_PORT, I2C1_SDA_PIN); + TEST_I2C_delay(); +} + +static void TEST_I2C_Ack(void) +{ + IOLo(I2C1_PORT, I2C1_SCL_PIN); + TEST_I2C_delay(); + IOLo(I2C1_PORT, I2C1_SDA_PIN); + TEST_I2C_delay(); + IOHi(I2C1_PORT, I2C1_SCL_PIN); + TEST_I2C_delay(); + IOLo(I2C1_PORT, I2C1_SCL_PIN); + TEST_I2C_delay(); +} + +static void TEST_I2C_NoAck(void) +{ + IOLo(I2C1_PORT, I2C1_SCL_PIN); + TEST_I2C_delay(); + IOHi(I2C1_PORT, I2C1_SDA_PIN); + TEST_I2C_delay(); + IOHi(I2C1_PORT, I2C1_SCL_PIN); + TEST_I2C_delay(); + IOLo(I2C1_PORT, I2C1_SCL_PIN); + TEST_I2C_delay(); +} + +static bool TEST_I2C_WaitAck(void) +{ + IOLo(I2C1_PORT, I2C1_SCL_PIN); + TEST_I2C_delay(); + IOHi(I2C1_PORT, I2C1_SDA_PIN); + TEST_I2C_delay(); + IOHi(I2C1_PORT, I2C1_SCL_PIN); + 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); + TEST_I2C_delay(); + if (byte & 0x80) { + IOHi(I2C1_PORT, I2C1_SDA_PIN); + } + else { + IOLo(I2C1_PORT, I2C1_SDA_PIN); + } + byte <<= 1; + TEST_I2C_delay(); + IOHi(I2C1_PORT, I2C1_SCL_PIN); + 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); + TEST_I2C_delay(); + IOHi(I2C1_PORT, I2C1_SCL_PIN); + 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, uint8_t readWrite) +{ + //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; @@ -74,8 +283,8 @@ unsigned int barometer_cmd_prom(char coef_num) 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_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; @@ -120,18 +329,133 @@ 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; + 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); + + 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); + 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 + 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); - 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); + //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++) + { + 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; + } + //m_i2c_send(CMD_PROM_RD + coef_num * 2); // send PROM READ command + + +// +// 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); @@ -200,22 +524,22 @@ void barometer_CaclulateValues() 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 + HAL_I2C_Master_Transmit(&baroI2C_handle, ADDRESS_BARO, CMD_ADC_CONV + (CMD_ADC_D2 + CMD_ADC_4096),1,1000); // 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 + 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 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 + HAL_I2C_Master_Transmit(&baroI2C_handle, ADDRESS_BARO, CMD_ADC_CONV + (CMD_ADC_D1 + CMD_ADC_4096),1, 1000); // 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 + 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 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; diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index ad801a9..03bc4b0 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -187,7 +187,7 @@ void systemTaskBaro(void) -// //read the barometer + //read the barometer // I2C_HandleTypeDef i2c_profile; // // //---- COMPAS WORKING ---- @@ -211,7 +211,7 @@ void systemTaskBaro(void) // // while (1) // { -// i2c_send(&i2c_profile, 0x3D, request_data, 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); // @@ -225,17 +225,17 @@ void systemTaskBaro(void) // int16_t y = (~(*(int16_t*)(response_data+4)))+1; // int stop = 5 *2; // } - - - - - - - - - - - +// +// +// +// +// +// +// +// +// +// +// From ec395d11e07931236ff854b220d05dfc3c185300 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Tue, 25 Oct 2016 17:10:36 +0200 Subject: [PATCH 06/25] Working software ic2 barometer Can read from baro and get temp, preassure and alt. Will look into optimizations. --- UAV-ControlSystem/inc/drivers/barometer.h | 2 +- UAV-ControlSystem/src/drivers/I2C.c | 6 +- UAV-ControlSystem/src/drivers/barometer.c | 214 +++++++--------------- UAV-ControlSystem/src/tasks_main.c | 14 +- 4 files changed, 83 insertions(+), 153 deletions(-) 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) From fe18550ca8a22246ab9924e5023e72e342fd5674 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Wed, 26 Oct 2016 15:25:36 +0200 Subject: [PATCH 07/25] Some sort of comm possible using hardware I2C Can get response by hardware but its not very fast since the setting are not optimized at all --- UAV-ControlSystem/src/drivers/I2C.c | 13 +++++---- UAV-ControlSystem/src/drivers/barometer.c | 32 ++++++++++++++++++----- 2 files changed, 34 insertions(+), 11 deletions(-) diff --git a/UAV-ControlSystem/src/drivers/I2C.c b/UAV-ControlSystem/src/drivers/I2C.c index 737946c..9fb8d48 100644 --- a/UAV-ControlSystem/src/drivers/I2C.c +++ b/UAV-ControlSystem/src/drivers/I2C.c @@ -34,8 +34,8 @@ bool i2c_configure(I2C_TypeDef *i2c, i2c_port = I2C1_PORT; sda_pin = I2C1_SDA_PIN; scl_pin = I2C1_SCL_PIN; - if(__HAL_RCC_I2C1_IS_CLK_DISABLED()) - __HAL_RCC_I2C1_CLK_ENABLE(); +// if(__HAL_RCC_I2C1_IS_CLK_DISABLED()) +// __HAL_RCC_I2C1_CLK_ENABLE(); } else if(i2c == I2C2) { @@ -55,15 +55,18 @@ 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_OUTPUT_OD/*GPIO_MODE_AF_PP*/; - GPIO_InitStruct.Pull = GPIO_NOPULL/*GPIO_PULLUP*/; + 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(); //Initialize I2C communication out_profile->Instance = i2c; - out_profile->Init.ClockSpeed = 400000; + out_profile->Init.ClockSpeed = 100000; out_profile->Init.DutyCycle = I2C_DUTYCYCLE_2/*I2C_DUTYCYCLE_2*/; out_profile->Init.OwnAddress1 = my_address; out_profile->Init.OwnAddress2 = 0; diff --git a/UAV-ControlSystem/src/drivers/barometer.c b/UAV-ControlSystem/src/drivers/barometer.c index e7f1d5a..93590fe 100644 --- a/UAV-ControlSystem/src/drivers/barometer.c +++ b/UAV-ControlSystem/src/drivers/barometer.c @@ -367,6 +367,18 @@ bool barometer_reset() coefficients_arr[i] = rxbuf[0] << 8 | rxbuf[1]; } + uint8_t cobuf2[3] = {0}; + cobuf2[0] = CMD_ADC_CONV + (CMD_ADC_D2 + CMD_ADC_4096); + HAL_GPIO_DeInit(I2C1_PORT, I2C1_SCL_PIN); + HAL_GPIO_DeInit(I2C1_PORT, I2C1_SDA_PIN); + i2c_configure(I2C1, &baroI2C_handle, 0x0); + bool isSent = i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf2, 1); + HAL_Delay(20); + + cobuf2[0] = CMD_ADC_READ; + 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 */ /* 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 ++) @@ -442,26 +454,34 @@ void barometer_CaclulateValues() 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 + //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); + 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 break; case CALCSTATE_D2_READ: - //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 + //TEST_i2cRead(I2C1, ADDRESS_BARO, CMD_ADC_READ, 3, cobuf); // send PROM READ command + cobuf[0] = CMD_ADC_READ; + 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 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 + //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); + 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 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 + //TEST_i2cRead(I2C1, ADDRESS_BARO, CMD_ADC_READ, 3, cobuf); // send PROM READ command + cobuf[0] = CMD_ADC_READ; + 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 currentCalculationState = CALCSTATE_CALCULATE_PTA; break; From 69c9f22dadedf6eeebb7a53bf21589969cd6e495 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Wed, 26 Oct 2016 16:22:33 +0200 Subject: [PATCH 08/25] Pid update angle caluclations --- UAV-ControlSystem/src/Flight/pid.c | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 2ddce64..065ad46 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -65,15 +65,21 @@ pt1Filter_t accelFilter[2] = {0}; float calcAngle(const uint8_t axis, const float x_axis, const float y_axis, const float z_axis) { float angle; + float angle_offset = (z_axis < 0 )? 90: 0; switch (axis) { case ROLL: - angle = atan2(z_axis, sqrt(x_axis*x_axis + y_axis*y_axis))*180/M_PI - 90; - angle = (x_axis < 0) ? -angle : angle; /*CW (right, form behind) = pos angle*/ + + angle = atan2(x_axis, sqrt(y_axis*y_axis + z_axis*z_axis))*180/M_PI; + angle = -1*((angle > 0)? (z_axis < 0 )? 180 - angle: angle : (z_axis < 0 )? - 180 - angle: angle); + break; case PITCH: - angle = atan2(sqrt(x_axis*x_axis + z_axis*z_axis), y_axis)*180/M_PI - 90; /*down (the front down against ground) = pos angle*/ + + angle = atan2( y_axis, sqrt(z_axis*z_axis + x_axis*x_axis))*180/M_PI; /*down (the front down against ground) = pos angle*/ + angle = (angle > 0)? ((z_axis < 0))? 180 - angle: angle : (z_axis < 0 )? - 180 - angle: angle; + break; default: angle = 0; From c3e969e982ed81e95d4aa710aaf71ea5219856c4 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Thu, 27 Oct 2016 11:55:16 +0200 Subject: [PATCH 09/25] PID Added new values to improve filter --- UAV-ControlSystem/src/Flight/pid.c | 20 +++++++++++++++++--- UAV-ControlSystem/src/drivers/motormix.c | 8 ++++---- 2 files changed, 21 insertions(+), 7 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 065ad46..98ffe07 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -26,7 +26,7 @@ #define RADIO_RANGE 500 /*Radio range input*/ #define BAROMETER_RANGE 2000 /*Determines the range of the maximum height (limits the rc input)*/ -#define ACCELEROMETER_RANGE 20 /*Determines the range of the maximum angle (limits the rc input) & (Accelerometer takes int to max 16 G)*/ +#define ACCELEROMETER_RANGE 35 /*Determines the range of the maximum angle (limits the rc input) & (Accelerometer takes int to max 16 G)*/ #define GYRO_RANGE 720 /*Determines the maximum rotational limit (limits the rc input)*/ #define COMPASS_RANGE 180 /*Determines the maximum compass limit (limits the rc input)*/ @@ -113,6 +113,8 @@ float constrainf(float amt, int low, int high) return amt; } + +float oldSensorValue[2] = {0}; /************************************************************************** * BRIEF: Update current sensor values * * INFORMATION: * @@ -136,10 +138,20 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) + float alpha = 0.5; /*May need Low pass filter since the accelerometer may drift*/ - sensorValues[ROLL] = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); - sensorValues[PITCH] = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); + float X_roll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); + float X_pitch = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); + + sensorValues[ROLL] = alpha*X_roll + (1-alpha)*oldSensorValue[0]; + sensorValues[PITCH] = alpha*X_pitch + (1-alpha)*oldSensorValue[1]; + + oldSensorValue[0] = sensorValues[ROLL]; + oldSensorValue[1] = sensorValues[PITCH]; + +// sensorValues[ROLL] = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); +// sensorValues[PITCH] = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); //float sensorRoll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); //sensorValues[ROLL] = pt1FilterApply4(&accelFilter[0], sensorRoll, 90, PidProfileBuff[ROLL].dT); @@ -384,6 +396,8 @@ void pidRun(uint8_t ID) } } +/*--------------------------------------------Init Functions----------------------------------------------------------------------------------*/ + /************************************************************************** * BRIEF: Initializes a certain pidbuffer profile connected to * * a PID controller * diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index 22624cf..3da3fec 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -114,8 +114,8 @@ void mix() * Calculation is: Output from control system * weight from model for each motor */ RPY_Mix[i] = \ - PidProfile[PID_ID_GYRO].PID_Out[ROLL] * mixerUAV[i].roll + \ - PidProfile[PID_ID_GYRO].PID_Out[PITCH] * mixerUAV[i].pitch + \ + (int)PidProfile[PID_ID_GYRO].PID_Out[ROLL] * mixerUAV[i].roll + \ + (int)PidProfile[PID_ID_GYRO].PID_Out[PITCH] * mixerUAV[i].pitch + \ PidProfile[PID_ID_GYRO].PID_Out[YAW] * mixerUAV[i].yaw * ((mixerConfig.yaw_reverse_direction) ? -1 : 1); @@ -194,8 +194,8 @@ void mix() { RPY_Mix[i] = \ throttle * mixerUAV[i].throttle + \ - PidProfile[PID_ID_GYRO].PID_Out[ROLL] * mixerUAV[i].roll + \ - PidProfile[PID_ID_GYRO].PID_Out[PITCH] * mixerUAV[i].pitch + \ + (int)PidProfile[PID_ID_GYRO].PID_Out[ROLL] * mixerUAV[i].roll + \ + (int)PidProfile[PID_ID_GYRO].PID_Out[PITCH] * mixerUAV[i].pitch + \ PidProfile[PID_ID_GYRO].PID_Out[YAW] * mixerUAV[i].yaw * ((mixerConfig.yaw_reverse_direction) ? -1 : 1); } From 3d3d16ceadd4dc842c9e9abb3d6d55768971c0c5 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Thu, 27 Oct 2016 14:30:26 +0200 Subject: [PATCH 10/25] PID added new lowpas filter and average calculations to Accelerometer values --- UAV-ControlSystem/src/Flight/pid.c | 30 ++++++++++++++++++++++++++++-- 1 file changed, 28 insertions(+), 2 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 98ffe07..d263d18 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -115,12 +115,19 @@ float constrainf(float amt, int low, int high) float oldSensorValue[2] = {0}; +float oldSensorValueRoll[50] = {0}; +float oldSensorValuePitch[50] = {0}; + +int i = 0; + /************************************************************************** * BRIEF: Update current sensor values * * INFORMATION: * **************************************************************************/ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) { + + switch (ID_profile) { case PID_ID_GYRO: @@ -144,9 +151,28 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) float X_roll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); float X_pitch = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); - sensorValues[ROLL] = alpha*X_roll + (1-alpha)*oldSensorValue[0]; - sensorValues[PITCH] = alpha*X_pitch + (1-alpha)*oldSensorValue[1]; + oldSensorValueRoll[i] = X_roll; + oldSensorValuePitch[i] = X_pitch; + float RollValue = 0; + float PitchValue = 0; + + for (int ii = 0; ii < 50; ii++) + { + RollValue = RollValue + oldSensorValueRoll[ii]; + PitchValue = PitchValue + oldSensorValuePitch[ii]; + + } + + + i = (i < 49)? i + 1:0; + + sensorValues[ROLL] = RollValue/50; + sensorValues[PITCH] = PitchValue/50; + + sensorValues[ROLL] = alpha*RollValue/50 + (1-alpha)*oldSensorValue[0]; + sensorValues[PITCH] = alpha*PitchValue/50 + (1-alpha)*oldSensorValue[1]; +// oldSensorValue[0] = sensorValues[ROLL]; oldSensorValue[1] = sensorValues[PITCH]; From 428a6140b412af0c761f76fb22cb524569fcf424 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Fri, 28 Oct 2016 08:39:05 +0200 Subject: [PATCH 11/25] PID added new dweight values --- UAV-ControlSystem/src/Flight/pid.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index d263d18..335013a 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -265,7 +265,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, /* -----calculate P component ---- */ - float PTerm = PTERM_SCALE * rateError * pidProfile->P[axis] * pidProfile-> PIDweight[axis] / 100; + float PTerm = PTERM_SCALE * rateError * (float)pidProfile->P[axis] * (float)pidProfile-> PIDweight[axis] / 100.0; // Constrain YAW by yaw_p_limit value if (axis == YAW) @@ -328,7 +328,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, delta = pt1FilterApply4(&pidProfileBuff->deltaFilter[axis], delta, pidProfile->dterm_lpf, pidProfileBuff->dT); } - DTerm = DTERM_SCALE * delta * pidProfile->D[axis] * pidProfile->PIDweight[axis] / 100; + DTerm = DTERM_SCALE * delta * (float)pidProfile->D[axis] * (float)pidProfile->PIDweight[axis] / 100.0; DTerm = constrainf(DTerm, -PID_MAX_D, PID_MAX_D); } @@ -520,6 +520,8 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID) PidProfile[ID].pidEnabled = true; PidProfile[ID].dterm_lpf = 90; PidProfile[ID].pid_out_limit = 1000; + pidProfile[ID].PIDweight[ROLL] = 50; + pidProfile[ID].PIDweight[PITCH] = 50; break; case PID_ID_COMPASS: From 738dca560f92dfd837381a8363c4a6abe8851711 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Fri, 28 Oct 2016 10:22:13 +0200 Subject: [PATCH 12/25] Added I2C soft implementation. Added barometer functionality. In this commit the the barometer is commented out and is not used in the system. This part also has athe beginnings of the calibration functions for accel and gyro --- UAV-ControlSystem/inc/drivers/I2C.h | 12 + .../inc/drivers/failsafe_toggles.h | 28 +- UAV-ControlSystem/inc/drivers/i2c_soft.h | 28 + UAV-ControlSystem/inc/stm32f4xx_revo.h | 2 + UAV-ControlSystem/src/Scheduler/tasks.c | 2 +- UAV-ControlSystem/src/drivers/I2C.c | 142 ++++- UAV-ControlSystem/src/drivers/barometer.c | 499 +++++++----------- UAV-ControlSystem/src/drivers/i2c_soft.c | 241 +++++++++ UAV-ControlSystem/src/main.c | 6 +- UAV-ControlSystem/src/tasks_main.c | 71 +-- .../HAL_Driver/Src/stm32f4xx_hal_i2c.c | 2 + 11 files changed, 638 insertions(+), 395 deletions(-) create mode 100644 UAV-ControlSystem/inc/drivers/i2c_soft.h create mode 100644 UAV-ControlSystem/src/drivers/i2c_soft.c 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); From c57ec513973d7f444925d62f8ac3cd9bb4295c63 Mon Sep 17 00:00:00 2001 From: philsson Date: Fri, 28 Oct 2016 10:22:42 +0200 Subject: [PATCH 13/25] Temp fix for eeprom problem --- UAV-ControlSystem/inc/Flight/pid.h | 2 ++ UAV-ControlSystem/src/Flight/pid.c | 30 +++++++++++++++++++++++++----- UAV-ControlSystem/src/main.c | 4 +++- 3 files changed, 30 insertions(+), 6 deletions(-) diff --git a/UAV-ControlSystem/inc/Flight/pid.h b/UAV-ControlSystem/inc/Flight/pid.h index dd9f80f..a19bd81 100644 --- a/UAV-ControlSystem/inc/Flight/pid.h +++ b/UAV-ControlSystem/inc/Flight/pid.h @@ -75,4 +75,6 @@ void pidInit(); **************************************************************************/ void pidRun(uint8_t ID); +void pidEproom(void); + #endif /* FLIGHT_PID_H_ */ diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 335013a..b614907 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -498,9 +498,6 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID) PidProfile[ID].PID_Out[PITCH] = 0; PidProfile[ID].PID_Out[YAW] = 0; - PidProfile[ID].PIDweight[ROLL] = 100; - PidProfile[ID].PIDweight[PITCH] = 100; - PidProfile[ID].PIDweight[YAW] = 100; PidProfile[ID].P[ROLL] = 10; PidProfile[ID].P[PITCH] = 10; @@ -510,6 +507,10 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID) { case PID_ID_GYRO: + PidProfile[ID].PIDweight[ROLL] = 100; + PidProfile[ID].PIDweight[PITCH] = 100; + PidProfile[ID].PIDweight[YAW] = 100; + PidProfile[ID].pidEnabled = true; PidProfile[ID].dterm_lpf = 90; PidProfile[ID].pid_out_limit = 3000; @@ -517,20 +518,30 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID) break; case PID_ID_ACCELEROMETER: + PidProfile[ID].PIDweight[ROLL] = 2; + PidProfile[ID].PIDweight[PITCH] = 2; + PidProfile[ID].PIDweight[YAW] = 100; + PidProfile[ID].pidEnabled = true; PidProfile[ID].dterm_lpf = 90; PidProfile[ID].pid_out_limit = 1000; - pidProfile[ID].PIDweight[ROLL] = 50; - pidProfile[ID].PIDweight[PITCH] = 50; break; case PID_ID_COMPASS: + PidProfile[ID].PIDweight[ROLL] = 100; + PidProfile[ID].PIDweight[PITCH] = 100; + PidProfile[ID].PIDweight[YAW] = 100; + PidProfile[ID].pidEnabled = false; break; case PID_ID_BAROMETER: + PidProfile[ID].PIDweight[ROLL] = 100; + PidProfile[ID].PIDweight[PITCH] = 100; + PidProfile[ID].PIDweight[YAW] = 100; + PidProfile[ID].pidEnabled = false; break; @@ -560,3 +571,12 @@ void pidInit() pidUAVInit(&PidProfile[PID_ID_BAROMETER], PID_ID_BAROMETER); pidUAVInit(&PidProfile[PID_ID_COMPASS], PID_ID_COMPASS); } + +void pidEproom(void) +{ + + PidProfile[PID_ID_ACCELEROMETER].PIDweight[ROLL] = 2; + PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 2; + PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100; + +} diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index b69cd1d..3edd170 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -51,8 +51,10 @@ void init_system() /* read saved variables from eeprom, in most cases eeprom should be read after a lot of the initializes */ readEEPROM(); + pidEproom(); + //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(); From 0f37f6600d71520f269e1f12780cc01ee8d49381 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Fri, 28 Oct 2016 14:34:49 +0200 Subject: [PATCH 14/25] Added calibration for accelerometer --- UAV-ControlSystem/inc/Flight/pid.h | 7 ++ UAV-ControlSystem/inc/config/eeprom.h | 9 +++ UAV-ControlSystem/inc/drivers/accel_gyro.h | 2 +- .../inc/drivers/failsafe_toggles.h | 38 ++++++++++ UAV-ControlSystem/inc/system_variables.h | 2 +- UAV-ControlSystem/src/Flight/pid.c | 41 ++++++++-- UAV-ControlSystem/src/Scheduler/tasks.c | 2 +- UAV-ControlSystem/src/config/cli.c | 22 +++++- UAV-ControlSystem/src/config/eeprom.c | 29 +++++++- UAV-ControlSystem/src/drivers/accel_gyro.c | 2 +- .../src/drivers/failsafe_toggles.c | 74 ++++++++++++++++--- UAV-ControlSystem/src/drivers/motormix.c | 4 +- UAV-ControlSystem/src/main.c | 11 ++- UAV-ControlSystem/src/tasks_main.c | 68 +++++++++++++---- 14 files changed, 266 insertions(+), 45 deletions(-) diff --git a/UAV-ControlSystem/inc/Flight/pid.h b/UAV-ControlSystem/inc/Flight/pid.h index a19bd81..39728c7 100644 --- a/UAV-ControlSystem/inc/Flight/pid.h +++ b/UAV-ControlSystem/inc/Flight/pid.h @@ -18,6 +18,7 @@ #include #include #include "Flight/filter.h" +#include "drivers/accel_gyro.h" #define XYZ_AXIS_COUNT 3 /*The maximum number of DOF that belongings to the PID*/ @@ -60,6 +61,12 @@ typedef struct pidProfile_s { /*Array of all pid profiles of the system*/ extern pidProfile_t PidProfile[PID_COUNT]; +/* */ +extern float accRollFineTune; +extern float accPitchFineTune; + +extern accel_t accelProfile; /*Struct profile for input data from sensor*/ + /*Is set in motor mix and used in pidUAVcore and mix */ bool motorLimitReached; diff --git a/UAV-ControlSystem/inc/config/eeprom.h b/UAV-ControlSystem/inc/config/eeprom.h index 282761d..1774a35 100644 --- a/UAV-ControlSystem/inc/config/eeprom.h +++ b/UAV-ControlSystem/inc/config/eeprom.h @@ -191,6 +191,15 @@ typedef enum { EEPROM_FLAG_MIXERLOWSCALE, EEPROM_FLAG_FLIGHTMODE_3, + /* accel calibration values */ + EEPROM_FLAG_ACCELTUNE_OFFSET_X, + EEPROM_FLAG_ACCELTUNE_OFFSET_Y, + EEPROM_FLAG_ACCELTUNE_OFFSET_Z, + + /* accel calibration fine tune values */ + EEPROM_FLAG_ACCELTUNE_FINE_ROLL, + EEPROM_FLAG_ACCELTUNE_FINE_PITCH, + /* Counts the amount of system settings */ EEPROM_SYS_COUNT } EEPROM_SYS_ID_t; diff --git a/UAV-ControlSystem/inc/drivers/accel_gyro.h b/UAV-ControlSystem/inc/drivers/accel_gyro.h index fd42421..805964d 100644 --- a/UAV-ControlSystem/inc/drivers/accel_gyro.h +++ b/UAV-ControlSystem/inc/drivers/accel_gyro.h @@ -27,7 +27,7 @@ #ifndef DRIVERS_ACCEL_GYRO_H_ #define DRIVERS_ACCEL_GYRO_H_ -#include +//#include #include "stm32f4xx.h" #include "stm32f4xx_revo.h" diff --git a/UAV-ControlSystem/inc/drivers/failsafe_toggles.h b/UAV-ControlSystem/inc/drivers/failsafe_toggles.h index 0a0d3bc..8e1044f 100644 --- a/UAV-ControlSystem/inc/drivers/failsafe_toggles.h +++ b/UAV-ControlSystem/inc/drivers/failsafe_toggles.h @@ -47,6 +47,15 @@ #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 +/* Stick booleans */ +#define systemFlags_throttleMax_id 10 + boolean_vals_offset +#define systemFlags_stickLeft_id 11 + boolean_vals_offset +#define systemFlags_stickRight_id 12 + boolean_vals_offset +#define systemFlags_stickUp_id 13 + boolean_vals_offset +#define systemFlags_stickDown_id 14 + boolean_vals_offset +#define systemFlags_stickCenterH_id 15 + boolean_vals_offset +#define systemFlags_stickCenterV_id 16 + boolean_vals_offset +#define systemFlags_throttleLeft_id 17 + boolean_vals_offset /*Mask values for each of the flag values*/ @@ -66,6 +75,15 @@ #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) +/* Stick booleans */ +#define systemFlags_throttleMax_mask getFlagMaskValue(systemFlags_throttleMax_id) +#define systemFlags_stickLeft_mask getFlagMaskValue(systemFlags_stickLeft_id) +#define systemFlags_stickRight_mask getFlagMaskValue(systemFlags_stickRight_id +#define systemFlags_stickUp_mask getFlagMaskValue(systemFlags_stickUp_id) +#define systemFlags_stickDown_mask getFlagMaskValue(systemFlags_stickDown_id) +#define systemFlags_stickCenterH_mask getFlagMaskValue(systemFlags_stickCenterH_id) +#define systemFlags_stickCenterV_mask getFlagMaskValue(systemFlags_stickCenterV_id) +#define systemFlags_throttleLeft_mask getFlagMaskValue(systemFlags_throttleLeft_id) @@ -92,6 +110,16 @@ typedef union bitSetRegion booleanValue_t flightMode_3 : 1; booleanValue_t barometerIsCalibrated : 1; booleanValue_t AcceleromterIsCalibrated : 1; + /* Stick booleans */ + booleanValue_t throttleMax : 1; + booleanValue_t stickLeft : 1; + booleanValue_t stickRight : 1; + booleanValue_t stickUp : 1; + booleanValue_t stickDown : 1; + booleanValue_t stickCenterH : 1; + booleanValue_t stickCenterV : 1; + booleanValue_t throttleLeft : 1; + }bitField; uint64_t intRepresentation; }boolFlags_t; @@ -115,6 +143,16 @@ typedef enum FLAG_CONFIGURATION_MIXERFULLSCALE, FLAG_CONFIGURATION_MIXERLOWSCALE, FLAG_CONFIGURATION_FLIGHTMODE_3, + /* Stick booleans */ + FLAG_CONFIGURATION_THROTTLEMAX, + FLAG_CONFIGURATION_STICKLEFT, + FLAG_CONFIGURATION_STICKRIGHT, + FLAG_CONFIGURATION_STICKUP, + FLAG_CONFIGURATION_STICKDOWN, + FLAG_CONFIGURATION_STICKCENTERH, + FLAG_CONFIGURATION_STICKCENTERV, + FLAG_CONFIGURATION_THROTTLELEFT, + //Counter FLAG_CONFIGURATION_COUNT diff --git a/UAV-ControlSystem/inc/system_variables.h b/UAV-ControlSystem/inc/system_variables.h index 607b8e1..0217fd9 100644 --- a/UAV-ControlSystem/inc/system_variables.h +++ b/UAV-ControlSystem/inc/system_variables.h @@ -14,7 +14,7 @@ #ifndef SYSTEM_VARIABLES_H_ #define SYSTEM_VARIABLES_H_ -#define EEPROM_SYS_VERSION 102 +#define EEPROM_SYS_VERSION 107 #define ADC_STATE #include "stm32f4xx.h" diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index b614907..c595d16 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -14,7 +14,6 @@ * **************************************************************************/ #include "Flight/pid.h" -#include "drivers/accel_gyro.h" #include "drivers/sbus.h" #include "scheduler/scheduler.h" #include @@ -53,11 +52,14 @@ typedef struct pidProfileBuff_s { pidProfile_t PidProfile[PID_COUNT] = {0}; /*Array of all pid profiles of the system*/ pidProfileBuff_t PidProfileBuff[PID_COUNT] = {0}; -accel_t accelProfile; /*Struct profile for input data from sensor*/ -gyro_t gyroProfile; /*Struct profile for input data from sensor*/ +accel_t accelProfile = {0}; /*Struct profile for input data from sensor*/ +gyro_t gyroProfile = {0}; /*Struct profile for input data from sensor*/ pt1Filter_t accelFilter[2] = {0}; +float accRollFineTune = 0; +float accPitchFineTune = 0; + /************************************************************************** * BRIEF: Calculates angle from accelerometer * * INFORMATION: * @@ -151,6 +153,11 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) float X_roll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); float X_pitch = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); + /*TODO add finetune for roll and pitch*/ + X_roll += accRollFineTune; + X_pitch += accPitchFineTune; + + oldSensorValueRoll[i] = X_roll; oldSensorValuePitch[i] = X_pitch; @@ -499,14 +506,20 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID) PidProfile[ID].PID_Out[YAW] = 0; - PidProfile[ID].P[ROLL] = 10; - PidProfile[ID].P[PITCH] = 10; - PidProfile[ID].P[YAW] = 10; + switch (ID) { case PID_ID_GYRO: + PidProfile[ID].P[ROLL] = 150; + PidProfile[ID].P[PITCH] = 135; + PidProfile[ID].P[YAW] = 150; + + PidProfile[ID].D[ROLL] = 75; + PidProfile[ID].D[PITCH] = 95; + PidProfile[ID].D[YAW] = 50; + PidProfile[ID].PIDweight[ROLL] = 100; PidProfile[ID].PIDweight[PITCH] = 100; PidProfile[ID].PIDweight[YAW] = 100; @@ -518,6 +531,14 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID) break; case PID_ID_ACCELEROMETER: + PidProfile[ID].P[ROLL] = 90; + PidProfile[ID].P[PITCH] = 90; + PidProfile[ID].P[YAW] = 0; + + PidProfile[ID].D[ROLL] = 40; + PidProfile[ID].D[PITCH] = 40; + PidProfile[ID].D[YAW] = 0; + PidProfile[ID].PIDweight[ROLL] = 2; PidProfile[ID].PIDweight[PITCH] = 2; PidProfile[ID].PIDweight[YAW] = 100; @@ -529,6 +550,10 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID) break; case PID_ID_COMPASS: + PidProfile[ID].P[ROLL] = 10; + PidProfile[ID].P[PITCH] = 10; + PidProfile[ID].P[YAW] = 10; + PidProfile[ID].PIDweight[ROLL] = 100; PidProfile[ID].PIDweight[PITCH] = 100; PidProfile[ID].PIDweight[YAW] = 100; @@ -538,6 +563,10 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID) break; case PID_ID_BAROMETER: + PidProfile[ID].P[ROLL] = 10; + PidProfile[ID].P[PITCH] = 10; + PidProfile[ID].P[YAW] = 10; + PidProfile[ID].PIDweight[ROLL] = 100; PidProfile[ID].PIDweight[PITCH] = 100; PidProfile[ID].PIDweight[YAW] = 100; diff --git a/UAV-ControlSystem/src/Scheduler/tasks.c b/UAV-ControlSystem/src/Scheduler/tasks.c index aa17518..f06d65d 100644 --- a/UAV-ControlSystem/src/Scheduler/tasks.c +++ b/UAV-ControlSystem/src/Scheduler/tasks.c @@ -88,7 +88,7 @@ task_t SystemTasks[TASK_COUNT] = { .taskName = "SERIAL", .taskFunction = systemTaskSerial, - .desiredPeriod = GetUpdateRateHz(100), //100 hz update rate (10 ms) + .desiredPeriod = GetUpdateRateHz(2), //100 hz update rate (10 ms) .staticPriority = PRIORITY_LOW, }, diff --git a/UAV-ControlSystem/src/config/cli.c b/UAV-ControlSystem/src/config/cli.c index e6fda2c..80481cf 100644 --- a/UAV-ControlSystem/src/config/cli.c +++ b/UAV-ControlSystem/src/config/cli.c @@ -24,6 +24,8 @@ #include "utilities.h" #include "Scheduler/scheduler.h" #include "drivers/motors.h" +#include "drivers/accel_gyro.h" +#include "Flight/pid.h" #define cliActivateCharacter 35 #define commandValueError 0xFFFFFFFFFFFFFFFF @@ -76,6 +78,7 @@ typedef enum ACTION_GYROCALIBRATE, ACTION_ACCELEROMTETERCALIBRATE, ACTION_COMPASSCALIBRATE, + ACTION_CALIBRATIONINFOACCEL, //The number of actions ACTION_COUNT, //Count of the number of actions @@ -100,7 +103,8 @@ const typeString commandAction_Strings[ACTION_COUNT] = { "calibrate_motors", "calibrate_gyro", "calibrate_accelerometer", - "calibrate_compass" + "calibrate_compass", + "calibration_info_accelerometer" }; /* String values descrbing information of a certain action */ @@ -120,7 +124,8 @@ const typeString commandActionInformation_Strings[ACTION_COUNT] = { "| calibrate_motors - Calibrates all motors.", "| calibrate_gyro - Calibrates the gyro.", "| calibrate_accelerometer - Calibrates the accelerometer.", - "| calibrate_compass - Calibrates the compass." + "| calibrate_compass - Calibrates the compass.", + "| calibration_info_accelerometer - Provides info on the accelerometer calibration." }; /* String array containing all the signature examples for each action */ @@ -140,7 +145,8 @@ const typeString commandActionSignature_Strings[ACTION_COUNT] = { " calibrate_motors", " calibrate_gyro", " calibrate_accelerometer", - " calibrate_compass" + " calibrate_compass", + " calibration_info_accelerometer" }; /* Size values for the values a command will require */ @@ -1477,7 +1483,6 @@ void cliRun() if (ReceiveBinaryDecision(121,110)) { TransmitBack("Starting calibration! \n\n\r"); - TransmitBack("NOT IMPLEMENTED! \n\n\r"); TransmitBack("Calibration complete! \n\n\r"); } else @@ -1500,6 +1505,15 @@ void cliRun() } break; + case commandMask(commandSize_1, ACTION_CALIBRATIONINFOACCEL): + { + char tempBuffer[100]; + TransmitBack("- Accelerometer calibration information: \n\n\r"); + TransmitBack(tempBuffer); + sprintf(tempBuffer, "- BaseTune: \n\r-OffsetX: %d \n\r-OffsetY: %d \n\r-OffsetZ: %d \n\n\r", accelProfile.offsetX, accelProfile.offsetY, accelProfile.offsetZ); + + break; + } default: if (actionId != ACTION_NOACTION) TransmitCommandInstruction(actionId); diff --git a/UAV-ControlSystem/src/config/eeprom.c b/UAV-ControlSystem/src/config/eeprom.c index c58f327..7852cc1 100644 --- a/UAV-ControlSystem/src/config/eeprom.c +++ b/UAV-ControlSystem/src/config/eeprom.c @@ -248,9 +248,34 @@ EEPROM_DATA_t eeprom_sys_Arr[EEPROM_SYS_COUNT] = { .dataPtr = &(flagConfigArr[FLAG_CONFIGURATION_FLIGHTMODE_3]), }, + /* accel calibration values */ + [EEPROM_FLAG_ACCELTUNE_OFFSET_X] = + { + .size = sizeof(int16_t), + .dataPtr = &(accelProfile.offsetX), + }, + [EEPROM_FLAG_ACCELTUNE_OFFSET_Y] = + { + .size = sizeof(int16_t), + .dataPtr = &(accelProfile.offsetY), + }, + [EEPROM_FLAG_ACCELTUNE_OFFSET_Z] = + { + .size = sizeof(int16_t), + .dataPtr = &(accelProfile.offsetZ), + }, - - + /* accel fine tune */ + [EEPROM_FLAG_ACCELTUNE_FINE_ROLL] = + { + .size = sizeof(float), + .dataPtr = &(accRollFineTune), + }, + [EEPROM_FLAG_ACCELTUNE_FINE_PITCH] = + { + .size = sizeof(float), + .dataPtr = &(accPitchFineTune), + }, }; /* Data pointers and sizes for profile content */ diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c index c77a4f7..759dcc5 100644 --- a/UAV-ControlSystem/src/drivers/accel_gyro.c +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -239,7 +239,7 @@ bool mpu6000_init(gyro_t* gyro, accel_t* accel) return false; mpu6000_read_gyro_offset(gyro); - mpu6000_read_acc_offset(accel); + //mpu6000_read_acc_offset(accel); HAL_Delay(60); diff --git a/UAV-ControlSystem/src/drivers/failsafe_toggles.c b/UAV-ControlSystem/src/drivers/failsafe_toggles.c index 3101c34..58b00ce 100644 --- a/UAV-ControlSystem/src/drivers/failsafe_toggles.c +++ b/UAV-ControlSystem/src/drivers/failsafe_toggles.c @@ -29,14 +29,14 @@ boolFlags_t systemFlags = {{0}}; flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT] = { [FLAG_CONFIGURATION_ARM] = { .minRange = 1500, - .maxRange = 2000, + .maxRange = 2100, .channelNumber = 8, .flagId = systemFlags_armed_id, }, [FLAG_CONFIGURATION_FLIGHTMODE_ACCELEROMETER] = { - .minRange = 1600, - .maxRange = 2000, - .channelNumber = 5, + .minRange = 1400, + .maxRange = 2100, + .channelNumber = 6, .flagId = systemFlags_flightmode_acceleromter_id, }, [FLAG_CONFIGURATION_FLIGHTMODE_BAROMETER] = { @@ -58,15 +58,15 @@ flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT] = { .flagId = systemFlags_flightmode_gps_id, }, [FLAG_CONFIGURATION_MIXERFULLSCALE] = { - .minRange = 0, - .maxRange = 0, - .channelNumber = 0, + .minRange = 1900, + .maxRange = 2100, + .channelNumber = 5, .flagId = systemFlags_mixerfullscale_id, }, [FLAG_CONFIGURATION_MIXERLOWSCALE] = { - .minRange = 0, - .maxRange = 0, - .channelNumber = 0, + .minRange = 900, + .maxRange = 1100, + .channelNumber = 5, .flagId = systemFlags_mixerlowscale_id, }, [FLAG_CONFIGURATION_FLIGHTMODE_3] = { @@ -74,7 +74,59 @@ flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT] = { .maxRange = 0, .channelNumber = 0, .flagId = systemFlags_flightMode_3_id, + }, + + /*Stick controls*/ + [FLAG_CONFIGURATION_THROTTLEMAX] = { + .minRange = 1950, + .maxRange = 2000, + .channelNumber = 3, + .flagId = systemFlags_throttleMax_id, + }, + [FLAG_CONFIGURATION_STICKLEFT] = { //negative + .minRange = 1000, + .maxRange = 1100, + .channelNumber = 1, + .flagId = systemFlags_stickLeft_id, + }, + [FLAG_CONFIGURATION_STICKRIGHT] = { //positive + .minRange = 1900, + .maxRange = 2000, + .channelNumber = 1, + .flagId = systemFlags_stickRight_id, + }, + [FLAG_CONFIGURATION_STICKUP] = { //negative + .minRange = 1000, + .maxRange = 1100, + .channelNumber = 2, + .flagId = systemFlags_stickUp_id, + }, + [FLAG_CONFIGURATION_STICKDOWN] = { //positive + .minRange = 1900, + .maxRange = 2000, + .channelNumber = 2, + .flagId = systemFlags_stickDown_id, + }, + [FLAG_CONFIGURATION_STICKCENTERH] = { + .minRange = 1400, + .maxRange = 1600, + .channelNumber = 1, + .flagId = systemFlags_stickCenterH_id, + }, + [FLAG_CONFIGURATION_STICKCENTERV] = { + .minRange = 1400, + .maxRange = 1600, + .channelNumber = 2, + .flagId = systemFlags_stickCenterV_id, + }, + [FLAG_CONFIGURATION_THROTTLELEFT] = { + .minRange = 2000, + .maxRange = 1900, + .channelNumber = 4, + .flagId = systemFlags_throttleLeft_id, } + + }; /*********************************************************************** @@ -123,7 +175,7 @@ void flags_ProcessRcChannel_Improved(uint8_t minChannel, uint8_t maxChannel) { int currentChannelNumb = flagConfigArr[i].channelNumber; //Check if the current Flag channel is within the set bounds - if(currentChannelNumb >= 5 && currentChannelNumb <= maxChannel) + if(currentChannelNumb >= minChannel && currentChannelNumb <= maxChannel) { //Get the value for the channel that the current flag is linked to int value = getChannelValue(sbusChannelData, currentChannelNumb); diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index 3da3fec..1261837 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -45,14 +45,14 @@ uint16_t motor_output[MOTOR_COUNT]; /* Default values for the mixerConfig */ // TODO: Implement in EEPROM mixerConfig_s mixerConfig = { - .minThrottle = 1050, + .minThrottle = 1040, .maxThrottle = MAX_PULSE - 100, .minCommand = 990, .maxCommand = MAX_PULSE, .minCheck = 1010, .pid_at_min_throttle = true, .motorstop = false, - .yaw_reverse_direction = false + .yaw_reverse_direction = true }; /* Used in "mixerUAV" to create the dynamic model of the UAV */ diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index 3b7cdcd..c4af970 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -61,9 +61,7 @@ void init_system() //init sbus, using USART1 sbus_init(); - //init motors to run with oneshot 125, small delay - HAL_Delay(1000); - pwmEnableAllMotors(Oneshot125); + #ifdef USE_LEDS @@ -99,6 +97,10 @@ void init_system() #endif + //init motors to run with oneshot 125, small delay + HAL_Delay(1000); + pwmEnableAllMotors(Oneshot125); + } /************************************************************************** @@ -114,6 +116,9 @@ int main(void) //Init the system init_system(); + //Light the yellow led + ledOnInverted(Led1, Led1_GPIO_PORT); + //Initialize the scheduler, add all the tasks that should run to the ready queue of the scheduler initScheduler(); diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index 00b15d7..4392405 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -92,7 +92,10 @@ void systemTaskRx(void) // } /*Updated flag processRcChannel function, not yet tested. Should work as the entire loop above*/ - flags_ProcessRcChannel_Improved(STICK_CHANNEL_COUNT+1, STICK_CHANNEL_COUNT + AUX_CHANNEL_COUNT); + //flags_ProcessRcChannel_Improved(STICK_CHANNEL_COUNT+1, STICK_CHANNEL_COUNT + AUX_CHANNEL_COUNT); + + /* Includes the stick channel in the toggles checks */ + flags_ProcessRcChannel_Improved(1, STICK_CHANNEL_COUNT + AUX_CHANNEL_COUNT); //temporary send data from the RC directly form the RC // RawRcCommand.Roll = frame.chan1; @@ -162,24 +165,63 @@ bool systemTaskRxCliCheck(uint32_t currentDeltaTime) void systemTaskSerial(void) { -// uint8_t c = 118; -// usart_transmit(&cliUsart, &c, 1, 1000000000); - if (flags_IsSet_ID(systemFlags_armed_id)) - ledOnInverted(Led0_PIN, Led0_GPIO_PORT); - else - ledOffInverted(Led0_PIN, Led0_GPIO_PORT); + static bool readyToCalibrate = true; + const float calibrationAmount = 0.5; + + //Only run if the system is not armed + if (!flags_IsSet_ID(systemFlags_armed_id)) + { + if (flags_IsSet_ID(systemFlags_throttleMax_id)) + { +// if(flags_IsSet_ID(systemFlags_throttleLeft_id)) +// { +// if(flags_IsSet_ID(systemFlags_stickDown_id)) +// { +// mpu6000_read_acc_offset(&accelProfile); +// } +// } + if(readyToCalibrate) + { + if (flags_IsSet_ID(systemFlags_stickLeft_id)) + { + accRollFineTune -= calibrationAmount; + } + else if (flags_IsSet_ID(systemFlags_stickRight_id)) + { + accRollFineTune += calibrationAmount; + } + else if (flags_IsSet_ID(systemFlags_stickUp_id)) + { + accPitchFineTune -= calibrationAmount; + } + else if (flags_IsSet_ID(systemFlags_stickDown_id)) + { + accPitchFineTune += calibrationAmount; + } + + } + else + { + //if the stick is centered set ready to calibrate to true + if(flags_IsSet_MASK(systemFlags_stickCenterH_mask | systemFlags_stickCenterV_mask)) + { + readyToCalibrate = true; + } + } + + } + } } void systemTaskBattery(void) { - //Keep track of the battery level of the system -// uint8_t c = 98; -// usart_transmit(&cliUsart, &c, 1, 1000000000); - if (flags_IsSet_MASK((systemFlags_flightmode_acceleromter_mask | systemFlags_armed_mask))) - ledOnInverted(Led1, Led1_GPIO_PORT); + // uint8_t c = 118; + // usart_transmit(&cliUsart, &c, 1, 1000000000); + if (flags_IsSet_ID(systemFlags_armed_id)) + ledOnInverted(Led0_PIN, Led0_GPIO_PORT); else - ledOffInverted(Led1, Led1_GPIO_PORT); + ledOffInverted(Led0_PIN, Led0_GPIO_PORT); } void systemTaskBaro(void) From e040e35cf640a01802f2c8f5dd4c8b6eef3680e9 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Fri, 28 Oct 2016 14:34:59 +0200 Subject: [PATCH 15/25] ops --- UAV-ControlSystem/src/config/cli.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/UAV-ControlSystem/src/config/cli.c b/UAV-ControlSystem/src/config/cli.c index 80481cf..1d7cb04 100644 --- a/UAV-ControlSystem/src/config/cli.c +++ b/UAV-ControlSystem/src/config/cli.c @@ -1483,6 +1483,7 @@ void cliRun() if (ReceiveBinaryDecision(121,110)) { TransmitBack("Starting calibration! \n\n\r"); + mpu6000_read_acc_offset(&accelProfile); TransmitBack("Calibration complete! \n\n\r"); } else @@ -1509,8 +1510,10 @@ void cliRun() { char tempBuffer[100]; TransmitBack("- Accelerometer calibration information: \n\n\r"); + sprintf(tempBuffer, "- Finetune values: \n\r- Pitch: %0.2f \n\r- Roll: %0.2f \n\n\r", accPitchFineTune, accRollFineTune); TransmitBack(tempBuffer); sprintf(tempBuffer, "- BaseTune: \n\r-OffsetX: %d \n\r-OffsetY: %d \n\r-OffsetZ: %d \n\n\r", accelProfile.offsetX, accelProfile.offsetY, accelProfile.offsetZ); + TransmitBack(tempBuffer); break; } From dab5b5626d6515c922819eadfa1cf0dabca810bd Mon Sep 17 00:00:00 2001 From: johan9107 Date: Mon, 31 Oct 2016 09:58:04 +0100 Subject: [PATCH 16/25] PID new test changes, cast I term to float --- UAV-ControlSystem/src/Flight/pid.c | 27 +++++++++++++++++---------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index c595d16..219a66d 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -18,6 +18,8 @@ #include "scheduler/scheduler.h" #include #include "drivers/failsafe_toggles.h" +#include "drivers/motormix.h" + #define PTERM_SCALE 0.032029f /*P-term used as a scale value to the PID controller*/ #define ITERM_SCALE 0.244381f /*I-term used as a scale value to the PID controller*/ @@ -29,7 +31,7 @@ #define GYRO_RANGE 720 /*Determines the maximum rotational limit (limits the rc input)*/ #define COMPASS_RANGE 180 /*Determines the maximum compass limit (limits the rc input)*/ -#define PID_MAX_I 256 /*Constrains ITerm*/ +#define PID_MAX_I 256/2 /*Constrains ITerm*/ #define PID_MAX_D 512 /*Constrains DTerm*/ /*Struct that belongs to a certain PID controller*/ @@ -117,8 +119,8 @@ float constrainf(float amt, int low, int high) float oldSensorValue[2] = {0}; -float oldSensorValueRoll[50] = {0}; -float oldSensorValuePitch[50] = {0}; +float oldSensorValueRoll[20] = {0}; +float oldSensorValuePitch[20] = {0}; int i = 0; @@ -164,7 +166,7 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) float RollValue = 0; float PitchValue = 0; - for (int ii = 0; ii < 50; ii++) + for (int ii = 0; ii < 20; ii++) { RollValue = RollValue + oldSensorValueRoll[ii]; PitchValue = PitchValue + oldSensorValuePitch[ii]; @@ -172,13 +174,13 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) } - i = (i < 49)? i + 1:0; + i = (i < 19)? i + 1:0; - sensorValues[ROLL] = RollValue/50; - sensorValues[PITCH] = PitchValue/50; + sensorValues[ROLL] = RollValue/20; + sensorValues[PITCH] = PitchValue/20; - sensorValues[ROLL] = alpha*RollValue/50 + (1-alpha)*oldSensorValue[0]; - sensorValues[PITCH] = alpha*PitchValue/50 + (1-alpha)*oldSensorValue[1]; + sensorValues[ROLL] = alpha*RollValue/20 + (1-alpha)*oldSensorValue[0]; + sensorValues[PITCH] = alpha*PitchValue/20 + (1-alpha)*oldSensorValue[1]; // oldSensorValue[0] = sensorValues[ROLL]; oldSensorValue[1] = sensorValues[PITCH]; @@ -291,7 +293,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, /* -----calculate I component ---- */ - float ITerm = pidProfileBuff->lastITerm[axis] + ITERM_SCALE * rateError * pidProfileBuff->dT * pidProfile->I[axis]; + float ITerm = pidProfileBuff->lastITerm[axis] + ITERM_SCALE * rateError * pidProfileBuff->dT * (float)pidProfile->I[axis]; // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I) moved before integration to make limiting independent from PID settings @@ -341,6 +343,11 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, /*----PID OUT----*/ + if(!flags_IsSet_ID(systemFlags_armed_id) || (rc_input.Throttle < mixerConfig.minCheck)) + { + ITerm = 0; + pidProfileBuff->lastITerm[axis] = 0; + } pidProfile->PID_Out[axis] = constrainf(PTerm + ITerm + DTerm, -pidProfile->pid_out_limit, pidProfile->pid_out_limit); } From e6ce36f9e946d681b9662f4be366d00e130a5fc4 Mon Sep 17 00:00:00 2001 From: philsson Date: Mon, 31 Oct 2016 11:18:35 +0100 Subject: [PATCH 17/25] Changes to scaling of the I-term. It is now 200 times smaller --- UAV-ControlSystem/src/Flight/pid.c | 4 ++-- UAV-ControlSystem/src/main.c | 8 +++++--- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 219a66d..0939bce 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -22,7 +22,7 @@ #define PTERM_SCALE 0.032029f /*P-term used as a scale value to the PID controller*/ -#define ITERM_SCALE 0.244381f /*I-term used as a scale value to the PID controller*/ +#define ITERM_SCALE 0.0012f /*I-term used as a scale value to the PID controller*/ #define DTERM_SCALE 0.000529f /*D-term used as a scale value to the PID controller*/ #define RADIO_RANGE 500 /*Radio range input*/ @@ -31,7 +31,7 @@ #define GYRO_RANGE 720 /*Determines the maximum rotational limit (limits the rc input)*/ #define COMPASS_RANGE 180 /*Determines the maximum compass limit (limits the rc input)*/ -#define PID_MAX_I 256/2 /*Constrains ITerm*/ +#define PID_MAX_I 256 /*Constrains ITerm*/ #define PID_MAX_D 512 /*Constrains DTerm*/ /*Struct that belongs to a certain PID controller*/ diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index c4af970..2c9dfaf 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -47,6 +47,10 @@ void init_system() //Configure the clock system_clock_config(); + //init motors to run with oneshot 125, small delay + HAL_Delay(1000); + pwmEnableAllMotors(Oneshot125); + //Initializes all the pids that are used in the system. This part will also init the gyro and accelerometer. pidInit(); @@ -97,9 +101,7 @@ void init_system() #endif - //init motors to run with oneshot 125, small delay - HAL_Delay(1000); - pwmEnableAllMotors(Oneshot125); + } From f787b195726932005409a8b209cd66234d0857f9 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Mon, 31 Oct 2016 11:20:16 +0100 Subject: [PATCH 18/25] PID, deleted overflow code --- UAV-ControlSystem/src/Flight/pid.c | 1 - 1 file changed, 1 deletion(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 0939bce..31d130e 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -69,7 +69,6 @@ float accPitchFineTune = 0; float calcAngle(const uint8_t axis, const float x_axis, const float y_axis, const float z_axis) { float angle; - float angle_offset = (z_axis < 0 )? 90: 0; switch (axis) { From dcf36fc542ac0677e9dbf6e692d778e41881eeaf Mon Sep 17 00:00:00 2001 From: johan9107 Date: Mon, 31 Oct 2016 14:44:34 +0100 Subject: [PATCH 19/25] PID changed buffer to filter --- UAV-ControlSystem/src/Flight/pid.c | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 31d130e..e538621 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -27,7 +27,7 @@ #define RADIO_RANGE 500 /*Radio range input*/ #define BAROMETER_RANGE 2000 /*Determines the range of the maximum height (limits the rc input)*/ -#define ACCELEROMETER_RANGE 35 /*Determines the range of the maximum angle (limits the rc input) & (Accelerometer takes int to max 16 G)*/ +#define ACCELEROMETER_RANGE 60 /*Determines the range of the maximum angle (limits the rc input) & (Accelerometer takes int to max 16 G)*/ #define GYRO_RANGE 720 /*Determines the maximum rotational limit (limits the rc input)*/ #define COMPASS_RANGE 180 /*Determines the maximum compass limit (limits the rc input)*/ @@ -92,6 +92,11 @@ float calcAngle(const uint8_t axis, const float x_axis, const float y_axis, cons return angle; } +float calcGravity(accel_t profile ) //const float x_axis, const float y_axis, const float z_axis) +{ + return sqrt(profile.accelXconv*profile.accelXconv + profile.accelYconv*profile.accelYconv + profile.accelZconv*profile.accelZconv); +} + /************************************************************************** * BRIEF: Scales data from input range to output range * * INFORMATION: * @@ -118,8 +123,8 @@ float constrainf(float amt, int low, int high) float oldSensorValue[2] = {0}; -float oldSensorValueRoll[20] = {0}; -float oldSensorValuePitch[20] = {0}; +float oldSensorValueRoll[12] = {0}; +float oldSensorValuePitch[12] = {0}; int i = 0; @@ -147,7 +152,6 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) mpu6000_read_accel(&accelProfile); /*Reads data from accelerometer*/ - float alpha = 0.5; /*May need Low pass filter since the accelerometer may drift*/ @@ -165,7 +169,7 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) float RollValue = 0; float PitchValue = 0; - for (int ii = 0; ii < 20; ii++) + for (int ii = 0; ii < 12; ii++) { RollValue = RollValue + oldSensorValueRoll[ii]; PitchValue = PitchValue + oldSensorValuePitch[ii]; @@ -173,13 +177,13 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) } - i = (i < 19)? i + 1:0; + i = (i < 11)? i + 1:0; - sensorValues[ROLL] = RollValue/20; - sensorValues[PITCH] = PitchValue/20; + sensorValues[ROLL] = RollValue/12; + sensorValues[PITCH] = PitchValue/12; - sensorValues[ROLL] = alpha*RollValue/20 + (1-alpha)*oldSensorValue[0]; - sensorValues[PITCH] = alpha*PitchValue/20 + (1-alpha)*oldSensorValue[1]; + sensorValues[ROLL] = alpha*RollValue/12 + (1-alpha)*oldSensorValue[0]; + sensorValues[PITCH] = alpha*PitchValue/12 + (1-alpha)*oldSensorValue[1]; // oldSensorValue[0] = sensorValues[ROLL]; oldSensorValue[1] = sensorValues[PITCH]; From 3039f3a3948ea69e0ff558ba0f617b928161316b Mon Sep 17 00:00:00 2001 From: johan9107 Date: Mon, 31 Oct 2016 18:17:13 +0100 Subject: [PATCH 20/25] pid SHIT FIX --- UAV-ControlSystem/src/Flight/pid.c | 28 ++++++++++++++++++++++++---- UAV-ControlSystem/src/main.c | 2 +- 2 files changed, 25 insertions(+), 5 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index e538621..018181f 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -151,6 +151,17 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) mpu6000_read_accel(&accelProfile); /*Reads data from accelerometer*/ +// +// if (calcGravity(accelProfile) > 1.15) +// { +// +// sensorValues[ROLL] = gyroProfile.gyroY*PidProfileBuff[ROLL].dT; +// sensorValues[PITCH] = gyroProfile.gyroX*PidProfileBuff[PITCH].dT; +// +// } +// else +// { + float alpha = 0.5; /*May need Low pass filter since the accelerometer may drift*/ @@ -188,6 +199,8 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) oldSensorValue[0] = sensorValues[ROLL]; oldSensorValue[1] = sensorValues[PITCH]; + + // sensorValues[ROLL] = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); // sensorValues[PITCH] = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); @@ -456,23 +469,23 @@ void pidUAVInitBuff(pidProfileBuff_t *pidProfile, uint8_t ID) case PID_ID_GYRO: PidProfileBuff[ID].DOF = 3; - PidProfileBuff[ID].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000; //ÄNDRA TILL SEKUNDER inte ms + PidProfileBuff[ID].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000000; //ÄNDRA TILL SEKUNDER inte ms break; case PID_ID_ACCELEROMETER: PidProfileBuff[ID].DOF = 2; - PidProfileBuff[ID].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000; + PidProfileBuff[ID].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000000; break; case PID_ID_COMPASS: - PidProfileBuff[ID].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000; + PidProfileBuff[ID].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000000; break; case PID_ID_BAROMETER: - PidProfileBuff[ID].dT = SystemTasks[TASK_BARO].desiredPeriod/1000; + PidProfileBuff[ID].dT = SystemTasks[TASK_BARO].desiredPeriod/1000000; break; default: @@ -618,4 +631,11 @@ void pidEproom(void) PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 2; PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100; + PidProfileBuff[PID_ID_GYRO].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000000; //ÄNDRA TILL SEKUNDER inte ms + PidProfileBuff[PID_ID_ACCELEROMETER].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000000; + PidProfileBuff[PID_ID_COMPASS].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000000; + PidProfileBuff[PID_ID_BAROMETER].dT = SystemTasks[TASK_BARO].desiredPeriod/1000000; + + + } diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index 2c9dfaf..f678bab 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -60,7 +60,7 @@ void init_system() pidEproom(); //initialize the CLI NOTE: Cant use the same usart as anything else or there will be some big trouble - cliInit(USART3); + cliInit(USART6); //init sbus, using USART1 sbus_init(); From 4b7fd40967e862ddee0e571234b849d1ce2320e2 Mon Sep 17 00:00:00 2001 From: mdhsweden Date: Mon, 31 Oct 2016 18:30:09 +0100 Subject: [PATCH 21/25] Cast Ints to float in constrainf. What happens if we return an int on a function which returns a float? Just to be sure --- UAV-ControlSystem/src/Flight/pid.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 018181f..b7fee19 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -1,6 +1,6 @@ /************************************************************************** * NAME: pid.c * -* AUTHOR: Johan Gärtner * +* AUTHOR: Johan G�rtner * * * PURPOSE: This file contains pid functions * * INFORMATION: pidUAV is the final pid controller which only takes * @@ -114,9 +114,9 @@ float convertData(int inputRange, int outputRange, int offset, float value) float constrainf(float amt, int low, int high) { if (amt < low) - return low; + return (float)low; else if (amt > high) - return high; + return (float)high; else return amt; } @@ -469,7 +469,7 @@ void pidUAVInitBuff(pidProfileBuff_t *pidProfile, uint8_t ID) case PID_ID_GYRO: PidProfileBuff[ID].DOF = 3; - PidProfileBuff[ID].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000000; //ÄNDRA TILL SEKUNDER inte ms + PidProfileBuff[ID].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000000; //�NDRA TILL SEKUNDER inte ms break; case PID_ID_ACCELEROMETER: @@ -631,7 +631,7 @@ void pidEproom(void) PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 2; PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100; - PidProfileBuff[PID_ID_GYRO].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000000; //ÄNDRA TILL SEKUNDER inte ms + PidProfileBuff[PID_ID_GYRO].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000000; //�NDRA TILL SEKUNDER inte ms PidProfileBuff[PID_ID_ACCELEROMETER].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000000; PidProfileBuff[PID_ID_COMPASS].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000000; PidProfileBuff[PID_ID_BAROMETER].dT = SystemTasks[TASK_BARO].desiredPeriod/1000000; From 40454ae608a73e766c787cf5af8404b5d488891b Mon Sep 17 00:00:00 2001 From: mdhsweden Date: Mon, 31 Oct 2016 19:07:45 +0100 Subject: [PATCH 22/25] Casts added to several functions in PID section Signed-off-by: mdhsweden --- UAV-ControlSystem/src/Flight/pid.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index b7fee19..24a8875 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -104,7 +104,6 @@ float calcGravity(accel_t profile ) //const float x_axis, const float y_axis, co float convertData(int inputRange, int outputRange, int offset, float value) { return ((float)outputRange/(float)inputRange)*(value-(float)offset); - //return 1.0; } /************************************************************************** @@ -113,9 +112,9 @@ float convertData(int inputRange, int outputRange, int offset, float value) **************************************************************************/ float constrainf(float amt, int low, int high) { - if (amt < low) + if (amt < (float)low) return (float)low; - else if (amt > high) + else if (amt > (float)high) return (float)high; else return amt; @@ -313,7 +312,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I) moved before integration to make limiting independent from PID settings - ITerm = constrainf(ITerm, -PID_MAX_I, PID_MAX_I); + ITerm = constrainf(ITerm, -(int)PID_MAX_I, (int)PID_MAX_I); // Anti windup protection if (motorLimitReached) @@ -363,8 +362,9 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, { ITerm = 0; pidProfileBuff->lastITerm[axis] = 0; + pidProfileBuff->ITermLimit[axis] = 0; } - pidProfile->PID_Out[axis] = constrainf(PTerm + ITerm + DTerm, -pidProfile->pid_out_limit, pidProfile->pid_out_limit); + pidProfile->PID_Out[axis] = constrainf(PTerm + ITerm + DTerm, -(int)pidProfile->pid_out_limit, (int)pidProfile->pid_out_limit); } /************************************************************************** From c6d50810cb1c5380546f331afed29cb0e2134960 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Tue, 1 Nov 2016 10:03:35 +0100 Subject: [PATCH 23/25] PID take away antiwindup protection --- UAV-ControlSystem/src/Flight/pid.c | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 24a8875..97a78f8 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -22,7 +22,7 @@ #define PTERM_SCALE 0.032029f /*P-term used as a scale value to the PID controller*/ -#define ITERM_SCALE 0.0012f /*I-term used as a scale value to the PID controller*/ +#define ITERM_SCALE 0.0012f /*I-term used as a scale value to the PID controller*/ #define DTERM_SCALE 0.000529f /*D-term used as a scale value to the PID controller*/ #define RADIO_RANGE 500 /*Radio range input*/ @@ -31,7 +31,7 @@ #define GYRO_RANGE 720 /*Determines the maximum rotational limit (limits the rc input)*/ #define COMPASS_RANGE 180 /*Determines the maximum compass limit (limits the rc input)*/ -#define PID_MAX_I 256 /*Constrains ITerm*/ +#define PID_MAX_I 256 /*Constrains ITerm*/ #define PID_MAX_D 512 /*Constrains DTerm*/ /*Struct that belongs to a certain PID controller*/ @@ -315,14 +315,14 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, ITerm = constrainf(ITerm, -(int)PID_MAX_I, (int)PID_MAX_I); // Anti windup protection - if (motorLimitReached) - { - ITerm = constrainf(ITerm, -pidProfileBuff->ITermLimit[axis], pidProfileBuff->ITermLimit[axis]); - } - else - { - pidProfileBuff->ITermLimit[axis] = abs(ITerm); - } +// if (motorLimitReached) +// { +// ITerm = constrainf(ITerm, -pidProfileBuff->ITermLimit[axis], pidProfileBuff->ITermLimit[axis]); +// } +// else +// { +// pidProfileBuff->ITermLimit[axis] = abs(ITerm); +// } pidProfileBuff->lastITerm[axis] = ITerm; @@ -631,10 +631,10 @@ void pidEproom(void) PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 2; PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100; - PidProfileBuff[PID_ID_GYRO].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000000; //�NDRA TILL SEKUNDER inte ms - PidProfileBuff[PID_ID_ACCELEROMETER].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000000; - PidProfileBuff[PID_ID_COMPASS].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000000; - PidProfileBuff[PID_ID_BAROMETER].dT = SystemTasks[TASK_BARO].desiredPeriod/1000000; + PidProfileBuff[PID_ID_GYRO].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000; //�NDRA TILL SEKUNDER inte ms + PidProfileBuff[PID_ID_ACCELEROMETER].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000; + PidProfileBuff[PID_ID_COMPASS].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000; + PidProfileBuff[PID_ID_BAROMETER].dT = SystemTasks[TASK_BARO].desiredPeriod/1000; From 4accab066097f5036795088bff24f509e49584c4 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Tue, 1 Nov 2016 12:18:50 +0100 Subject: [PATCH 24/25] Fixed the problem with windup --- UAV-ControlSystem/inc/Flight/pid.h | 3 +-- UAV-ControlSystem/inc/drivers/motormix.h | 3 +++ UAV-ControlSystem/src/Flight/pid.c | 11 ++++++++++- UAV-ControlSystem/src/drivers/motormix.c | 5 +++-- UAV-ControlSystem/src/main.c | 2 +- 5 files changed, 18 insertions(+), 6 deletions(-) diff --git a/UAV-ControlSystem/inc/Flight/pid.h b/UAV-ControlSystem/inc/Flight/pid.h index 39728c7..7213f7d 100644 --- a/UAV-ControlSystem/inc/Flight/pid.h +++ b/UAV-ControlSystem/inc/Flight/pid.h @@ -67,8 +67,7 @@ extern float accPitchFineTune; extern accel_t accelProfile; /*Struct profile for input data from sensor*/ -/*Is set in motor mix and used in pidUAVcore and mix */ -bool motorLimitReached; + /************************************************************************** * BRIEF: Initializes PID profiles * diff --git a/UAV-ControlSystem/inc/drivers/motormix.h b/UAV-ControlSystem/inc/drivers/motormix.h index 85522b9..fc999e7 100644 --- a/UAV-ControlSystem/inc/drivers/motormix.h +++ b/UAV-ControlSystem/inc/drivers/motormix.h @@ -46,6 +46,9 @@ typedef struct { /* Global mixerConfig to bee available to EEPROM */ extern mixerConfig_s mixerConfig; +/*Is set in motor mix and used in pidUAVcore and mix */ +extern bool motorLimitReached; + /*********************************************************************** * BRIEF: The motormixer * * INFORMATION: Sums the output from all control loops and adapts the * diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 97a78f8..87370fd 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -62,6 +62,8 @@ pt1Filter_t accelFilter[2] = {0}; float accRollFineTune = 0; float accPitchFineTune = 0; + + /************************************************************************** * BRIEF: Calculates angle from accelerometer * * INFORMATION: * @@ -324,6 +326,12 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, // pidProfileBuff->ITermLimit[axis] = abs(ITerm); // } + if (motorLimitReached) + { + ITerm = pidProfileBuff->lastITerm[axis]; + } + + pidProfileBuff->lastITerm[axis] = ITerm; @@ -611,7 +619,6 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID) void pidInit() { mpu6000_init(&gyroProfile,&accelProfile); /*Init gyro and accelerometer*/ - motorLimitReached = false; pidUAVInitBuff(&PidProfileBuff[PID_ID_GYRO], PID_ID_GYRO); pidUAVInitBuff(&PidProfileBuff[PID_ID_ACCELEROMETER], PID_ID_ACCELEROMETER); @@ -636,6 +643,8 @@ void pidEproom(void) PidProfileBuff[PID_ID_COMPASS].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000; PidProfileBuff[PID_ID_BAROMETER].dT = SystemTasks[TASK_BARO].desiredPeriod/1000; + PidProfile[PID_ID_GYRO].I[YAW] = 40; + } diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index 1261837..0def06d 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -42,6 +42,9 @@ /* An array containing the calculated motor outputs */ uint16_t motor_output[MOTOR_COUNT]; +/* Bool to see if motors are maxed out. Stops windup in PID implementation */ +bool motorLimitReached = false; + /* Default values for the mixerConfig */ // TODO: Implement in EEPROM mixerConfig_s mixerConfig = { @@ -102,8 +105,6 @@ void mix() int16_t RPY_Mix_Max = 0; // Maximum desired command for any motor int16_t throttle = PidProfile[PID_ID_BAROMETER].PID_Out[THROTTLE]; // Import throttle value from remote - // Might be used for some debug if necessary - //static bool motorLimitReached; /* Mixer Full Scale enabled */ if (flags_IsSet_ID(systemFlags_mixerfullscale_id)) diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index f678bab..2c9dfaf 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -60,7 +60,7 @@ void init_system() pidEproom(); //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(); From fbae0cf4528eadf5ee820e63792a321ed6f634c8 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Tue, 1 Nov 2016 14:05:52 +0100 Subject: [PATCH 25/25] Fixed abs function for float values This could cause the anti windup to work. Needs testing. --- UAV-ControlSystem/inc/utilities.h | 4 ++++ UAV-ControlSystem/src/Flight/pid.c | 27 +++++++++++++-------------- UAV-ControlSystem/src/main.c | 1 + 3 files changed, 18 insertions(+), 14 deletions(-) diff --git a/UAV-ControlSystem/inc/utilities.h b/UAV-ControlSystem/inc/utilities.h index 446ddea..b8fa784 100644 --- a/UAV-ControlSystem/inc/utilities.h +++ b/UAV-ControlSystem/inc/utilities.h @@ -25,6 +25,8 @@ #define maxStringSize_CLI 100 //Max sting size used for the messages in the CLI +#define ABS_FLOAT(x) (((x) < 0)? -(x): (x)) + typedef char typeString[maxStringSize_CLI]; typedef struct typeStringArr { char val[maxStringSize_CLI]; } typeStringArr; @@ -81,6 +83,8 @@ uint32_t accumulate(uint32_t list[], int length); ***********************************************************************/ void Error_Handler(void); + + uint8_t reverse(uint8_t byte); int16_t constrain(int16_t value, int16_t min, int16_t max); diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 87370fd..c16c8ec 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -19,6 +19,7 @@ #include #include "drivers/failsafe_toggles.h" #include "drivers/motormix.h" +#include "utilities.h" #define PTERM_SCALE 0.032029f /*P-term used as a scale value to the PID controller*/ @@ -317,19 +318,19 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, ITerm = constrainf(ITerm, -(int)PID_MAX_I, (int)PID_MAX_I); // Anti windup protection -// if (motorLimitReached) -// { -// ITerm = constrainf(ITerm, -pidProfileBuff->ITermLimit[axis], pidProfileBuff->ITermLimit[axis]); -// } -// else -// { -// pidProfileBuff->ITermLimit[axis] = abs(ITerm); -// } + if (motorLimitReached) + { + ITerm = constrainf(ITerm, -pidProfileBuff->ITermLimit[axis], pidProfileBuff->ITermLimit[axis]); + } + else + { + pidProfileBuff->ITermLimit[axis] = ABS_FLOAT(ITerm); + } - if (motorLimitReached) - { - ITerm = pidProfileBuff->lastITerm[axis]; - } +// if (motorLimitReached) +// { +// ITerm = pidProfileBuff->lastITerm[axis]; +// } pidProfileBuff->lastITerm[axis] = ITerm; @@ -645,6 +646,4 @@ void pidEproom(void) PidProfile[PID_ID_GYRO].I[YAW] = 40; - - } diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index 2c9dfaf..bdefb67 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -124,6 +124,7 @@ 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