From 9f309a2aa6991a1bb18844eee60914c3af018c75 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 13 May 2013 04:20:41 +1000 Subject: [PATCH 01/14] AP_L1_Control: Remove potential nan errors If WP A and B were the same or ground speed was exactly zero, then the previous code would produce a nan output. Protection against these two cases has been added. If WP A and B are equal, we track directly to the target waypoint --- libraries/AP_L1_Control/AP_L1_Control.cpp | 50 +++++++++++++---------- 1 file changed, 28 insertions(+), 22 deletions(-) diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp index f5442bd0ce..9f4847a8c3 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.cpp +++ b/libraries/AP_L1_Control/AP_L1_Control.cpp @@ -95,7 +95,7 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct 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 @@ -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 From a17b85c661e3172167bebf43761f8b1a53ceaa9b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 13 May 2013 11:24:48 +1000 Subject: [PATCH 02/14] AP_AHRS: use vectors for ground vector complimentary filter --- libraries/AP_AHRS/AP_AHRS.cpp | 13 +++++-------- libraries/AP_AHRS/AP_AHRS.h | 11 ++++------- 2 files changed, 9 insertions(+), 15 deletions(-) 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 From dd0630dc16c909796e28bdaa808936c6e35d9488 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 13 May 2013 11:25:40 +1000 Subject: [PATCH 03/14] PX4: fixed handling of nostart option --- mk/PX4/ROMFS/init.d/rc.APM | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 + From 589b8cdb58d9c3101f666d38f979a139f1fdacbe Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 13 May 2013 15:12:43 +1000 Subject: [PATCH 04/14] 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 --- libraries/AP_HAL/AnalogIn.h | 4 ++++ libraries/AP_HAL_AVR/AnalogIn.h | 1 + libraries/AP_HAL_AVR/AnalogIn_ADC.cpp | 11 +++++++++++ libraries/AP_HAL_AVR_SITL/AnalogIn.cpp | 2 +- libraries/AP_HAL_AVR_SITL/AnalogIn.h | 1 + libraries/AP_HAL_AVR_SITL/sitl_ins.cpp | 2 +- libraries/AP_HAL_Empty/AnalogIn.h | 1 + libraries/AP_HAL_PX4/AnalogIn.h | 1 + libraries/AP_HAL_SMACCM/AnalogIn.h | 1 + 9 files changed, 22 insertions(+), 2 deletions(-) diff --git a/libraries/AP_HAL/AnalogIn.h b/libraries/AP_HAL/AnalogIn.h index d64d7c5ab6..57b4947c03 100644 --- a/libraries/AP_HAL/AnalogIn.h +++ b/libraries/AP_HAL/AnalogIn.h @@ -27,6 +27,10 @@ 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 { diff --git a/libraries/AP_HAL_AVR/AnalogIn.h b/libraries/AP_HAL_AVR/AnalogIn.h index b13a36a57f..16e5b4c6a6 100644 --- a/libraries/AP_HAL_AVR/AnalogIn.h +++ b/libraries/AP_HAL_AVR/AnalogIn.h @@ -19,6 +19,7 @@ public: 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); diff --git a/libraries/AP_HAL_AVR/AnalogIn_ADC.cpp b/libraries/AP_HAL_AVR/AnalogIn_ADC.cpp index 7b3825107e..f72f140e1c 100644 --- a/libraries/AP_HAL_AVR/AnalogIn_ADC.cpp +++ b/libraries/AP_HAL_AVR/AnalogIn_ADC.cpp @@ -59,6 +59,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_SITL/AnalogIn.cpp b/libraries/AP_HAL_AVR_SITL/AnalogIn.cpp index 44f36abf44..958d788600 100644 --- a/libraries/AP_HAL_AVR_SITL/AnalogIn.cpp +++ b/libraries/AP_HAL_AVR_SITL/AnalogIn.cpp @@ -22,7 +22,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() { diff --git a/libraries/AP_HAL_AVR_SITL/AnalogIn.h b/libraries/AP_HAL_AVR_SITL/AnalogIn.h index 502e3da707..165745652a 100644 --- a/libraries/AP_HAL_AVR_SITL/AnalogIn.h +++ b/libraries/AP_HAL_AVR_SITL/AnalogIn.h @@ -19,6 +19,7 @@ public: 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) {} diff --git a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp index b6e62f12a8..ddc16d5bba 100644 --- a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp +++ b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp @@ -31,7 +31,7 @@ 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 = 503; float airspeed_pressure, airspeed_raw; airspeed_pressure = (airspeed*airspeed) / airspeed_ratio; diff --git a/libraries/AP_HAL_Empty/AnalogIn.h b/libraries/AP_HAL_Empty/AnalogIn.h index ec7b9f13b7..b26e8209a7 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; }; diff --git a/libraries/AP_HAL_PX4/AnalogIn.h b/libraries/AP_HAL_PX4/AnalogIn.h index 30978d01f6..11076c8d90 100644 --- a/libraries/AP_HAL_PX4/AnalogIn.h +++ b/libraries/AP_HAL_PX4/AnalogIn.h @@ -26,6 +26,7 @@ public: float read_latest(); void set_pin(uint8_t p); float voltage_average(); + float voltage_average_ratiometric() { return voltage_average(); } // stop pins not implemented on PX4 yet void set_stop_pin(uint8_t p) {} diff --git a/libraries/AP_HAL_SMACCM/AnalogIn.h b/libraries/AP_HAL_SMACCM/AnalogIn.h index 8af8fbf331..7b98a2fa05 100644 --- a/libraries/AP_HAL_SMACCM/AnalogIn.h +++ b/libraries/AP_HAL_SMACCM/AnalogIn.h @@ -11,6 +11,7 @@ public: 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 p) {} void set_settle_time(uint16_t settle_time_ms) {} From dfb98490f42c4aec7a69592e682705cfefaf930a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 13 May 2013 15:13:19 +1000 Subject: [PATCH 05/14] AP_Airspeed: use rationmetric analog input for airspeed sensor --- libraries/AP_Airspeed/AP_Airspeed.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) 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; From cec08be9cb170b3ce6aa8a8e8015dd305271591a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 13 May 2013 15:13:43 +1000 Subject: [PATCH 06/14] AP_Rangefinder: use ratiometric analog input for analog sonar --- libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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(); } /* From 48cfdac6f0b1a6d42670ddabb27e3ba5a9f08b3a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 13 May 2013 15:13:59 +1000 Subject: [PATCH 07/14] AP_ADC_AnalogSource: added voltage_average_ratiometric() --- libraries/AP_ADC_AnalogSource/AP_ADC_AnalogSource.h | 1 + 1 file changed, 1 insertion(+) 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) {} From e1ac097e0e0eba2156a3e8ab402f88d18b3b6c97 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 13 May 2013 15:14:23 +1000 Subject: [PATCH 08/14] Rover: use voltage_average() for RSSI this allows it to work on PX4 --- APMrover2/APMrover2.pde | 2 +- APMrover2/sensors.pde | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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); } From 97b6aaac77cdb2725037c76ae907deafd0250df5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 13 May 2013 15:14:56 +1000 Subject: [PATCH 09/14] Plane: use voltage_average() for RSSI this fixes it on PX4 --- ArduPlane/ArduPlane.pde | 5 ++--- ArduPlane/sensors.pde | 2 +- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index ec9b1641c1..5639330f7f 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -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,7 +661,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, 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, 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); } From 001d18b51dea80beb5022225121f3f7280ccbe95 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 13 May 2013 15:15:16 +1000 Subject: [PATCH 10/14] Plane: removed scaling of pitot source --- ArduPlane/ArduPlane.pde | 5 +++-- ArduPlane/config.h | 5 ----- ArduPlane/test.pde | 4 ++-- 3 files changed, 5 insertions(+), 9 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 5639330f7f..f1fc7ba247 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -665,9 +665,10 @@ void setup() { #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/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: ")); From c44fd42349078dfef3ae0fae92c5b4c2fe42b02a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 13 May 2013 15:18:32 +1000 Subject: [PATCH 11/14] Copter: fixed RSSI reading on PX4 needed for different analog input scaling --- ArduCopter/ArduCopter.pde | 5 ++--- ArduCopter/sensors.pde | 2 +- 2 files changed, 3 insertions(+), 4 deletions(-) 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); } From a5b20b4dfcaca1178c92af08021c8d01325fd01c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 13 May 2013 15:29:42 +1000 Subject: [PATCH 12/14] AP_HAL: removed scaling factor on analog sources these are not use anymore, as voltage_average() is used instead --- libraries/AP_HAL/AnalogIn.h | 1 - libraries/AP_HAL_AVR/AnalogIn.h | 8 ++------ libraries/AP_HAL_AVR/AnalogIn_ADC.cpp | 9 ++++----- libraries/AP_HAL_AVR/AnalogIn_Common.cpp | 12 +++++------- libraries/AP_HAL_AVR_SITL/AnalogIn.cpp | 11 +++-------- libraries/AP_HAL_AVR_SITL/AnalogIn.h | 5 +---- libraries/AP_HAL_Empty/AnalogIn.cpp | 3 --- libraries/AP_HAL_Empty/AnalogIn.h | 1 - libraries/AP_HAL_PX4/AnalogIn.cpp | 12 +++--------- libraries/AP_HAL_PX4/AnalogIn.h | 4 +--- libraries/AP_HAL_SMACCM/AnalogIn.cpp | 4 ---- libraries/AP_HAL_SMACCM/AnalogIn.h | 1 - 12 files changed, 19 insertions(+), 52 deletions(-) diff --git a/libraries/AP_HAL/AnalogIn.h b/libraries/AP_HAL/AnalogIn.h index 57b4947c03..fa9f291055 100644 --- a/libraries/AP_HAL/AnalogIn.h +++ b/libraries/AP_HAL/AnalogIn.h @@ -37,7 +37,6 @@ 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 16e5b4c6a6..f4456a8125 100644 --- a/libraries/AP_HAL_AVR/AnalogIn.h +++ b/libraries/AP_HAL_AVR/AnalogIn.h @@ -12,7 +12,7 @@ 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(); @@ -58,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 @@ -70,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 f72f140e1c..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; } } 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 958d788600..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() { @@ -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 165745652a..2a24b6ff81 100644 --- a/libraries/AP_HAL_AVR_SITL/AnalogIn.h +++ b/libraries/AP_HAL_AVR_SITL/AnalogIn.h @@ -12,7 +12,7 @@ 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(); @@ -24,10 +24,8 @@ public: 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 @@ -39,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_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 b26e8209a7..460f06b863 100644 --- a/libraries/AP_HAL_Empty/AnalogIn.h +++ b/libraries/AP_HAL_Empty/AnalogIn.h @@ -23,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; j Date: Mon, 13 May 2013 15:42:38 +1000 Subject: [PATCH 13/14] SITL: fixed simulated airspeed value --- libraries/AP_HAL_AVR_SITL/sitl_ins.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp index ddc16d5bba..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 = 503; + 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; } From bd4b7d136fbf339a03dbbd2c525537ce5e6d23c4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 13 May 2013 17:32:33 +1000 Subject: [PATCH 14/14] Plane: mark ArduPlane 2.74beta --- ArduPlane/ArduPlane.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index f1fc7ba247..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 *