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

AP_InertialSensor: Add support to get auxiliary bus of multiple instances of the same sensor

This commit is contained in:
José Roberto de Souza 2015-10-02 15:45:21 -03:00 committed by Andrew Tridgell
parent 3e1b974fd2
commit 588df53429
2 changed files with 14 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -405,10 +405,11 @@ void AP_InertialSensor::_start_backends()
}
}
/* Find a backend that has already been succesfully detected */
AP_InertialSensor_Backend *AP_InertialSensor::_find_backend(int16_t backend_id)
/* Find the N instance of the backend that has already been successfully detected */
AP_InertialSensor_Backend *AP_InertialSensor::_find_backend(int16_t backend_id, uint8_t instance)
{
assert(_backends_detected);
uint8_t found = 0;
for (uint8_t i = 0; i < _backend_count; i++) {
int16_t id = _backends[i]->get_id();
@ -416,7 +417,11 @@ AP_InertialSensor_Backend *AP_InertialSensor::_find_backend(int16_t backend_id)
if (id < 0 || id != backend_id)
continue;
return _backends[i];
if (instance == found) {
return _backends[i];
} else {
found++;
}
}
return nullptr;
@ -1539,13 +1544,13 @@ void AP_InertialSensor::set_delta_angle(uint8_t instance, const Vector3f &deltaa
}
/*
* Get an AuxiliaryBus on the backend identified by @backend_id
* Get an AuxiliaryBus of N @instance of backend identified by @backend_id
*/
AuxiliaryBus *AP_InertialSensor::get_auxiliary_bus(int16_t backend_id)
AuxiliaryBus *AP_InertialSensor::get_auxiliary_bus(int16_t backend_id, uint8_t instance)
{
_detect_backends();
AP_InertialSensor_Backend *backend = _find_backend(backend_id);
AP_InertialSensor_Backend *backend = _find_backend(backend_id, instance);
if (backend == NULL)
return NULL;

5
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -225,7 +225,8 @@ public:
void set_delta_velocity(uint8_t instance, float deltavt, const Vector3f &deltav);
void set_delta_angle(uint8_t instance, const Vector3f &deltaa);
AuxiliaryBus *get_auxiliary_bus(int16_t backend_id);
AuxiliaryBus *get_auxiliary_bus(int16_t backend_id) { return get_auxiliary_bus(backend_id, 0); }
AuxiliaryBus *get_auxiliary_bus(int16_t backend_id, uint8_t instance);
private:
@ -233,7 +234,7 @@ private:
void _add_backend(AP_InertialSensor_Backend *backend);
void _detect_backends(void);
void _start_backends();
AP_InertialSensor_Backend *_find_backend(int16_t backend_id);
AP_InertialSensor_Backend *_find_backend(int16_t backend_id, uint8_t instance);
// gyro initialisation
void _init_gyro();