mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-12 02:48:28 -04:00
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:
parent
f7954ee885
commit
22c787058e
libraries/AP_InertialSensor
@ -321,6 +321,7 @@ AP_InertialSensor::AP_InertialSensor() :
|
|||||||
_hil_mode(false),
|
_hil_mode(false),
|
||||||
_calibrating(false),
|
_calibrating(false),
|
||||||
_log_raw_data(false),
|
_log_raw_data(false),
|
||||||
|
_backends_detected(false),
|
||||||
_dataflash(NULL)
|
_dataflash(NULL)
|
||||||
{
|
{
|
||||||
AP_Param::setup_object_defaults(this, var_info);
|
AP_Param::setup_object_defaults(this, var_info);
|
||||||
@ -369,6 +370,24 @@ uint8_t AP_InertialSensor::register_accel(void)
|
|||||||
return _accel_count++;
|
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
|
void
|
||||||
AP_InertialSensor::init( Start_style style,
|
AP_InertialSensor::init( Start_style style,
|
||||||
Sample_rate sample_rate)
|
Sample_rate sample_rate)
|
||||||
@ -377,8 +396,7 @@ AP_InertialSensor::init( Start_style style,
|
|||||||
_sample_rate = sample_rate;
|
_sample_rate = sample_rate;
|
||||||
|
|
||||||
if (_gyro_count == 0 && _accel_count == 0) {
|
if (_gyro_count == 0 && _accel_count == 0) {
|
||||||
// detect available backends. Only called once
|
_start_backends();
|
||||||
_detect_backends();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// initialise accel scale if need be. This is needed as we can't
|
// 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
|
void
|
||||||
AP_InertialSensor::_detect_backends(void)
|
AP_InertialSensor::_detect_backends(void)
|
||||||
{
|
{
|
||||||
|
if (_backends_detected)
|
||||||
|
return;
|
||||||
|
|
||||||
|
_backends_detected = true;
|
||||||
|
|
||||||
if (_hil_mode) {
|
if (_hil_mode) {
|
||||||
_add_backend(AP_InertialSensor_HIL::detect(*this));
|
_add_backend(AP_InertialSensor_HIL::detect(*this));
|
||||||
return;
|
return;
|
||||||
@ -458,9 +481,7 @@ AP_InertialSensor::_detect_backends(void)
|
|||||||
#error Unrecognised HAL_INS_TYPE setting
|
#error Unrecognised HAL_INS_TYPE setting
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
if (_backend_count == 0 ||
|
if (_backend_count == 0) {
|
||||||
_gyro_count == 0 ||
|
|
||||||
_accel_count == 0) {
|
|
||||||
hal.scheduler->panic(PSTR("No INS backends available"));
|
hal.scheduler->panic(PSTR("No INS backends available"));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -243,6 +243,7 @@ private:
|
|||||||
// load backend drivers
|
// load backend drivers
|
||||||
void _add_backend(AP_InertialSensor_Backend *backend);
|
void _add_backend(AP_InertialSensor_Backend *backend);
|
||||||
void _detect_backends(void);
|
void _detect_backends(void);
|
||||||
|
void _start_backends();
|
||||||
|
|
||||||
// gyro initialisation
|
// gyro initialisation
|
||||||
void _init_gyro();
|
void _init_gyro();
|
||||||
@ -334,6 +335,8 @@ private:
|
|||||||
// should we log raw accel/gyro data?
|
// should we log raw accel/gyro data?
|
||||||
bool _log_raw_data:1;
|
bool _log_raw_data:1;
|
||||||
|
|
||||||
|
bool _backends_detected:1;
|
||||||
|
|
||||||
// the delta time in seconds for the last sample
|
// the delta time in seconds for the last sample
|
||||||
float _delta_time;
|
float _delta_time;
|
||||||
|
|
||||||
|
@ -52,6 +52,12 @@ public:
|
|||||||
*/
|
*/
|
||||||
virtual bool gyro_sample_available() = 0;
|
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
|
return the product ID
|
||||||
*/
|
*/
|
||||||
|
Loading…
Reference in New Issue
Block a user