Added average function to get the altitude

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

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;
+}
+
 

UAV-ControlSystem/src/main.c

@@ -77,8 +77,8 @@ void init_system()
 #endif
 
 #ifdef BARO
-	//barometer_init();
+	barometer_init();
-	//barometer_reset();
+	barometer_reset();
 #endif
 
 #ifdef COMPASS

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);