Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: added support for multiple accel/gyro devices

Browse Source 
this makes it possible to ask for the gyro and accel vectors from
secondary INS devices.
This commit is contained in:
Andrew Tridgell 2013-12-08 20:43:53 +11:00
parent 145a8ed128
commit 2753449e75
17 changed files with 78 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -588,5 +588,42 @@ void AP_InertialSensor::_calculate_trim(Vector3f accel_sample, float& trim_roll,
}
}
/**
default versions of multi-device accessor functions
*/
bool AP_InertialSensor::get_gyro_instance_health(uint8_t instance) const
{
if (instance != 0) {
return false;
}
return healthy();
}
bool AP_InertialSensor::get_gyro_instance(uint8_t instance, Vector3f &gyro) const
{
if (instance != 0) {
return false;
}
gyro = get_gyro();
return true;
}
bool AP_InertialSensor::get_accel_instance_health(uint8_t instance) const
{
if (instance != 0) {
return false;
}
return healthy();
}
bool AP_InertialSensor::get_accel_instance(uint8_t instance, Vector3f &accel) const
{
if (instance != 0) {
return false;
}
accel = get_accel();
return true;
}
#endif // __AVR_ATmega1280__

14
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -97,6 +97,15 @@ public:
const Vector3f &get_accel(void) const { return _accel; }
virtual void set_accel(const Vector3f &accel) {}
// multi-device interface
virtual bool get_gyro_instance_health(uint8_t instance) const;
virtual uint8_t get_gyro_count(void) const { return 1; };
virtual bool get_gyro_instance(uint8_t instance, Vector3f &gyro) const;
virtual bool get_accel_instance_health(uint8_t instance) const;
virtual uint8_t get_accel_count(void) const { return 1; };
virtual bool get_accel_instance(uint8_t instance, Vector3f &accel) const;
// get accel offsets in m/s/s
Vector3f get_accel_offsets() { return _accel_offset; }
void set_accel_offsets(Vector3f offsets) { _accel_offset.set(offsets); }
@ -118,9 +127,6 @@ public:
// depends on what gyro chips are being used
virtual float get_gyro_drift_rate(void) = 0;
// true if a new sample is available from the sensors
virtual bool sample_available() = 0;
// wait for a sample to be available, with timeout in milliseconds
virtual bool wait_for_sample(uint16_t timeout_ms) = 0;
@ -140,7 +146,7 @@ public:
}
virtual uint16_t error_count(void) const { return 0; }
virtual bool healthy(void) { return true; }
virtual bool healthy(void) const { return true; }
protected:

6
libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp
View File

@ -295,7 +295,7 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
i2c_sem->give();
}
bool AP_InertialSensor_Flymaple::sample_available(void)
bool AP_InertialSensor_Flymaple::_sample_available(void)
{
_accumulate();
return min(_accel_samples, _gyro_samples) / _sample_divider > 0;
@ -303,13 +303,13 @@ bool AP_InertialSensor_Flymaple::sample_available(void)
bool AP_InertialSensor_Flymaple::wait_for_sample(uint16_t timeout_ms)
{
if (sample_available()) {
if (_sample_available()) {
return true;
}
uint32_t start = hal.scheduler->millis();
while ((hal.scheduler->millis() - start) < timeout_ms) {
hal.scheduler->delay_microseconds(100);
if (sample_available()) {
if (_sample_available()) {
return true;
}
}

2
libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h
View File

@ -21,12 +21,12 @@ public:
bool update();
float get_delta_time();
float get_gyro_drift_rate();
bool sample_available();
bool wait_for_sample(uint16_t timeout_ms);
private:
uint16_t _init_sensor( Sample_rate sample_rate );
static void _accumulate(void);
bool _sample_available();
uint64_t _last_update_usec;
float _delta_time;
static Vector3f _accel_filtered;

8
libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp
View File

@ -41,7 +41,7 @@ float AP_InertialSensor_HIL::get_gyro_drift_rate(void) {
return ToRad(0.5/60);
}
bool AP_InertialSensor_HIL::sample_available()
bool AP_InertialSensor_HIL::_sample_available()
{
uint16_t ret = (hal.scheduler->millis() - _last_update_ms)
/ _sample_period_ms;
@ -51,13 +51,13 @@ bool AP_InertialSensor_HIL::sample_available()
bool AP_InertialSensor_HIL::wait_for_sample(uint16_t timeout_ms)
{
if (sample_available()) {
if (_sample_available()) {
return true;
}
uint32_t start = hal.scheduler->millis();
while ((hal.scheduler->millis() - start) < timeout_ms) {
hal.scheduler->delay(1);
if (sample_available()) {
if (_sample_available()) {
return true;
}
}
@ -80,7 +80,7 @@ void AP_InertialSensor_HIL::set_gyro(const Vector3f &gyro)
/**
try to detect bad accel/gyro sensors
*/
bool AP_InertialSensor_HIL::healthy(void)
bool AP_InertialSensor_HIL::healthy(void) const
{
uint32_t tnow = hal.scheduler->micros();
if ((tnow - _last_accel_usec) > 40000) {

6
libraries/AP_InertialSensor/AP_InertialSensor_HIL.h
View File

@ -16,13 +16,13 @@ public:
bool update();
float get_delta_time();
float get_gyro_drift_rate();
bool sample_available();
bool wait_for_sample(uint16_t timeout_ms);
void set_accel(const Vector3f &accel);
void set_gyro(const Vector3f &gyro);
bool healthy(void);
bool healthy(void) const;
protected:
private:
bool _sample_available();
uint16_t _init_sensor( Sample_rate sample_rate );
uint32_t _sample_period_ms;
uint32_t _last_update_ms;

6
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
View File

@ -344,14 +344,14 @@ void AP_InertialSensor_L3G4200D::_accumulate(void)
i2c_sem->give();
}
bool AP_InertialSensor_L3G4200D::sample_available(void)
bool AP_InertialSensor_L3G4200D::_sample_available(void)
{
return (_gyro_samples_available >= _gyro_samples_needed);
}
bool AP_InertialSensor_L3G4200D::wait_for_sample(uint16_t timeout_ms)
{
if (sample_available()) {
if (_sample_available()) {
_last_sample_time = hal.scheduler->micros();
return true;
}
@ -359,7 +359,7 @@ bool AP_InertialSensor_L3G4200D::wait_for_sample(uint16_t timeout_ms)
while ((hal.scheduler->millis() - start) < timeout_ms) {
hal.scheduler->delay_microseconds(100);
_accumulate();
if (sample_available()) {
if (_sample_available()) {
_last_sample_time = hal.scheduler->micros();
return true;
}

2
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h
View File

@ -21,12 +21,12 @@ public:
bool update();
float get_delta_time();
float get_gyro_drift_rate();
bool sample_available();
bool wait_for_sample(uint16_t timeout_ms);
private:
uint16_t _init_sensor( Sample_rate sample_rate );
void _accumulate(void);
bool _sample_available();
uint64_t _last_update_usec;
Vector3f _accel_filtered;
Vector3f _gyro_filtered;

8
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -235,13 +235,13 @@ uint16_t AP_InertialSensor_MPU6000::_init_sensor( Sample_rate sample_rate )
bool AP_InertialSensor_MPU6000::wait_for_sample(uint16_t timeout_ms)
{
if (sample_available()) {
if (_sample_available()) {
return true;
}
uint32_t start = hal.scheduler->millis();
while ((hal.scheduler->millis() - start) < timeout_ms) {
hal.scheduler->delay_microseconds(100);
if (sample_available()) {
if (_sample_available()) {
return true;
}
}
@ -319,7 +319,7 @@ void AP_InertialSensor_MPU6000::_poll_data(void)
if (!_spi_sem->take_nonblocking()) {
/*
the semaphore being busy is an expected condition when the
mainline code is calling sample_available() which will
mainline code is calling wait_for_sample() which will
grab the semaphore. We return now and rely on the mainline
code grabbing the latest sample.
*/
@ -563,7 +563,7 @@ float AP_InertialSensor_MPU6000::get_gyro_drift_rate(void)
}
// return true if a sample is available
bool AP_InertialSensor_MPU6000::sample_available()
bool AP_InertialSensor_MPU6000::_sample_available()
{
_poll_data();
return (_sum_count >> _sample_shift) > 0;

6
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -24,9 +24,6 @@ public:
bool update();
float get_gyro_drift_rate();
// sample_available - true when a new sample is available
bool sample_available();
// wait for a sample to be available, with timeout in milliseconds
bool wait_for_sample(uint16_t timeout_ms);
@ -34,7 +31,7 @@ public:
float get_delta_time();
uint16_t error_count(void) const { return _error_count; }
bool healthy(void) { return _error_count <= 4; }
bool healthy(void) const { return _error_count <= 4; }
protected:
uint16_t _init_sensor( Sample_rate sample_rate );
@ -42,6 +39,7 @@ protected:
private:
AP_HAL::DigitalSource *_drdy_pin;
bool _sample_available();
void _read_data_transaction();
bool _data_ready();
void _poll_data(void);

6
libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
View File

@ -124,20 +124,20 @@ float AP_InertialSensor_Oilpan::get_gyro_drift_rate(void)
}
// return true if a new sample is available
bool AP_InertialSensor_Oilpan::sample_available()
bool AP_InertialSensor_Oilpan::_sample_available()
{
return (_adc->num_samples_available(_sensors) / _sample_threshold) > 0;
}
bool AP_InertialSensor_Oilpan::wait_for_sample(uint16_t timeout_ms)
{
if (sample_available()) {
if (_sample_available()) {
return true;
}
uint32_t start = hal.scheduler->millis();
while ((hal.scheduler->millis() - start) < timeout_ms) {
hal.scheduler->delay_microseconds(100);
if (sample_available()) {
if (_sample_available()) {
return true;
}
}

5
libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h
View File

@ -20,9 +20,6 @@ public:
float get_delta_time(); // get_delta_time returns the time period in seconds overwhich the sensor data was collected
float get_gyro_drift_rate();
// sample_available() - true when a new sample is available
bool sample_available();
// wait for a sample to be available, with timeout in milliseconds
bool wait_for_sample(uint16_t timeout_ms);
@ -31,6 +28,8 @@ protected:
private:
bool _sample_available();
AP_ADC * _adc;
float _temp;

2
libraries/AP_InertialSensor/examples/Flymaple/Flymaple.pde
View File

@ -165,7 +165,7 @@ void run_test()
while( !hal.console->available() ) {
// wait until we have a sample
while (ins.sample_available() == false) /* noop */ ;
ins.wait_for_sample();
// read samples from ins
ins.update();

2
libraries/AP_InertialSensor/examples/L3G4200D/L3G4200D.pde
View File

@ -166,7 +166,7 @@ void run_test()
while( !hal.console->available() ) {
// wait until we have a sample
while (ins.sample_available() == false) hal.scheduler->delay(1);
ins.wait_for_sample();
// read samples from ins
ins.update();

2
libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde
View File

@ -170,7 +170,7 @@ void run_test()
while( !hal.console->available() ) {
// wait until we have a sample
while (ins.sample_available() == false) /* noop */ ;
ins.wait_for_sample();
// read samples from ins
ins.update();

2
libraries/AP_InertialSensor/examples/OilPan/OilPan.pde
View File

@ -174,7 +174,7 @@ void run_test()
while( !hal.console->available() ) {
// wait until we have a sample
while (ins.sample_available() == false) /* noop */ ;
ins.wait_for_sample();
// read samples from ins
ins.update();

4
libraries/AP_InertialSensor/examples/PX4/PX4.pde
View File

@ -164,9 +164,7 @@ void run_test()
while( !hal.console->available() ) {
// wait until we have a sample
while (ins.sample_available() == false) {
hal.scheduler->delay(1);
}
ins.wait_for_sample();
// read samples from ins
ins.update();