AP_InertialSensor: allow to split detection and initialization

This allows backends to have a separate detection and initialization
logic. It doesn't change any backend yet and with the current code
there's no change in behavior either. This only allows
AP_InertialSensor::_detect_backend() to be called earlier so
AP_InertialSensor object can be used by other libraries. If it's not
called, later on AP_InertialSensor::init() will detect and start all
backends.
This commit is contained in:
Lucas De Marchi 2015-08-05 10:01:52 -03:00 committed by Andrew Tridgell
parent f7954ee885
commit 22c787058e
3 changed files with 35 additions and 5 deletions

View File

@ -321,6 +321,7 @@ AP_InertialSensor::AP_InertialSensor() :
_hil_mode(false),
_calibrating(false),
_log_raw_data(false),
_backends_detected(false),
_dataflash(NULL)
{
AP_Param::setup_object_defaults(this, var_info);
@ -369,6 +370,24 @@ uint8_t AP_InertialSensor::register_accel(void)
return _accel_count++;
}
/*
* Start all backends for gyro and accel measurements. It automatically calls
* _detect_backends() if it has not been called already.
*/
void AP_InertialSensor::_start_backends()
{
_detect_backends();
for (uint8_t i = 0; i < _backend_count; i++) {
_backends[i]->start();
}
if (_gyro_count == 0 || _accel_count == 0) {
hal.scheduler->panic(PSTR("INS needs at least 1 gyro and 1 accel"));
}
}
void
AP_InertialSensor::init( Start_style style,
Sample_rate sample_rate)
@ -377,8 +396,7 @@ AP_InertialSensor::init( Start_style style,
_sample_rate = sample_rate;
if (_gyro_count == 0 && _accel_count == 0) {
// detect available backends. Only called once
_detect_backends();
_start_backends();
}
// initialise accel scale if need be. This is needed as we can't
@ -432,6 +450,11 @@ void AP_InertialSensor::_add_backend(AP_InertialSensor_Backend *backend)
void
AP_InertialSensor::_detect_backends(void)
{
if (_backends_detected)
return;
_backends_detected = true;
if (_hil_mode) {
_add_backend(AP_InertialSensor_HIL::detect(*this));
return;
@ -458,9 +481,7 @@ AP_InertialSensor::_detect_backends(void)
#error Unrecognised HAL_INS_TYPE setting
#endif
if (_backend_count == 0 ||
_gyro_count == 0 ||
_accel_count == 0) {
if (_backend_count == 0) {
hal.scheduler->panic(PSTR("No INS backends available"));
}

View File

@ -243,6 +243,7 @@ private:
// load backend drivers
void _add_backend(AP_InertialSensor_Backend *backend);
void _detect_backends(void);
void _start_backends();
// gyro initialisation
void _init_gyro();
@ -334,6 +335,8 @@ private:
// should we log raw accel/gyro data?
bool _log_raw_data:1;
bool _backends_detected:1;
// the delta time in seconds for the last sample
float _delta_time;

View File

@ -52,6 +52,12 @@ public:
*/
virtual bool gyro_sample_available() = 0;
/*
* Configure and start all sensors. The empty implementation allows
* subclasses to already start the sensors when it's detected
*/
virtual void start() { }
/*
return the product ID
*/