diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp new file mode 100644 index 0000000000..b5319c96ce --- /dev/null +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -0,0 +1,75 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + APM_Airspeed.cpp - airspeed (pitot) driver + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public License + as published by the Free Software Foundation; either version 2.1 + of the License, or (at your option) any later version. +*/ + +#include +#include +#include +#include + +// table of user settable parameters +const AP_Param::GroupInfo AP_Airspeed::var_info[] PROGMEM = { + + // @Param: ARSPD_ENABLE + // @DisplayName: Airspeed enable + // @Description: enable airspeed sensor + // @Values: 0:Disable,1:Enable + AP_GROUPINFO("ENABLE", 0, AP_Airspeed, _enable), + + // @Param: ARSPD_USE + // @DisplayName: Airspeed use + // @Description: use airspeed for flight control + // @Values: 0:Use,1:Don't Use + AP_GROUPINFO("USE", 1, AP_Airspeed, _use), + + // @Param: ARSPD_OFFSET + // @DisplayName: Airspeed offset + // @Description: Airspeed calibration offset + // @Increment: 0.1 + AP_GROUPINFO("OFFSET", 2, AP_Airspeed, _offset), + + // @Param: ARSPD_RATIO + // @DisplayName: Airspeed ratio + // @Description: Airspeed calibration ratio + // @Increment: 0.1 + AP_GROUPINFO("RATIO", 3, AP_Airspeed, _ratio), + + AP_GROUPEND +}; + +// 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)) +{ + float sum = 0; + uint8_t c; + if (!_enable) { + return; + } + _source->read(); + for (c = 0; c < 10; c++) { + callback(100); + sum += _source->read(); + } + _airspeed_raw = sum/c; + _offset.set_and_save(_airspeed_raw); + _airspeed = 0; +} + +// read the airspeed sensor +void AP_Airspeed::read(void) +{ + float airspeed_pressure; + if (!_enable) { + return; + } + _airspeed_raw = _filter.apply(_source->read()); + airspeed_pressure = max((_airspeed_raw - _offset), 0); + _airspeed = sqrt(airspeed_pressure * _ratio); +} diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h new file mode 100644 index 0000000000..abb1f8b1a7 --- /dev/null +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -0,0 +1,63 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#ifndef __AP_AIRSPEED_H__ +#define __AP_AIRSPEED_H__ + +#include +#include +#include +#include +#include + +class AP_Airspeed +{ +public: + // constructor + AP_Airspeed(AP_AnalogSource *source, float ratio, bool enable) { + _source = source; + _offset.set(0); + _ratio.set(ratio); + + // by default enable but don't use the airspeed sensor + _use.set(1); + _enable.set(enable?1:0); + } + + // 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)); + + // return the current airspeed in m/s + float get_airspeed(void) { return _airspeed; } + + // return the current airspeed in cm/s + float get_airspeed_cm(void) { return _airspeed*100; } + + // return true if airspeed is enabled, and airspeed use is set + bool use(void) { return _enable && _use; } + + // return true if airspeed is enabled + bool enabled(void) { return _enable; } + + // used by HIL to set the airspeed + void set_HIL(float airspeed) { _airspeed = airspeed; } + + static const struct AP_Param::GroupInfo var_info[]; + +private: + AP_AnalogSource *_source; + AP_Float _offset; + AP_Float _ratio; + AP_Int8 _use; + AP_Int8 _enable; + float _airspeed; + float _airspeed_raw; + AverageFilterFloat_Size5 _filter; +}; + + +#endif // __AP_AIRSPEED_H__ diff --git a/libraries/AP_Airspeed/examples/Airspeed/Airspeed.pde b/libraries/AP_Airspeed/examples/Airspeed/Airspeed.pde new file mode 100644 index 0000000000..8430084de4 --- /dev/null +++ b/libraries/AP_Airspeed/examples/Airspeed/Airspeed.pde @@ -0,0 +1,51 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + Airspeed.pde - airspeed example sketch + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public License + as published by the Free Software Foundation; either version 2.1 + of the License, or (at your option) any later version. +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +Arduino_Mega_ISR_Registry isr_registry; +AP_TimerProcess scheduler; + +FastSerialPort0(Serial); + +AP_AnalogSource_Arduino pin0(0); +AP_Airspeed airspeed(&pin0, 1.9936, true); + +void setup() +{ + Serial.begin(115200, 128, 128); + Serial.println("ArduPilot Airspeed library test"); + + isr_registry.init(); + scheduler.init(&isr_registry); + AP_AnalogSource_Arduino::init_timer(&scheduler); + + pinMode(0, INPUT); + + airspeed.calibrate(delay); +} + +void loop(void) +{ + static uint32_t timer; + if((millis() - timer) > 100){ + timer = millis(); + airspeed.read(); + Serial.printf("airspeed %.2f\n", airspeed.get_airspeed()); + } +} diff --git a/libraries/AP_Airspeed/examples/Airspeed/Makefile b/libraries/AP_Airspeed/examples/Airspeed/Makefile new file mode 100644 index 0000000000..d1f40fd90f --- /dev/null +++ b/libraries/AP_Airspeed/examples/Airspeed/Makefile @@ -0,0 +1 @@ +include ../../../AP_Common/Arduino.mk