Browse Source

SITL: added basic analogin implementation

mission-4.1.18
Andrew Tridgell 12 years ago
parent
commit
46f7c9e92b
  1. 20
      libraries/AP_HAL_AVR_SITL/AnalogIn.cpp
  2. 1
      libraries/AP_HAL_AVR_SITL/AnalogIn.h
  3. 4
      libraries/AP_HAL_AVR_SITL/HAL_AVR_SITL_Class.cpp

20
libraries/AP_HAL_AVR_SITL/AnalogIn.cpp

@ -13,18 +13,26 @@ using namespace AVR_SITL; @@ -13,18 +13,26 @@ using namespace AVR_SITL;
extern const AP_HAL::HAL& hal;
ADCSource::ADCSource(uint8_t pin, float prescale) :
_pin(pin),
_prescale(prescale)
{}
float ADCSource::read_average() {
return 0;
return read_latest();
}
float ADCSource::read_latest() {
return 0;
switch (_pin) {
case ANALOG_INPUT_BOARD_VCC:
return 4900;
case ANALOG_INPUT_NONE:
default:
return 0.0;
}
}
void ADCSource::set_pin(uint8_t pin) {
_pin = pin;
}
SITLAnalogIn::SITLAnalogIn() {}
@ -32,12 +40,12 @@ SITLAnalogIn::SITLAnalogIn() {} @@ -32,12 +40,12 @@ SITLAnalogIn::SITLAnalogIn() {}
void SITLAnalogIn::init(void *ap_hal_scheduler) {
}
AP_HAL::AnalogSource* SITLAnalogIn::channel(int16_t n) {
return NULL;
AP_HAL::AnalogSource* SITLAnalogIn::channel(int16_t pin) {
return channel(pin, 1.0);
}
AP_HAL::AnalogSource* SITLAnalogIn::channel(int16_t n, float prescale) {
return NULL;
AP_HAL::AnalogSource* SITLAnalogIn::channel(int16_t pin, float prescale) {
return new ADCSource(pin, prescale);
}
#endif

1
libraries/AP_HAL_AVR_SITL/AnalogIn.h

@ -21,6 +21,7 @@ public: @@ -21,6 +21,7 @@ public:
private:
/* prescale scales the raw measurments for read()*/
uint8_t _pin;
const float _prescale;
};

4
libraries/AP_HAL_AVR_SITL/HAL_AVR_SITL_Class.cpp

@ -29,9 +29,9 @@ static SITLConsoleDriver consoleDriver; @@ -29,9 +29,9 @@ static SITLConsoleDriver consoleDriver;
static SITL_State sitlState;
static SITLRCInput sitlRCInput(&sitlState);
static SITLRCOutput sitlRCOutput(&sitlState);
static SITLAnalogIn sitlAnalogIn;
static Empty::EmptyGPIO emptyGPIO;
static Empty::EmptyAnalogIn emptyAnalogIn;
static SITLUARTDriver sitlUart0Driver(0, &sitlState);
static SITLUARTDriver sitlUart1Driver(1, &sitlState);
@ -44,7 +44,7 @@ HAL_AVR_SITL::HAL_AVR_SITL() : @@ -44,7 +44,7 @@ HAL_AVR_SITL::HAL_AVR_SITL() :
&sitlUart2Driver, /* uartC */
NULL, /* i2c */
NULL, /* spi */
&emptyAnalogIn, /* analogin */
&sitlAnalogIn, /* analogin */
&sitlEEPROMStorage, /* storage */
&consoleDriver, /* console */
&emptyGPIO, /* gpio */

Loading…
Cancel
Save