From e791cad9eff87e1c2e9b1ff27be6bae93d43f6ec Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 29 Jun 2012 21:51:05 +1000 Subject: [PATCH] AnalogSource: convert analogRead() calls to the new API this should give us much better analog values, plus save a bunch of CPU time --- APMrover2/sensors.pde | 9 ++++++--- APMrover2/system.pde | 28 ++-------------------------- ArduCopter/sensors.pde | 9 ++++++--- ArduCopter/system.pde | 31 +++++-------------------------- ArduPlane/sensors.pde | 9 ++++++--- ArduPlane/system.pde | 34 ++++++++-------------------------- 6 files changed, 33 insertions(+), 87 deletions(-) diff --git a/APMrover2/sensors.pde b/APMrover2/sensors.pde index e1fc03e859..1b4477a819 100644 --- a/APMrover2/sensors.pde +++ b/APMrover2/sensors.pde @@ -105,10 +105,13 @@ static void read_battery(void) return; } - if(g.battery_monitoring == 3 || g.battery_monitoring == 4) - battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN_1)) * .1 + battery_voltage1 * .9; + if(g.battery_monitoring == 3 || g.battery_monitoring == 4) { + static AP_AnalogSource_Arduino bat_pin(BATTERY_PIN_1); + battery_voltage1 = BATTERY_VOLTAGE(bat_pin.read_average()); + } if(g.battery_monitoring == 4) { - current_amps1 = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps1 * .9; //reads power sensor current pin + static AP_AnalogSource_Arduino current_pin(CURRENT_PIN_1); + current_amps1 = CURRENT_AMPS(current_pin.read_average()); current_total1 += current_amps1 * (float)delta_ms_medium_loop * 0.0002778; // .0002778 is 1/3600 (conversion to hours) } diff --git a/APMrover2/system.pde b/APMrover2/system.pde index fb6e98ef46..05d878d5e3 100644 --- a/APMrover2/system.pde +++ b/APMrover2/system.pde @@ -602,34 +602,10 @@ void flash_leds(bool on) #ifndef DESKTOP_BUILD /* * Read Vcc vs 1.1v internal reference - * - * This call takes about 150us total. ADC conversion is 13 cycles of - * 125khz default changes the mux if it isn't set, and return last - * reading (allows necessary settle time) otherwise trigger the - * conversion */ uint16_t board_voltage(void) { - const uint8_t mux = (_BV(REFS0)|_BV(MUX4)|_BV(MUX3)|_BV(MUX2)|_BV(MUX1)); - - if (ADMUX == mux) { - ADCSRA |= _BV(ADSC); // Convert - uint16_t counter=4000; // normally takes about 1700 loops - while (bit_is_set(ADCSRA, ADSC) && counter) // Wait - counter--; - if (counter == 0) { - // we don't actually expect this timeout to happen, - // but we don't want any more code that could hang. We - // report 0V so it is clear in the logs that we don't know - // the value - return 0; - } - uint32_t result = ADCL | ADCH<<8; - return 1126400UL / result; // Read and back-calculate Vcc in mV - } - // switch mux, settle time is needed. We don't want to delay - // waiting for the settle, so report 0 as a "don't know" value - ADMUX = mux; - return 0; // we don't know the current voltage + static AP_AnalogSource_Arduino vcc(ANALOG_PIN_VCC); + return vcc.read_vcc(); } #endif diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index 9620e7033f..5977f93e0c 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -68,10 +68,13 @@ static void read_battery(void) return; } - if(g.battery_monitoring == 3 || g.battery_monitoring == 4) - battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN_1)) * .1 + battery_voltage1 * .9; + if(g.battery_monitoring == 3 || g.battery_monitoring == 4) { + static AP_AnalogSource_Arduino bat_pin(BATTERY_PIN_1); + battery_voltage1 = BATTERY_VOLTAGE(bat_pin.read_average()); + } if(g.battery_monitoring == 4) { - current_amps1 = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps1 * .9; //reads power sensor current pin + static AP_AnalogSource_Arduino current_pin(CURRENT_PIN_1); + current_amps1 = CURRENT_AMPS(current_pin.read_average()); current_total1 += current_amps1 * 0.02778; // called at 100ms on average, .0002778 is 1/3600 (conversion to hours) } diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index abdea16806..36a945ce6f 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -206,6 +206,9 @@ static void init_ardupilot() timer_scheduler.init( &isr_registry ); + // initialise the analog port reader + AP_AnalogSource_Arduino::init_timer(&timer_scheduler); + #if HIL_MODE != HIL_MODE_ATTITUDE #if CONFIG_ADC == ENABLED // begin filtering the ADC Gyros @@ -657,34 +660,10 @@ void flash_leds(bool on) #ifndef DESKTOP_BUILD /* * Read Vcc vs 1.1v internal reference - * - * This call takes about 150us total. ADC conversion is 13 cycles of - * 125khz default changes the mux if it isn't set, and return last - * reading (allows necessary settle time) otherwise trigger the - * conversion */ uint16_t board_voltage(void) { - const uint8_t mux = (_BV(REFS0)|_BV(MUX4)|_BV(MUX3)|_BV(MUX2)|_BV(MUX1)); - - if (ADMUX == mux) { - ADCSRA |= _BV(ADSC); // Convert - uint16_t counter=4000; // normally takes about 1700 loops - while (bit_is_set(ADCSRA, ADSC) && counter) // Wait - counter--; - if (counter == 0) { - // we don't actually expect this timeout to happen, - // but we don't want any more code that could hang. We - // report 0V so it is clear in the logs that we don't know - // the value - return 0; - } - uint32_t result = ADCL | ADCH<<8; - return 1126400UL / result; // Read and back-calculate Vcc in mV - } - // switch mux, settle time is needed. We don't want to delay - // waiting for the settle, so report 0 as a "don't know" value - ADMUX = mux; - return 0; // we don't know the current voltage + static AP_AnalogSource_Arduino vcc(ANALOG_PIN_VCC); + return vcc.read_vcc(); } #endif diff --git a/ArduPlane/sensors.pde b/ArduPlane/sensors.pde index 1fe1ca8f67..deaa3e601e 100644 --- a/ArduPlane/sensors.pde +++ b/ArduPlane/sensors.pde @@ -66,10 +66,13 @@ static void read_battery(void) return; } - if(g.battery_monitoring == 3 || g.battery_monitoring == 4) - battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN_1)) * .1 + battery_voltage1 * .9; + if(g.battery_monitoring == 3 || g.battery_monitoring == 4) { + static AP_AnalogSource_Arduino bat_pin(BATTERY_PIN_1); + battery_voltage1 = BATTERY_VOLTAGE(bat_pin.read_average()); + } if(g.battery_monitoring == 4) { - current_amps1 = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps1 * .9; //reads power sensor current pin + static AP_AnalogSource_Arduino current_pin(CURRENT_PIN_1); + current_amps1 = CURRENT_AMPS(current_pin.read_average()); current_total1 += current_amps1 * (float)delta_ms_medium_loop * 0.0002778; // .0002778 is 1/3600 (conversion to hours) } diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 6cc0f652f3..181b0ddd1f 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -118,6 +118,9 @@ static void init_ardupilot() timer_scheduler.init( & isr_registry ); + // initialise the analog port reader + AP_AnalogSource_Arduino::init_timer(&timer_scheduler); + // // Check the EEPROM format version before loading any parameters from EEPROM. // @@ -164,6 +167,9 @@ static void init_ardupilot() adc.Init(&timer_scheduler); // APM ADC library initialization #endif + // initialise the analog port reader + AP_AnalogSource_Arduino::init_timer(&timer_scheduler); + barometer.init(&timer_scheduler); if (g.compass_enabled==true) { @@ -574,34 +580,10 @@ void flash_leds(bool on) #ifndef DESKTOP_BUILD /* * Read Vcc vs 1.1v internal reference - * - * This call takes about 150us total. ADC conversion is 13 cycles of - * 125khz default changes the mux if it isn't set, and return last - * reading (allows necessary settle time) otherwise trigger the - * conversion */ uint16_t board_voltage(void) { - const uint8_t mux = (_BV(REFS0)|_BV(MUX4)|_BV(MUX3)|_BV(MUX2)|_BV(MUX1)); - - if (ADMUX == mux) { - ADCSRA |= _BV(ADSC); // Convert - uint16_t counter=4000; // normally takes about 1700 loops - while (bit_is_set(ADCSRA, ADSC) && counter) // Wait - counter--; - if (counter == 0) { - // we don't actually expect this timeout to happen, - // but we don't want any more code that could hang. We - // report 0V so it is clear in the logs that we don't know - // the value - return 0; - } - uint32_t result = ADCL | ADCH<<8; - return 1126400UL / result; // Read and back-calculate Vcc in mV - } - // switch mux, settle time is needed. We don't want to delay - // waiting for the settle, so report 0 as a "don't know" value - ADMUX = mux; - return 0; // we don't know the current voltage + static AP_AnalogSource_Arduino vcc(ANALOG_PIN_VCC); + return vcc.read_vcc(); } #endif