diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index d5b805ee21..5c2bfccdf3 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -8,10 +8,14 @@ * of the License, or (at your option) any later version. */ -#include +#include + #include +#include #include +extern const AP_HAL::HAL& hal; + // table of user settable parameters const AP_Param::GroupInfo AP_Airspeed::var_info[] PROGMEM = { @@ -44,7 +48,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] PROGMEM = { // calibrate the airspeed. This must be called at least once before // the get_airspeed() interface can be used -void AP_Airspeed::calibrate(void (*callback)(unsigned long t)) +void AP_Airspeed::calibrate() { float sum = 0; uint8_t c; @@ -53,7 +57,7 @@ void AP_Airspeed::calibrate(void (*callback)(unsigned long t)) } _source->read(); for (c = 0; c < 10; c++) { - callback(100); + hal.scheduler->delay(100); sum += _source->read(); } _airspeed_raw = sum/c; @@ -69,6 +73,8 @@ void AP_Airspeed::read(void) return; } _airspeed_raw = _source->read(); - airspeed_pressure = max((_airspeed_raw - _offset), 0); - _airspeed = 0.7 * _airspeed + 0.3 * sqrt(airspeed_pressure * _ratio); + airspeed_pressure = ((_airspeed_raw - _offset) < 0) ? + (_airspeed_raw - _offset) : 0; + _airspeed = 0.7 * _airspeed + + 0.3 * sqrt(airspeed_pressure * _ratio); } diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index a4b94217d5..5e5c63fe05 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -4,24 +4,25 @@ #define __AP_AIRSPEED_H__ #include +#include #include -#include class AP_Airspeed { public: // constructor - AP_Airspeed(AP_AnalogSource *source) { + AP_Airspeed() {}; + + void init(AP_HAL::AnalogSource *source) { _source = source; } - // read the analog source and update _airspeed void read(void); // calibrate the airspeed. This must be called on startup if the // altitude/climb_rate/acceleration interfaces are ever used // the callback is a delay() like routine - void calibrate(void (*callback)(unsigned long t)); + void calibrate(); // return the current airspeed in m/s float get_airspeed(void) { @@ -48,10 +49,10 @@ public: _airspeed = airspeed; } - static const struct AP_Param::GroupInfo var_info[]; + static const struct AP_Param::GroupInfo var_info[]; private: - AP_AnalogSource * _source; + AP_HAL::AnalogSource *_source; AP_Float _offset; AP_Float _ratio; AP_Int8 _use; diff --git a/libraries/AP_Airspeed/examples/Airspeed/Airspeed.pde b/libraries/AP_Airspeed/examples/Airspeed/Airspeed.pde index a62630be48..5036f72ffc 100644 --- a/libraries/AP_Airspeed/examples/Airspeed/Airspeed.pde +++ b/libraries/AP_Airspeed/examples/Airspeed/Airspeed.pde @@ -8,46 +8,37 @@ * of the License, or (at your option) any later version. */ -#include #include #include #include -#include -#include -#include -#include +#include +#include + #include #include #include -Arduino_Mega_ISR_Registry isr_registry; -AP_TimerProcess scheduler; +const AP_HAL::HAL& hal = AP_HAL_AVR_APM2; -FastSerialPort0(Serial); - -AP_AnalogSource_Arduino pin0(0); -AP_Airspeed airspeed(&pin0); +AP_Airspeed airspeed; void setup() { - Serial.begin(115200, 128, 128); - Serial.println("ArduPilot Airspeed library test"); + hal.uart0->begin(115200, 128, 128); + hal.console->println("ArduPilot Airspeed library test"); - isr_registry.init(); - scheduler.init(&isr_registry); - AP_AnalogSource_Arduino::init_timer(&scheduler); - - pinMode(0, INPUT); - - airspeed.calibrate(delay); + airspeed.init(hal.analogin->channel(0)); + airspeed.calibrate(); } void loop(void) { static uint32_t timer; - if((millis() - timer) > 100) { - timer = millis(); + if((hal.scheduler->millis() - timer) > 100) { + timer = hal.scheduler->millis(); airspeed.read(); - Serial.printf("airspeed %.2f\n", airspeed.get_airspeed()); + hal.console->printf("airspeed %.2f\n", airspeed.get_airspeed()); } } + +AP_HAL_MAIN(); diff --git a/libraries/AP_Airspeed/examples/Airspeed/Arduino.h b/libraries/AP_Airspeed/examples/Airspeed/Arduino.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_Airspeed/examples/Airspeed/nocore.inoflag b/libraries/AP_Airspeed/examples/Airspeed/nocore.inoflag new file mode 100644 index 0000000000..e69de29bb2