Browse Source

ADC: use floats for ADC averaging

this costs almost nothing and improved accel/gyro calibration
mission-4.1.18
Andrew Tridgell 13 years ago
parent
commit
a0a360f955
  1. 2
      libraries/AP_ADC/AP_ADC.h
  2. 4
      libraries/AP_ADC/AP_ADC_ADS7844.cpp
  3. 2
      libraries/AP_ADC/AP_ADC_ADS7844.h
  4. 2
      libraries/AP_ADC/AP_ADC_HIL.cpp
  5. 2
      libraries/AP_ADC/AP_ADC_HIL.h

2
libraries/AP_ADC/AP_ADC.h

@ -38,7 +38,7 @@ class AP_ADC @@ -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;

4
libraries/AP_ADC/AP_ADC_ADS7844.cpp

@ -204,7 +204,7 @@ bool AP_ADC_ADS7844::new_data_available(const uint8_t *channel_numbers) @@ -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) @@ -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];
}

2
libraries/AP_ADC/AP_ADC_ADS7844.h

@ -28,7 +28,7 @@ class AP_ADC_ADS7844 : public AP_ADC @@ -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);

2
libraries/AP_ADC/AP_ADC_HIL.cpp

@ -54,7 +54,7 @@ float AP_ADC_HIL::Ch(unsigned char ch_num) @@ -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]);

2
libraries/AP_ADC/AP_ADC_HIL.h

@ -36,7 +36,7 @@ class AP_ADC_HIL : public AP_ADC @@ -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);

Loading…
Cancel
Save