mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
469efb00f6
This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
44 lines
912 B
C++
44 lines
912 B
C++
#include <AP_HAL/AP_HAL.h>
|
|
#include "AP_InertialSensor_HIL.h"
|
|
|
|
const extern AP_HAL::HAL& hal;
|
|
|
|
AP_InertialSensor_HIL::AP_InertialSensor_HIL(AP_InertialSensor &imu) :
|
|
AP_InertialSensor_Backend(imu)
|
|
{
|
|
}
|
|
|
|
/*
|
|
detect the sensor
|
|
*/
|
|
AP_InertialSensor_Backend *AP_InertialSensor_HIL::detect(AP_InertialSensor &_imu)
|
|
{
|
|
AP_InertialSensor_HIL *sensor = new AP_InertialSensor_HIL(_imu);
|
|
if (sensor == nullptr) {
|
|
return nullptr;
|
|
}
|
|
if (!sensor->_init_sensor()) {
|
|
delete sensor;
|
|
return nullptr;
|
|
}
|
|
return sensor;
|
|
}
|
|
|
|
bool AP_InertialSensor_HIL::_init_sensor(void)
|
|
{
|
|
// grab the used instances
|
|
_imu.register_gyro(1200, 1);
|
|
_imu.register_accel(1200, 1);
|
|
|
|
_imu.set_hil_mode();
|
|
|
|
return true;
|
|
}
|
|
|
|
bool AP_InertialSensor_HIL::update(void)
|
|
{
|
|
// the data is stored directly in the frontend, so update()
|
|
// doesn't need to do anything
|
|
return true;
|
|
}
|