2015-08-11 03:28:43 -03:00
|
|
|
#include <AP_HAL/AP_HAL.h>
|
2014-10-15 02:37:59 -03:00
|
|
|
#include "AP_InertialSensor_HIL.h"
|
|
|
|
|
2012-10-11 21:27:19 -03:00
|
|
|
const extern AP_HAL::HAL& hal;
|
2011-11-13 02:42:20 -04:00
|
|
|
|
2014-10-15 05:54:30 -03:00
|
|
|
AP_InertialSensor_HIL::AP_InertialSensor_HIL(AP_InertialSensor &imu) :
|
2014-10-16 17:52:21 -03:00
|
|
|
AP_InertialSensor_Backend(imu)
|
2014-08-13 10:59:22 -03:00
|
|
|
{
|
2013-04-12 01:30:35 -03:00
|
|
|
}
|
2013-03-31 05:38:48 -03:00
|
|
|
|
2014-10-15 02:37:59 -03:00
|
|
|
/*
|
|
|
|
detect the sensor
|
|
|
|
*/
|
2014-10-16 17:52:21 -03:00
|
|
|
AP_InertialSensor_Backend *AP_InertialSensor_HIL::detect(AP_InertialSensor &_imu)
|
2014-10-15 02:37:59 -03:00
|
|
|
{
|
2014-10-15 05:54:30 -03:00
|
|
|
AP_InertialSensor_HIL *sensor = new AP_InertialSensor_HIL(_imu);
|
2016-10-30 02:24:21 -03:00
|
|
|
if (sensor == nullptr) {
|
|
|
|
return nullptr;
|
2014-10-15 02:37:59 -03:00
|
|
|
}
|
2014-10-16 17:52:21 -03:00
|
|
|
if (!sensor->_init_sensor()) {
|
2014-10-15 02:37:59 -03:00
|
|
|
delete sensor;
|
2016-10-30 02:24:21 -03:00
|
|
|
return nullptr;
|
2014-10-15 02:37:59 -03:00
|
|
|
}
|
|
|
|
return sensor;
|
|
|
|
}
|
|
|
|
|
2014-10-16 17:52:21 -03:00
|
|
|
bool AP_InertialSensor_HIL::_init_sensor(void)
|
2014-10-15 02:37:59 -03:00
|
|
|
{
|
|
|
|
// grab the used instances
|
2021-03-17 22:36:41 -03:00
|
|
|
uint8_t instance;
|
|
|
|
_imu.register_gyro(instance, 1200, 1);
|
|
|
|
_imu.register_accel(instance, 1200, 1);
|
2011-11-13 02:42:20 -04:00
|
|
|
|
2014-10-24 01:05:44 -03:00
|
|
|
_imu.set_hil_mode();
|
2014-10-15 23:14:56 -03:00
|
|
|
|
2012-08-17 03:19:56 -03:00
|
|
|
return true;
|
|
|
|
}
|
2011-11-13 02:42:20 -04:00
|
|
|
|
2014-10-15 02:37:59 -03:00
|
|
|
bool AP_InertialSensor_HIL::update(void)
|
2012-09-06 23:47:19 -03:00
|
|
|
{
|
2014-10-15 20:31:55 -03:00
|
|
|
// the data is stored directly in the frontend, so update()
|
|
|
|
// doesn't need to do anything
|
2014-10-15 02:37:59 -03:00
|
|
|
return true;
|
2014-02-23 04:10:07 -04:00
|
|
|
}
|