Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of https://github.com/diydrones/ardupilot

This commit is contained in:
Randy Mackay 2013-05-13 18:01:53 +09:00
parent bb7aec2600 bd4b7d136f
commit 94a05a9285
28 changed files with 102 additions and 119 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
APMrover2/APMrover2.pde
View File

@ -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);

2
APMrover2/sensors.pde
View File

@ -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);
}

5
ArduCopter/ArduCopter.pde
View File

@ -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);

2
ArduCopter/sensors.pde
View File

@ -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);
}

12
ArduPlane/ArduPlane.pde
View File

@ -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);

5
ArduPlane/config.h
View File

@ -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

2
ArduPlane/sensors.pde
View File

@ -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);
}

4
ArduPlane/test.pde
View File

@ -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: "));

1
libraries/AP_ADC_AnalogSource/AP_ADC_AnalogSource.h
View File

@ -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) {}

13
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -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) {

11
libraries/AP_AHRS/AP_AHRS.h
View File

@ -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 <AP_AHRS_DCM.h>
#include <AP_AHRS_MPU6000.h>

10
libraries/AP_Airspeed/AP_Airspeed.cpp
View File

@ -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;

5
libraries/AP_HAL/AnalogIn.h
View File

@ -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

9
libraries/AP_HAL_AVR/AnalogIn.h
View File

@ -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];

20
libraries/AP_HAL_AVR/AnalogIn_ADC.cpp
View File

@ -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;
}

12
libraries/AP_HAL_AVR/AnalogIn_Common.cpp
View File

@ -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);
}
}

13
libraries/AP_HAL_AVR_SITL/AnalogIn.cpp
View File

@ -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

6
libraries/AP_HAL_AVR_SITL/AnalogIn.h
View File

@ -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];

4
libraries/AP_HAL_AVR_SITL/sitl_ins.cpp
View File

@ -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;
}

3
libraries/AP_HAL_Empty/AnalogIn.cpp
View File

@ -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);
}

2
libraries/AP_HAL_Empty/AnalogIn.h
View File

@ -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__

12
libraries/AP_HAL_PX4/AnalogIn.cpp
View File

@ -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<PX4_ANALOG_MAX_CHANNELS; j++) {
if (_channels[j] == NULL) {
_channels[j] = new PX4AnalogSource(pin, 0.0, scale);
_channels[j] = new PX4AnalogSource(pin, 0.0);
return _channels[j];
}
}

5
libraries/AP_HAL_PX4/AnalogIn.h
View File

@ -21,11 +21,12 @@
class PX4::PX4AnalogSource : public AP_HAL::AnalogSource {
public:
friend class PX4::PX4AnalogIn;
PX4AnalogSource(int16_t pin, float initial_value, float scale);
PX4AnalogSource(int16_t pin, float initial_value);
float read_average();
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) {}
@ -40,7 +41,6 @@ private:
float _latest_value;
uint8_t _sum_count;
float _sum_value;
float _scale;
void _add_value(float v);
};
@ -49,7 +49,6 @@ public:
PX4AnalogIn();
void init(void* implspecific);
AP_HAL::AnalogSource* channel(int16_t pin);
AP_HAL::AnalogSource* channel(int16_t pin, float scale);
private:
static int _adc_fd;

4
libraries/AP_HAL_SMACCM/AnalogIn.cpp
View File

@ -34,7 +34,3 @@ AP_HAL::AnalogSource* SMACCMAnalogIn::channel(int16_t n) {
return new SMACCMAnalogSource(1.11);
}
AP_HAL::AnalogSource* SMACCMAnalogIn::channel(int16_t n, float scale) {
return new SMACCMAnalogSource(scale/2);
}

2
libraries/AP_HAL_SMACCM/AnalogIn.h
View File

@ -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) {}
@ -23,6 +24,5 @@ public:
SMACCMAnalogIn();
void init(void* implspecific);
AP_HAL::AnalogSource* channel(int16_t n);
AP_HAL::AnalogSource* channel(int16_t n, float scale);
};
#endif // __AP_HAL_SMACCM_ANALOGIN_H__

50
libraries/AP_L1_Control/AP_L1_Control.cpp
View File

@ -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 &center_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 &center_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

2
libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp
View File

@ -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();
}
/*

3
mk/PX4/ROMFS/init.d/rc.APM
View File

@ -113,8 +113,9 @@ then
else
echo "No APM sketch found"
fi
fi
echo "rc.APM finished"
echo "rc.APM finished" >> $logfile
fi