From 404a4e48963e4570d92c14971f6d5cfdec0f0a07 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 7 Mar 2012 15:14:29 +1100 Subject: [PATCH] ADC: use floats for ADC averaging this costs almost nothing and improved accel/gyro calibration --- libraries/AP_ADC/AP_ADC.h | 2 +- libraries/AP_ADC/AP_ADC_ADS7844.cpp | 4 ++-- libraries/AP_ADC/AP_ADC_ADS7844.h | 2 +- libraries/AP_ADC/AP_ADC_HIL.cpp | 2 +- libraries/AP_ADC/AP_ADC_HIL.h | 2 +- 5 files changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/AP_ADC/AP_ADC.h b/libraries/AP_ADC/AP_ADC.h index 5e6b2d0909..e1b24e4c24 100644 --- a/libraries/AP_ADC/AP_ADC.h +++ b/libraries/AP_ADC/AP_ADC.h @@ -38,7 +38,7 @@ class AP_ADC The function returns the amount of time (in microseconds) since the last call to Ch6(). */ - virtual uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result) = 0; + virtual uint32_t Ch6(const uint8_t *channel_numbers, float *result) = 0; // check if Ch6() can return new data virtual bool new_data_available(const uint8_t *channel_numbers) = 0; diff --git a/libraries/AP_ADC/AP_ADC_ADS7844.cpp b/libraries/AP_ADC/AP_ADC_ADS7844.cpp index f216f5a895..761d237516 100644 --- a/libraries/AP_ADC/AP_ADC_ADS7844.cpp +++ b/libraries/AP_ADC/AP_ADC_ADS7844.cpp @@ -204,7 +204,7 @@ bool AP_ADC_ADS7844::new_data_available(const uint8_t *channel_numbers) // equal. This will only be true if we always consistently access a // sensor by either Ch6() or Ch() and never mix them. If you mix them // then you will get very strange results -uint32_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result) +uint32_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, float *result) { uint16_t count[6]; uint32_t sum[6]; @@ -230,7 +230,7 @@ uint32_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result) // division. That costs us 36 bytes of stack, but I think its // worth it. for (i = 0; i < 6; i++) { - result[i] = (sum[i] + (count[i]/2)) / count[i]; + result[i] = (sum[i] + count[i]) / (float)count[i]; } diff --git a/libraries/AP_ADC/AP_ADC_ADS7844.h b/libraries/AP_ADC/AP_ADC_ADS7844.h index 73be670b3b..36f76f5770 100644 --- a/libraries/AP_ADC/AP_ADC_ADS7844.h +++ b/libraries/AP_ADC/AP_ADC_ADS7844.h @@ -28,7 +28,7 @@ class AP_ADC_ADS7844 : public AP_ADC float Ch(unsigned char ch_num); // Read 6 sensors at once - uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result); + uint32_t Ch6(const uint8_t *channel_numbers, float *result); // check if Ch6 would block bool new_data_available(const uint8_t *channel_numbers); diff --git a/libraries/AP_ADC/AP_ADC_HIL.cpp b/libraries/AP_ADC/AP_ADC_HIL.cpp index e443941642..24587de2f4 100644 --- a/libraries/AP_ADC/AP_ADC_HIL.cpp +++ b/libraries/AP_ADC/AP_ADC_HIL.cpp @@ -54,7 +54,7 @@ float AP_ADC_HIL::Ch(unsigned char ch_num) } // Read 6 channel values -uint32_t AP_ADC_HIL::Ch6(const uint8_t *channel_numbers, uint16_t *result) +uint32_t AP_ADC_HIL::Ch6(const uint8_t *channel_numbers, float *result) { for (uint8_t i=0; i<6; i++) { result[i] = Ch(channel_numbers[i]); diff --git a/libraries/AP_ADC/AP_ADC_HIL.h b/libraries/AP_ADC/AP_ADC_HIL.h index d9c53f6fdb..1f129f3378 100644 --- a/libraries/AP_ADC/AP_ADC_HIL.h +++ b/libraries/AP_ADC/AP_ADC_HIL.h @@ -36,7 +36,7 @@ class AP_ADC_HIL : public AP_ADC /// // Read 6 sensors at once - uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result); + uint32_t Ch6(const uint8_t *channel_numbers, float *result); // see if Ch6 would block bool new_data_available(const uint8_t *channel_numbers);