2011-12-21 00:30:22 -04:00
|
|
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
2011-11-13 02:42:20 -04:00
|
|
|
|
|
|
|
#include "AP_InertialSensor_Stub.h"
|
2012-10-11 21:27:19 -03:00
|
|
|
#include <AP_HAL.h>
|
|
|
|
const extern AP_HAL::HAL& hal;
|
2011-11-13 02:42:20 -04:00
|
|
|
|
2013-04-12 01:30:35 -03:00
|
|
|
AP_InertialSensor_Stub::AP_InertialSensor_Stub() : AP_InertialSensor() {
|
2013-03-31 05:38:48 -03:00
|
|
|
Vector3f accels;
|
2013-04-04 23:48:41 -03:00
|
|
|
accels.z = -GRAVITY_MSS;
|
2013-03-31 05:38:48 -03:00
|
|
|
set_accel(accels);
|
2013-04-12 01:30:35 -03:00
|
|
|
}
|
2013-03-31 05:38:48 -03:00
|
|
|
|
2012-10-11 21:27:19 -03:00
|
|
|
uint16_t AP_InertialSensor_Stub::_init_sensor( Sample_rate sample_rate ) {
|
2012-12-03 08:27:21 -04:00
|
|
|
switch (sample_rate) {
|
|
|
|
case RATE_50HZ:
|
|
|
|
_sample_period_ms = 20;
|
|
|
|
break;
|
|
|
|
case RATE_100HZ:
|
|
|
|
_sample_period_ms = 10;
|
|
|
|
break;
|
|
|
|
case RATE_200HZ:
|
|
|
|
_sample_period_ms = 5;
|
|
|
|
break;
|
|
|
|
}
|
2012-08-17 03:19:56 -03:00
|
|
|
return AP_PRODUCT_ID_NONE;
|
2012-05-08 23:26:35 -03:00
|
|
|
}
|
2011-11-13 02:42:20 -04:00
|
|
|
|
|
|
|
/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
|
|
|
|
|
2012-08-17 03:19:56 -03:00
|
|
|
bool AP_InertialSensor_Stub::update( void ) {
|
2012-10-11 21:27:19 -03:00
|
|
|
uint32_t now = hal.scheduler->millis();
|
2012-12-03 08:27:21 -04:00
|
|
|
_delta_time_usec = (now - _last_update_ms) * 1000;
|
|
|
|
_last_update_ms = now;
|
2012-08-17 03:19:56 -03:00
|
|
|
return true;
|
|
|
|
}
|
2011-11-13 02:42:20 -04:00
|
|
|
|
2013-01-11 06:17:21 -04:00
|
|
|
float AP_InertialSensor_Stub::get_delta_time() {
|
|
|
|
return _delta_time_usec * 1.0e-6;
|
2012-08-17 03:19:56 -03:00
|
|
|
}
|
2012-11-05 00:27:03 -04:00
|
|
|
uint32_t AP_InertialSensor_Stub::get_last_sample_time_micros() {
|
2012-12-03 08:27:21 -04:00
|
|
|
return _last_update_ms * 1000;
|
2012-08-17 03:19:56 -03:00
|
|
|
}
|
|
|
|
float AP_InertialSensor_Stub::get_gyro_drift_rate(void) {
|
2013-05-04 02:16:35 -03:00
|
|
|
// 0.5 degrees/second/minute
|
|
|
|
return ToRad(0.5/60);
|
2012-08-17 03:19:56 -03:00
|
|
|
}
|
2013-09-26 21:33:08 -03:00
|
|
|
|
|
|
|
bool AP_InertialSensor_Stub::sample_available()
|
2012-09-06 23:47:19 -03:00
|
|
|
{
|
2012-10-11 21:27:19 -03:00
|
|
|
uint16_t ret = (hal.scheduler->millis() - _last_update_ms)
|
|
|
|
/ _sample_period_ms;
|
2012-12-03 08:27:21 -04:00
|
|
|
|
2013-09-26 21:33:08 -03:00
|
|
|
return ret > 0;
|
2012-09-06 23:47:19 -03:00
|
|
|
}
|