diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index 3e6147b7a7..893c739d55 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -553,7 +553,7 @@ void setup() { // load the default values of variables listed in var_info[] AP_Param::setup_sketch_defaults(); - rssi_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE, 1.0); + rssi_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE); vcc_pin = hal.analogin->channel(ANALOG_INPUT_BOARD_VCC); batt_volt_pin = hal.analogin->channel(g.battery_volt_pin); batt_curr_pin = hal.analogin->channel(g.battery_curr_pin); diff --git a/APMrover2/sensors.pde b/APMrover2/sensors.pde index fdc16cff57..196db0a45a 100644 --- a/APMrover2/sensors.pde +++ b/APMrover2/sensors.pde @@ -44,7 +44,7 @@ static void read_battery(void) void read_receiver_rssi(void) { rssi_analog_source->set_pin(g.rssi_pin); - float ret = rssi_analog_source->read_average(); + float ret = rssi_analog_source->voltage_average() * 50; receiver_rssi = constrain_int16(ret, 0, 255); } diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index b9545408ee..f3b262ae37 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -782,8 +782,7 @@ static AP_Relay relay; static AP_Camera camera(&relay); #endif -// a pin for reading the receiver RSSI voltage. The scaling by 0.25 -// is to take the 0 to 1024 range down to an 8 bit range for MAVLink +// a pin for reading the receiver RSSI voltage. static AP_HAL::AnalogSource* rssi_analog_source; @@ -878,7 +877,7 @@ void setup() { &sonar_mode_filter); #endif - rssi_analog_source = hal.analogin->channel(g.rssi_pin, 0.25); + rssi_analog_source = hal.analogin->channel(g.rssi_pin); batt_volt_analog_source = hal.analogin->channel(g.battery_volt_pin); batt_curr_analog_source = hal.analogin->channel(g.battery_curr_pin); board_vcc_analog_source = hal.analogin->channel(ANALOG_INPUT_BOARD_VCC); diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index e5a2332806..7662d5d4b0 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -142,6 +142,6 @@ static void read_battery(void) void read_receiver_rssi(void) { rssi_analog_source->set_pin(g.rssi_pin); - float ret = rssi_analog_source->read_latest(); + float ret = rssi_analog_source->voltage_average() * 50; receiver_rssi = constrain_int16(ret, 0, 255); } diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index ec9b1641c1..84d9ef57d6 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduPlane V2.73beta" +#define THISFIRMWARE "ArduPlane V2.74beta" /* * Lead developer: Andrew Tridgell * @@ -242,8 +242,7 @@ static AP_Navigation *nav_controller = &L1_controller; static AP_HAL::AnalogSource *pitot_analog_source; -// a pin for reading the receiver RSSI voltage. The scaling by 0.25 -// is to take the 0 to 1024 range down to an 8 bit range for MAVLink +// a pin for reading the receiver RSSI voltage. static AP_HAL::AnalogSource *rssi_analog_source; static AP_HAL::AnalogSource *vcc_pin; @@ -662,13 +661,14 @@ void setup() { // load the default values of variables listed in var_info[] AP_Param::setup_sketch_defaults(); - rssi_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE, 0.25); + rssi_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE); #if CONFIG_PITOT_SOURCE == PITOT_SOURCE_ADC pitot_analog_source = new AP_ADC_AnalogSource( &adc, - CONFIG_PITOT_SOURCE_ADC_CHANNEL, 1.0); + CONFIG_PITOT_SOURCE_ADC_CHANNEL, 1.0f); #elif CONFIG_PITOT_SOURCE == PITOT_SOURCE_ANALOG_PIN - pitot_analog_source = hal.analogin->channel(CONFIG_PITOT_SOURCE_ANALOG_PIN, CONFIG_PITOT_SCALING); + pitot_analog_source = hal.analogin->channel(CONFIG_PITOT_SOURCE_ANALOG_PIN); + hal.gpio->write(hal.gpio->analogPinToDigitalPin(CONFIG_PITOT_SOURCE_ANALOG_PIN), 0); #endif vcc_pin = hal.analogin->channel(ANALOG_INPUT_BOARD_VCC); diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 646ac15e38..52292075c2 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -118,7 +118,6 @@ # define CONFIG_INS_TYPE CONFIG_INS_MPU6000 # define CONFIG_PITOT_SOURCE PITOT_SOURCE_ANALOG_PIN # define CONFIG_PITOT_SOURCE_ANALOG_PIN 0 - # define CONFIG_PITOT_SCALING 4.0 # ifdef APM2_BETA_HARDWARE # define CONFIG_BARO AP_BARO_BMP085 # else // APM2 Production Hardware (default) @@ -137,7 +136,6 @@ # define CONFIG_INS_TYPE CONFIG_INS_STUB # define CONFIG_PITOT_SOURCE PITOT_SOURCE_ANALOG_PIN # define CONFIG_PITOT_SOURCE_ANALOG_PIN 0 - # define CONFIG_PITOT_SCALING 4.0 # define CONFIG_BARO AP_BARO_HIL # define CONFIG_COMPASS AP_COMPASS_HIL #elif CONFIG_HAL_BOARD == HAL_BOARD_PX4 @@ -152,7 +150,6 @@ # define CONFIG_INS_TYPE CONFIG_INS_PX4 # define CONFIG_PITOT_SOURCE PITOT_SOURCE_ANALOG_PIN # define CONFIG_PITOT_SOURCE_ANALOG_PIN 11 - # define CONFIG_PITOT_SCALING (4.0*5.0/3.3) # define CONFIG_BARO AP_BARO_PX4 # define CONFIG_COMPASS AP_COMPASS_PX4 # define SERIAL0_BAUD 115200 @@ -202,8 +199,6 @@ #define CONFIG_PITOT_SOURCE PITOT_SOURCE_ANALOG_PIN #undef CONFIG_PITOT_SOURCE_ANALOG_PIN #define CONFIG_PITOT_SOURCE_ANALOG_PIN -1 - #undef CONFIG_PITOT_SCALING - #define CONFIG_PITOT_SCALING 4.0 #undef CONFIG_COMPASS #define CONFIG_COMPASS AP_COMPASS_HIL #endif diff --git a/ArduPlane/sensors.pde b/ArduPlane/sensors.pde index 9b80f8ff66..8a128ae38b 100644 --- a/ArduPlane/sensors.pde +++ b/ArduPlane/sensors.pde @@ -67,7 +67,7 @@ static void read_battery(void) void read_receiver_rssi(void) { rssi_analog_source->set_pin(g.rssi_pin); - float ret = rssi_analog_source->read_average(); + float ret = rssi_analog_source->voltage_average() * 50; receiver_rssi = constrain_int16(ret, 0, 255); } diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index e594db3f5d..f22e123222 100644 --- a/ArduPlane/test.pde +++ b/ArduPlane/test.pde @@ -606,9 +606,9 @@ test_mag(uint8_t argc, const Menu::arg *argv) static int8_t test_airspeed(uint8_t argc, const Menu::arg *argv) { - float airspeed_ch = pitot_analog_source->read_average(); + float airspeed_ch = pitot_analog_source->voltage_average(); // cliSerial->println(pitot_analog_source.read()); - cliSerial->printf_P(PSTR("airspeed_ch: %.1f\n"), airspeed_ch); + cliSerial->printf_P(PSTR("airspeed_ch: %.3f\n"), airspeed_ch); if (!airspeed.enabled()) { cliSerial->printf_P(PSTR("airspeed: ")); diff --git a/libraries/AP_ADC_AnalogSource/AP_ADC_AnalogSource.h b/libraries/AP_ADC_AnalogSource/AP_ADC_AnalogSource.h index 9817dd555f..c9ce70f206 100644 --- a/libraries/AP_ADC_AnalogSource/AP_ADC_AnalogSource.h +++ b/libraries/AP_ADC_AnalogSource/AP_ADC_AnalogSource.h @@ -15,6 +15,7 @@ public: float read_latest(void); void set_pin(uint8_t); float voltage_average(); + float voltage_average_ratiometric() { return voltage_average(); } // stop pins not implemented on ADC yet void set_stop_pin(uint8_t p) {} diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index f61283d28b..4a2fa6cd73 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -184,15 +184,14 @@ Vector2f AP_AHRS::groundspeed_vector(void) if (gotAirspeed) { Vector3f wind = wind_estimate(); Vector2f wind2d = Vector2f(wind.x, wind.y); - Vector2f airspeed_vector = Vector2f(cos(yaw), sin(yaw)) * airspeed; + Vector2f airspeed_vector = Vector2f(cosf(yaw), sinf(yaw)) * airspeed; gndVelADS = airspeed_vector - wind2d; } // Generate estimate of ground speed vector using GPS if (gotGPS) { - Vector2f v; float cog = radians(_gps->ground_course*0.01f); - gndVelGPS = Vector2f(cos(cog), sin(cog)) * _gps->ground_speed * 0.01f; + gndVelGPS = Vector2f(cosf(cog), sinf(cog)) * _gps->ground_speed * 0.01f; } // If both ADS and GPS data is available, apply a complementary filter if (gotAirspeed && gotGPS) { @@ -207,15 +206,13 @@ Vector2f AP_AHRS::groundspeed_vector(void) // To-Do - set Tau as a function of GPS lag. const float alpha = 1.0f - beta; // Run LP filters - _xlp = beta*gndVelGPS.x + alpha*_xlp; - _ylp = beta*gndVelGPS.y + alpha*_ylp; + _lp = gndVelGPS * beta + _lp * alpha; // Run HP filters - _xhp = gndVelADS.x - _lastGndVelADS.x + alpha*_xhp; - _yhp = gndVelADS.y - _lastGndVelADS.y + alpha*_yhp; + _hp = (gndVelADS - _lastGndVelADS) + _hp * alpha; // Save the current ADS ground vector for the next time step _lastGndVelADS = gndVelADS; // Sum the HP and LP filter outputs - return Vector2f(_xhp + _xlp, _yhp + _ylp); + return _hp + _lp; } // Only ADS data is available return ADS estimate if (gotAirspeed && !gotGPS) { diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 1165a54af8..c523370265 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -219,13 +219,10 @@ protected: // Declare filter states for HPF and LPF used by complementary // filter in AP_AHRS::groundspeed_vector - float _xlp; // x component low-pass filter - float _ylp; // y component low-pass filter - float _xhp; // x component high-pass filter - float _yhp; // y component high-pass filter - Vector2f _lastGndVelADS; // previous HPF input - - }; + Vector2f _lp; // ground vector low-pass filter + Vector2f _hp; // ground vector high-pass filter + Vector2f _lastGndVelADS; // previous HPF input +}; #include #include diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index cd2d54763b..0d1d170639 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -47,10 +47,10 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] PROGMEM = { /* this scaling factor converts from the old system where we used a - 0 to 1023 raw ADC value for 0-5V to the new system which gets the + 0 to 4095 raw ADC value for 0-5V to the new system which gets the voltage in volts directly from the ADC driver */ -#define SCALING_OLD_CALIBRATION 204.8f // 1024/5 +#define SCALING_OLD_CALIBRATION 819 // 4095/5 // calibrate the airspeed. This must be called at least once before // the get_airspeed() interface can be used @@ -61,10 +61,10 @@ void AP_Airspeed::calibrate() if (!_enable) { return; } - _source->voltage_average(); + _source->voltage_average_ratiometric(); for (c = 0; c < 10; c++) { hal.scheduler->delay(100); - sum += _source->voltage_average() * SCALING_OLD_CALIBRATION; + sum += _source->voltage_average_ratiometric() * SCALING_OLD_CALIBRATION; } float raw = sum/c; _offset.set_and_save(raw); @@ -78,7 +78,7 @@ void AP_Airspeed::read(void) if (!_enable) { return; } - float raw = _source->voltage_average() * SCALING_OLD_CALIBRATION; + float raw = _source->voltage_average_ratiometric() * SCALING_OLD_CALIBRATION; airspeed_pressure = max(raw - _offset, 0); float new_airspeed = sqrtf(airspeed_pressure * _ratio); _airspeed = 0.7f * _airspeed + 0.3f * new_airspeed; diff --git a/libraries/AP_HAL/AnalogIn.h b/libraries/AP_HAL/AnalogIn.h index d64d7c5ab6..fa9f291055 100644 --- a/libraries/AP_HAL/AnalogIn.h +++ b/libraries/AP_HAL/AnalogIn.h @@ -27,13 +27,16 @@ public: // return a voltage from 0.0 to 5.0V, scaled // against a reference voltage 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 { public: virtual void init(void* implspecific) = 0; virtual AP_HAL::AnalogSource* channel(int16_t n) = 0; - virtual AP_HAL::AnalogSource* channel(int16_t n, float scale) = 0; }; #define ANALOG_INPUT_BOARD_VCC 254 diff --git a/libraries/AP_HAL_AVR/AnalogIn.h b/libraries/AP_HAL_AVR/AnalogIn.h index b13a36a57f..f4456a8125 100644 --- a/libraries/AP_HAL_AVR/AnalogIn.h +++ b/libraries/AP_HAL_AVR/AnalogIn.h @@ -12,13 +12,14 @@ public: friend class AP_HAL_AVR::AVRAnalogIn; /* pin designates the ADC input number, or when == AVR_ANALOG_PIN_VCC, * board vcc */ - ADCSource(uint8_t pin, float prescale = 1.0); + ADCSource(uint8_t pin); /* implement AnalogSource virtual api: */ float read_average(); float read_latest(); void set_pin(uint8_t p); float voltage_average(); + float voltage_average_ratiometric(); void set_stop_pin(uint8_t p); void set_settle_time(uint16_t settle_time_ms); @@ -57,9 +58,6 @@ private: uint8_t _stop_pin; uint16_t _settle_time_ms; uint32_t _read_start_time_ms; - - /* prescale scales the raw measurments for read()*/ - const float _prescale; }; /* AVRAnalogIn : a concrete class providing the implementations of the @@ -69,10 +67,9 @@ public: AVRAnalogIn(); void init(void* ap_hal_scheduler); AP_HAL::AnalogSource* channel(int16_t n); - AP_HAL::AnalogSource* channel(int16_t n, float prescale); protected: - static ADCSource* _create_channel(int16_t num, float scale); + static ADCSource* _create_channel(int16_t num); static void _register_channel(ADCSource*); static void _timer_event(uint32_t); static ADCSource* _channels[AVR_INPUT_MAX_CHANNELS]; diff --git a/libraries/AP_HAL_AVR/AnalogIn_ADC.cpp b/libraries/AP_HAL_AVR/AnalogIn_ADC.cpp index 7b3825107e..442dd98f87 100644 --- a/libraries/AP_HAL_AVR/AnalogIn_ADC.cpp +++ b/libraries/AP_HAL_AVR/AnalogIn_ADC.cpp @@ -11,14 +11,13 @@ using namespace AP_HAL_AVR; extern const AP_HAL::HAL& hal; -ADCSource::ADCSource(uint8_t pin, float prescale) : +ADCSource::ADCSource(uint8_t pin) : _pin(pin), _stop_pin(ANALOG_INPUT_NONE), _sum_count(0), _sum(0), _settle_time_ms(0), - _last_average(0), - _prescale(prescale) + _last_average(0) {} float ADCSource::read_average() { @@ -26,7 +25,7 @@ float ADCSource::read_average() { uint16_t v = (uint16_t) _read_average(); return 1126400UL / v; } else { - return _prescale * _read_average(); + return _read_average(); } } @@ -38,7 +37,7 @@ float ADCSource::read_latest() { if (_pin == ANALOG_INPUT_BOARD_VCC) { return 1126400UL / latest; } else { - return _prescale * latest; + return latest; } } @@ -59,6 +58,17 @@ float ADCSource::voltage_average(void) 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) { _pin = pin; } diff --git a/libraries/AP_HAL_AVR/AnalogIn_Common.cpp b/libraries/AP_HAL_AVR/AnalogIn_Common.cpp index 1dc390d8ef..9f9cfdf5d5 100644 --- a/libraries/AP_HAL_AVR/AnalogIn_Common.cpp +++ b/libraries/AP_HAL_AVR/AnalogIn_Common.cpp @@ -33,8 +33,8 @@ void AVRAnalogIn::init(void* machtnichts) { _register_channel(&_vcc); } -ADCSource* AVRAnalogIn::_create_channel(int16_t chnum, float scale) { - ADCSource *ch = new ADCSource(chnum, scale); +ADCSource* AVRAnalogIn::_create_channel(int16_t chnum) { + ADCSource *ch = new ADCSource(chnum); _register_channel(ch); return ch; } @@ -110,14 +110,12 @@ next_channel: } -AP_HAL::AnalogSource* AVRAnalogIn::channel(int16_t ch) { - return this->channel(ch, 1.0); -} -AP_HAL::AnalogSource* AVRAnalogIn::channel(int16_t ch, float scale) { +AP_HAL::AnalogSource* AVRAnalogIn::channel(int16_t ch) +{ if (ch == ANALOG_INPUT_BOARD_VCC) { return &_vcc; } else { - return _create_channel(ch, scale); + return _create_channel(ch); } } diff --git a/libraries/AP_HAL_AVR_SITL/AnalogIn.cpp b/libraries/AP_HAL_AVR_SITL/AnalogIn.cpp index 44f36abf44..57d41725cf 100644 --- a/libraries/AP_HAL_AVR_SITL/AnalogIn.cpp +++ b/libraries/AP_HAL_AVR_SITL/AnalogIn.cpp @@ -11,10 +11,9 @@ using namespace AVR_SITL; extern const AP_HAL::HAL& hal; -ADCSource::ADCSource(SITL_State *sitlState, uint8_t pin, float prescale) : +ADCSource::ADCSource(SITL_State *sitlState, uint8_t pin) : _sitlState(sitlState), - _pin(pin), - _prescale(prescale) + _pin(pin) {} float ADCSource::read_average() { @@ -22,7 +21,7 @@ float ADCSource::read_average() { } float ADCSource::voltage_average() { - return (5.0/1024.0) * read_average(); + return (5.0f/1023.0f) * read_average(); } float ADCSource::read_latest() { @@ -47,11 +46,7 @@ void SITLAnalogIn::init(void *ap_hal_scheduler) { } AP_HAL::AnalogSource* SITLAnalogIn::channel(int16_t pin) { - return channel(pin, 1.0); -} - -AP_HAL::AnalogSource* SITLAnalogIn::channel(int16_t pin, float prescale) { - return new ADCSource(_sitlState, pin, prescale); + return new ADCSource(_sitlState, pin); } #endif diff --git a/libraries/AP_HAL_AVR_SITL/AnalogIn.h b/libraries/AP_HAL_AVR_SITL/AnalogIn.h index 502e3da707..2a24b6ff81 100644 --- a/libraries/AP_HAL_AVR_SITL/AnalogIn.h +++ b/libraries/AP_HAL_AVR_SITL/AnalogIn.h @@ -12,21 +12,20 @@ public: friend class AVR_SITL::SITLAnalogIn; /* pin designates the ADC input number, or when == AVR_ANALOG_PIN_VCC, * board vcc */ - ADCSource(SITL_State *sitlState, uint8_t pin, float prescale = 1.0); + ADCSource(SITL_State *sitlState, uint8_t pin); /* implement AnalogSource virtual api: */ float read_average(); float read_latest(); void set_pin(uint8_t p); float voltage_average(); + float voltage_average_ratiometric() { return voltage_average(); } void set_stop_pin(uint8_t pin) {} void set_settle_time(uint16_t settle_time_ms) {} private: - /* prescale scales the raw measurments for read()*/ SITL_State *_sitlState; uint8_t _pin; - const float _prescale; }; /* AVRAnalogIn : a concrete class providing the implementations of the @@ -38,7 +37,6 @@ public: } void init(void* ap_hal_scheduler); AP_HAL::AnalogSource* channel(int16_t n); - AP_HAL::AnalogSource* channel(int16_t n, float prescale); private: static ADCSource* _channels[SITL_INPUT_MAX_CHANNELS]; diff --git a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp index b6e62f12a8..3299c19860 100644 --- a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp +++ b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp @@ -31,12 +31,12 @@ using namespace AVR_SITL; uint16_t SITL_State::_airspeed_sensor(float airspeed) { const float airspeed_ratio = 1.9936; - const float airspeed_offset = 2820; + const float airspeed_offset = 2013; float airspeed_pressure, airspeed_raw; airspeed_pressure = (airspeed*airspeed) / airspeed_ratio; airspeed_raw = airspeed_pressure + airspeed_offset; - return airspeed_raw; + return airspeed_raw/4; } diff --git a/libraries/AP_HAL_Empty/AnalogIn.cpp b/libraries/AP_HAL_Empty/AnalogIn.cpp index a449b1bdd6..f8d258064c 100644 --- a/libraries/AP_HAL_Empty/AnalogIn.cpp +++ b/libraries/AP_HAL_Empty/AnalogIn.cpp @@ -33,7 +33,4 @@ AP_HAL::AnalogSource* EmptyAnalogIn::channel(int16_t n) { return new EmptyAnalogSource(1.11); } -AP_HAL::AnalogSource* EmptyAnalogIn::channel(int16_t n, float scale) { - return new EmptyAnalogSource(scale/2); -} diff --git a/libraries/AP_HAL_Empty/AnalogIn.h b/libraries/AP_HAL_Empty/AnalogIn.h index ec7b9f13b7..460f06b863 100644 --- a/libraries/AP_HAL_Empty/AnalogIn.h +++ b/libraries/AP_HAL_Empty/AnalogIn.h @@ -13,6 +13,7 @@ public: void set_stop_pin(uint8_t p); void set_settle_time(uint16_t settle_time_ms); float voltage_average(); + float voltage_average_ratiometric() { return voltage_average(); } private: float _v; }; @@ -22,6 +23,5 @@ public: EmptyAnalogIn(); void init(void* implspecific); AP_HAL::AnalogSource* channel(int16_t n); - AP_HAL::AnalogSource* channel(int16_t n, float scale); }; #endif // __AP_HAL_EMPTY_ANALOGIN_H__ diff --git a/libraries/AP_HAL_PX4/AnalogIn.cpp b/libraries/AP_HAL_PX4/AnalogIn.cpp index 8e2905440d..2deda41890 100644 --- a/libraries/AP_HAL_PX4/AnalogIn.cpp +++ b/libraries/AP_HAL_PX4/AnalogIn.cpp @@ -41,13 +41,12 @@ PX4AnalogSource* PX4AnalogIn::_channels[PX4_ANALOG_MAX_CHANNELS] = {}; int PX4AnalogIn::_battery_handle = -1; uint64_t PX4AnalogIn::_battery_timestamp; -PX4AnalogSource::PX4AnalogSource(int16_t pin, float initial_value, float scale) : +PX4AnalogSource::PX4AnalogSource(int16_t pin, float initial_value) : _pin(pin), _value(initial_value), _latest_value(initial_value), _sum_count(0), - _sum_value(0), - _scale(scale) + _sum_value(0) { } @@ -176,15 +175,10 @@ void PX4AnalogIn::_analogin_timer(uint32_t now) } AP_HAL::AnalogSource* PX4AnalogIn::channel(int16_t pin) -{ - return channel(pin, 1.0); -} - -AP_HAL::AnalogSource* PX4AnalogIn::channel(int16_t pin, float scale) { for (uint8_t j=0; jgroundspeed_vector(); //Calculate groundspeed - float groundSpeed = _groundspeed_vector.length(); + float groundSpeed = _maxf(_groundspeed_vector.length(), 1.0f); // Calculate time varying control parameters // Calculate the L1 length required for specified period @@ -107,46 +107,52 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct Vector2f A_v((prev_WP.lat*1.0e-7f), (prev_WP.lng*1.0e-7f)); Vector2f B_v((next_WP.lat*1.0e-7f), (next_WP.lng*1.0e-7f)); - //Calculate the NE position of the aircraft and WP B relative to WP A - A_air = _geo2planar(A_v, A_air)*RADIUS_OF_EARTH; - Vector2f AB = _geo2planar(A_v, B_v)*RADIUS_OF_EARTH; + // Calculate the NE position of WP B relative to WP A + Vector2f AB = _geo2planar(A_v, B_v); - //Calculate the unit vector from WP A to WP B - Vector2f AB_unit = (AB).normalized(); + // Check for AB zero length and track directly to the destination + // if too small + if (AB.length() < 1.0e-6f) { + AB = _geo2planar(A_air, B_v); + } + AB.normalize(); + + // Calculate the NE position of the aircraft relative to WP A + A_air = _geo2planar(A_v, A_air); // calculate distance to target track, for reporting - _crosstrack_error = AB_unit % A_air; + _crosstrack_error = AB % A_air; - //Determine if the aircraft is behind a +-135 degree degree arc centred on WP A + //Determine if the aircraft is behind a +-135 degree degree arc centred on WP A //and further than L1 distance from WP A. Then use WP A as the L1 reference point - //Otherwise do normal L1 guidance + //Otherwise do normal L1 guidance float WP_A_dist = A_air.length(); - float alongTrackDist = A_air * AB_unit; - if (WP_A_dist > _L1_dist && alongTrackDist/(WP_A_dist + 1.0f) < -0.7071f) { + float alongTrackDist = A_air * AB; + if (WP_A_dist > _L1_dist && alongTrackDist/_maxf(WP_A_dist , 1.0f) < -0.7071f) { - //Calc Nu to fly To WP A + //Calc Nu to fly To WP A Vector2f A_air_unit = (A_air).normalized(); // Unit vector from WP A to aircraft xtrackVel = _groundspeed_vector % (-A_air_unit); // Velocity across line ltrackVel = _groundspeed_vector * (-A_air_unit); // Velocity along line Nu = atan2f(xtrackVel,ltrackVel); _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point - } else { //Calc Nu to fly along AB line - + } else { //Calc Nu to fly along AB line + //Calculate Nu2 angle (angle of velocity vector relative to line connecting waypoints) - xtrackVel = _groundspeed_vector % AB_unit; // Velocity cross track - ltrackVel = _groundspeed_vector * AB_unit; // Velocity along track + xtrackVel = _groundspeed_vector % AB; // Velocity cross track + ltrackVel = _groundspeed_vector * AB; // Velocity along track float Nu2 = atan2f(xtrackVel,ltrackVel); //Calculate Nu1 angle (Angle to L1 reference point) - float xtrackErr = A_air % AB_unit; + float xtrackErr = A_air % AB; float sine_Nu1 = xtrackErr/_maxf(_L1_dist , 0.1f); //Limit sine of Nu1 to provide a controlled track capture angle of 45 deg sine_Nu1 = constrain_float(sine_Nu1, -0.7854f, 0.7854f); float Nu1 = asinf(sine_Nu1); Nu = Nu1 + Nu2; - _nav_bearing = atan2f(AB_unit.y, AB_unit.x) + Nu1; // bearing (radians) from AC to L1 point + _nav_bearing = atan2f(AB.y, AB.x) + Nu1; // bearing (radians) from AC to L1 point } - + //Limit Nu to +-pi Nu = constrain_float(Nu, -1.5708f, +1.5708f); _latAccDem = K_L1 * groundSpeed * groundSpeed / _L1_dist * sinf(Nu); @@ -179,7 +185,7 @@ void AP_L1_Control::update_loiter(const struct Location ¢er_WP, float radius Vector2f _groundspeed_vector = _ahrs->groundspeed_vector(); //Calculate groundspeed - float groundSpeed = _groundspeed_vector.length(); + float groundSpeed = _maxf(_groundspeed_vector.length() , 1.0f); // Calculate time varying control parameters // Calculate the L1 length required for specified period @@ -191,7 +197,7 @@ void AP_L1_Control::update_loiter(const struct Location ¢er_WP, float radius Vector2f A_v((center_WP.lat*1.0e-7f), (center_WP.lng*1.0e-7f)); //Calculate the NE position of the aircraft relative to WP A - A_air = _geo2planar(A_v, A_air)*RADIUS_OF_EARTH; + A_air = _geo2planar(A_v, A_air); //Calculate the unit vector from WP A to aircraft Vector2f A_air_unit = (A_air).normalized(); @@ -307,7 +313,7 @@ Vector2f AP_L1_Control::_geo2planar(const Vector2f &ref, const Vector2f &wp) con out.x=radians((wp.x-ref.x)); out.y=radians((wp.y-ref.y)*cosf(radians(ref.x))); - return out; + return out * RADIUS_OF_EARTH; } float AP_L1_Control::_maxf(const float &num1, const float &num2) const diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp index b26e778d70..79cb539414 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp @@ -120,7 +120,7 @@ float AP_RangeFinder_analog::voltage(void) _source->set_pin(_pin); _source->set_stop_pin((uint8_t)_stop_pin); _source->set_settle_time((uint16_t)_settle_time_ms); - return _source->voltage_average(); + return _source->voltage_average_ratiometric(); } /* diff --git a/mk/PX4/ROMFS/init.d/rc.APM b/mk/PX4/ROMFS/init.d/rc.APM index 5d2a441dfb..4b6e72cbe9 100644 --- a/mk/PX4/ROMFS/init.d/rc.APM +++ b/mk/PX4/ROMFS/init.d/rc.APM @@ -113,8 +113,9 @@ then else echo "No APM sketch found" fi -fi echo "rc.APM finished" echo "rc.APM finished" >> $logfile +fi +