From 440640d8ef5fac5c465577ca459178eca8d41ef3 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Wed, 9 Nov 2016 10:35:55 +0100 Subject: [PATCH] Added average function to get the altitude --- UAV-ControlSystem/inc/drivers/barometer.h | 7 +++ UAV-ControlSystem/src/drivers/barometer.c | 54 ++++++++++++++++++++++- UAV-ControlSystem/src/main.c | 4 +- UAV-ControlSystem/src/tasks_main.c | 6 +-- 4 files changed, 64 insertions(+), 7 deletions(-) diff --git a/UAV-ControlSystem/inc/drivers/barometer.h b/UAV-ControlSystem/inc/drivers/barometer.h index 7ce3b75..a22b488 100644 --- a/UAV-ControlSystem/inc/drivers/barometer.h +++ b/UAV-ControlSystem/inc/drivers/barometer.h @@ -103,4 +103,11 @@ double barometer_GetCurrentTemperature(); ***********************************************************************/ float barometer_GetCurrentAltitude(); +/*********************************************************************** + * BRIEF: Gets the altitude based on the last number of values. * + * INFORMATION: Averages the value on the last few reading to get a more* + * accurate reading. * + ***********************************************************************/ +float barometer_GetCurrentAveragedtAltitude(); + #endif /* DRIVERS_BAROMETER_H_ */ diff --git a/UAV-ControlSystem/src/drivers/barometer.c b/UAV-ControlSystem/src/drivers/barometer.c index 798d71a..3074445 100644 --- a/UAV-ControlSystem/src/drivers/barometer.c +++ b/UAV-ControlSystem/src/drivers/barometer.c @@ -49,7 +49,8 @@ #define SEA_PRESS 1013.25 //default sea level pressure level in mb #define FTMETERS 0.3048 //convert feet to meters -#define CALIBRATION_VAL_AMOUNT 30 +#define CALIBRATION_VAL_AMOUNT 40 +#define NUMB_AVERAGE_VALS 10 I2C_HandleTypeDef baroI2C_handle; //Handle for the HW i2c (NOT USED ATM) DMA_HandleTypeDef baroI2C_Rx_DMA_handle; //Dma handle receive (NOT USED ATM) @@ -62,17 +63,46 @@ double baro_Temperature; // compensated temperature value ( double baro_Altitude; // altitude double baro_S; // sea level barometer (mB) +/* Calibration variables */ 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; //Counter for the amount of calibration samples int calibrationSamplesIterator = 0; //Iterator for when going through all the samples +/* average altitude variables */ +float averageAltitude[NUMB_AVERAGE_VALS]; +uint8_t averageAltitudeIterator = 0; +uint8_t averageAltitudeCount = 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 uint8_t cobuf[3] = {0}; //Array used when writing and reading data over the I2C +/*********************************************************************** + * BRIEF: Adds a new altitude value to the average buffer vals * + * INFORMATION: Will add the last calculated altitude value to the * + * buffer used to provide a average calc of altitude * + ***********************************************************************/ +void barometer_addAverageAltitudeSample() +{ + //fisrt check if the amount of samples is greater than the array + if (!(averageAltitudeCount >= NUMB_AVERAGE_VALS)) + averageAltitudeCount++; //if not increase the counter + + //Check if the iterator should restart from the beginning because of overflow + if (averageAltitudeIterator >= NUMB_AVERAGE_VALS) + averageAltitudeIterator = 0; //if it is set it to zero + + //Add the lates calculated altitude value to the samples + averageAltitude[averageAltitudeIterator] = baro_Altitude; + + //increase the iterator + averageAltitudeIterator ++; +} + /*********************************************************************** * BRIEF: Adds a new altitude value to the calibration samples. * * INFORMATION: Will add the last calculated altitude value to the * @@ -283,8 +313,10 @@ void barometer_CalculatePTA(uint32_t D1, uint32_t D2) /* 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); + + /* Add altitude values to altitude buffer containing the last few readings */ + barometer_addAverageAltitudeSample(); } /*********************************************************************** @@ -465,4 +497,22 @@ float barometer_GetCurrentAltitude() return baro_Altitude; } +/*********************************************************************** + * BRIEF: Gets the altitude based on the last number of values. * + * INFORMATION: Averages the value on the last few reading to get a more* + * accurate reading. * + ***********************************************************************/ +float barometer_GetCurrentAveragedtAltitude() +{ + float toReturn = 0; + /* Go through all the values in the buffer */ + for (int i = 0; i < averageAltitudeCount; i++) + { + toReturn += averageAltitude[i]; + } + + /* Return the average of the stored values */ + return toReturn/averageAltitudeCount; +} + diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index 2c9dfaf..4441435 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -77,8 +77,8 @@ void init_system() #endif #ifdef BARO - //barometer_init(); - //barometer_reset(); + barometer_init(); + barometer_reset(); #endif #ifdef COMPASS diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index 624ded6..8f199d9 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -112,7 +112,6 @@ void systemTaskRx(void) continuousMissedFrames = (frame.flag_FrameLost) ? continuousMissedFrames + 1 : 0; (continuousMissedFrames > 10) ? flags_Set_ID(systemFlags_Failsafe_toManyMissedFrames_id) : flags_Clear_ID(systemFlags_Failsafe_toManyMissedFrames_id); - } bool systemTaskRxCheck(uint32_t currentDeltaTime) @@ -234,7 +233,7 @@ void systemTaskBattery(void) void systemTaskBaro(void) { - //barometer_CaclulateValues(); + barometer_CaclulateValues(); } void systemTaskCompass(void) @@ -258,7 +257,8 @@ void systemTaskAltitude(void) //double temperature = barometer_GetCurrentTemperature(); //double pressure = barometer_GetCurrentPreassure(); - //float altitute = barometer_GetCurrentAltitudeBasedOnSeaLevel(); + //float altitute = barometer_GetCurrentAltitude(); + float altitute = barometer_GetCurrentAveragedtAltitude(); //pid run, should probably be moved to systemTaskAltitude pidRun(PID_ID_BAROMETER);