mirror of https://github.com/ArduPilot/ardupilot
AP_HAL: added voltage_average_ratiometric() call to AnalogIn
this is for ratiometric sensors such as the 3DR airspeed sensor and the Maxbotix analog sonar
This commit is contained in:
parent
dd0630dc16
commit
589b8cdb58
|
@ -27,6 +27,10 @@ public:
|
||||||
// return a voltage from 0.0 to 5.0V, scaled
|
// return a voltage from 0.0 to 5.0V, scaled
|
||||||
// against a reference voltage
|
// against a reference voltage
|
||||||
virtual float voltage_average() = 0;
|
virtual float voltage_average() = 0;
|
||||||
|
|
||||||
|
// return a voltage from 0.0 to 5.0V, assuming a ratiometric
|
||||||
|
// sensor
|
||||||
|
virtual float voltage_average_ratiometric() = 0;
|
||||||
};
|
};
|
||||||
|
|
||||||
class AP_HAL::AnalogIn {
|
class AP_HAL::AnalogIn {
|
||||||
|
|
|
@ -19,6 +19,7 @@ public:
|
||||||
float read_latest();
|
float read_latest();
|
||||||
void set_pin(uint8_t p);
|
void set_pin(uint8_t p);
|
||||||
float voltage_average();
|
float voltage_average();
|
||||||
|
float voltage_average_ratiometric();
|
||||||
void set_stop_pin(uint8_t p);
|
void set_stop_pin(uint8_t p);
|
||||||
void set_settle_time(uint16_t settle_time_ms);
|
void set_settle_time(uint16_t settle_time_ms);
|
||||||
|
|
||||||
|
|
|
@ -59,6 +59,17 @@ float ADCSource::voltage_average(void)
|
||||||
return v * vcc_mV * 9.765625e-7; // 9.765625e-7 = 1.0/(1024*1000)
|
return v * vcc_mV * 9.765625e-7; // 9.765625e-7 = 1.0/(1024*1000)
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
return voltage from 0.0 to 5.0V, assuming a ratiometric sensor. This
|
||||||
|
means the result is really a pseudo-voltage, that assumes the supply
|
||||||
|
voltage is exactly 5.0V.
|
||||||
|
*/
|
||||||
|
float ADCSource::voltage_average_ratiometric(void)
|
||||||
|
{
|
||||||
|
float v = read_average();
|
||||||
|
return v * (5.0f / 1023.0f);
|
||||||
|
}
|
||||||
|
|
||||||
void ADCSource::set_pin(uint8_t pin) {
|
void ADCSource::set_pin(uint8_t pin) {
|
||||||
_pin = pin;
|
_pin = pin;
|
||||||
}
|
}
|
||||||
|
|
|
@ -22,7 +22,7 @@ float ADCSource::read_average() {
|
||||||
}
|
}
|
||||||
|
|
||||||
float ADCSource::voltage_average() {
|
float ADCSource::voltage_average() {
|
||||||
return (5.0/1024.0) * read_average();
|
return (5.0f/1023.0f) * read_average();
|
||||||
}
|
}
|
||||||
|
|
||||||
float ADCSource::read_latest() {
|
float ADCSource::read_latest() {
|
||||||
|
|
|
@ -19,6 +19,7 @@ public:
|
||||||
float read_latest();
|
float read_latest();
|
||||||
void set_pin(uint8_t p);
|
void set_pin(uint8_t p);
|
||||||
float voltage_average();
|
float voltage_average();
|
||||||
|
float voltage_average_ratiometric() { return voltage_average(); }
|
||||||
void set_stop_pin(uint8_t pin) {}
|
void set_stop_pin(uint8_t pin) {}
|
||||||
void set_settle_time(uint16_t settle_time_ms) {}
|
void set_settle_time(uint16_t settle_time_ms) {}
|
||||||
|
|
||||||
|
|
|
@ -31,7 +31,7 @@ using namespace AVR_SITL;
|
||||||
uint16_t SITL_State::_airspeed_sensor(float airspeed)
|
uint16_t SITL_State::_airspeed_sensor(float airspeed)
|
||||||
{
|
{
|
||||||
const float airspeed_ratio = 1.9936;
|
const float airspeed_ratio = 1.9936;
|
||||||
const float airspeed_offset = 2820;
|
const float airspeed_offset = 503;
|
||||||
float airspeed_pressure, airspeed_raw;
|
float airspeed_pressure, airspeed_raw;
|
||||||
|
|
||||||
airspeed_pressure = (airspeed*airspeed) / airspeed_ratio;
|
airspeed_pressure = (airspeed*airspeed) / airspeed_ratio;
|
||||||
|
|
|
@ -13,6 +13,7 @@ public:
|
||||||
void set_stop_pin(uint8_t p);
|
void set_stop_pin(uint8_t p);
|
||||||
void set_settle_time(uint16_t settle_time_ms);
|
void set_settle_time(uint16_t settle_time_ms);
|
||||||
float voltage_average();
|
float voltage_average();
|
||||||
|
float voltage_average_ratiometric() { return voltage_average(); }
|
||||||
private:
|
private:
|
||||||
float _v;
|
float _v;
|
||||||
};
|
};
|
||||||
|
|
|
@ -26,6 +26,7 @@ public:
|
||||||
float read_latest();
|
float read_latest();
|
||||||
void set_pin(uint8_t p);
|
void set_pin(uint8_t p);
|
||||||
float voltage_average();
|
float voltage_average();
|
||||||
|
float voltage_average_ratiometric() { return voltage_average(); }
|
||||||
|
|
||||||
// stop pins not implemented on PX4 yet
|
// stop pins not implemented on PX4 yet
|
||||||
void set_stop_pin(uint8_t p) {}
|
void set_stop_pin(uint8_t p) {}
|
||||||
|
|
|
@ -11,6 +11,7 @@ public:
|
||||||
float read_latest();
|
float read_latest();
|
||||||
void set_pin(uint8_t p);
|
void set_pin(uint8_t p);
|
||||||
float voltage_average();
|
float voltage_average();
|
||||||
|
float voltage_average_ratiometric() { return voltage_average(); }
|
||||||
void set_stop_pin(uint8_t p) {}
|
void set_stop_pin(uint8_t p) {}
|
||||||
void set_settle_time(uint16_t settle_time_ms) {}
|
void set_settle_time(uint16_t settle_time_ms) {}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue