Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder: PWM: take an average of any readings accumulated in irq

This commit is contained in:
Peter Barker 2018-11-07 23:48:10 +11:00 committed by Andrew Tridgell
parent 2116737e35
commit c2b334eaad
2 changed files with 9 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_RangeFinder/AP_RangeFinder_PWM.cpp
View File

@ -43,8 +43,9 @@ void AP_RangeFinder_PWM::irq_handler(uint8_t pin, bool pin_high, uint32_t timest
irq_pulse_start_us = timestamp_us; irq_pulse_start_us = timestamp_us;
} else { } else {
if (irq_pulse_start_us != 0) { if (irq_pulse_start_us != 0) {
irq_value_us = timestamp_us - irq_pulse_start_us; irq_value_us += timestamp_us - irq_pulse_start_us;
irq_pulse_start_us = 0; irq_pulse_start_us = 0;
irq_sample_count++;
} }
} }
} }
@ -55,13 +56,15 @@ bool AP_RangeFinder_PWM::get_reading(uint16_t &reading_cm)
// disable interrupts and grab state // disable interrupts and grab state
void *irqstate = hal.scheduler->disable_interrupts_save(); void *irqstate = hal.scheduler->disable_interrupts_save();
const uint32_t value_us = irq_value_us; const uint32_t value_us = irq_value_us;
const uint16_t sample_count = irq_sample_count;
irq_value_us = 0; irq_value_us = 0;
irq_sample_count = 0;
hal.scheduler->restore_interrupts(irqstate); hal.scheduler->restore_interrupts(irqstate);
if (value_us == 0) { if (value_us == 0 || sample_count == 0) {
return false; return false;
} }
reading_cm = value_us * 1e-1f; // correct for LidarLite. Parameter needed? reading_cm = (value_us/sample_count) * 1e-1f; // correct for LidarLite. Parameter needed?
return true; return true;
} }

5
libraries/AP_RangeFinder/AP_RangeFinder_PWM.h
View File

@ -45,8 +45,9 @@ private:
int8_t last_pin; // last pin used for reading pwm (used to recognise change in pin assignment) int8_t last_pin; // last pin used for reading pwm (used to recognise change in pin assignment)
uint32_t last_reading_ms; // system time of last read (used for health reporting) uint32_t last_reading_ms; // system time of last read (used for health reporting)
// the following two members are updated by the interrupt handler // the following three members are updated by the interrupt handler
uint32_t irq_value_us; // last calculated pwm value (irq copy) uint32_t irq_value_us; // some of calculated pwm values (irq copy)
uint16_t irq_sample_count; // number of pwm values in irq_value_us (irq copy)
uint32_t irq_pulse_start_us; // system time of start of pulse uint32_t irq_pulse_start_us; // system time of start of pulse
void irq_handler(uint8_t pin, bool pin_high, uint32_t timestamp_us); void irq_handler(uint8_t pin, bool pin_high, uint32_t timestamp_us);