Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: converted HIL backend, which gets SITL working

This commit is contained in:
Andrew Tridgell 2014-10-15 16:37:59 +11:00
parent 448efc70a3
commit ec11417705
4 changed files with 86 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -300,7 +300,13 @@ AP_InertialSensor::init( Start_style style,
void
AP_InertialSensor::_detect_backends(Sample_rate sample_rate)
{
#if HAL_INS_DEFAULT == HAL_INS_HIL
_backends[0] = AP_InertialSensor_HIL::detect(*this, sample_rate, _gyro[_gyro_count], _accel[_accel_count]);
#elif HAL_INS_DEFAULT == HAL_INS_MPU6000
_backends[0] = AP_InertialSensor_MPU6000::detect(*this, sample_rate, _gyro[_gyro_count], _accel[_accel_count]);
#else
#error Unrecognised HAL_INS_TYPE setting
#endif
if (_backends[0] == NULL ||
_gyro_count == 0 ||
_accel_count == 0) {
@ -917,9 +923,27 @@ void AP_InertialSensor::wait_for_sample(void)
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_backends[i] != NULL) {
gyro_available |= _backends[i]->gyro_sample_available();
accel_available |= _backends[i]->gyro_sample_available();
accel_available |= _backends[i]->accel_sample_available();
}
}
hal.scheduler->delay_microseconds(100);
hal.scheduler->delay(1);
}
}
/*
support for setting accel and gyro vectors, for use by HIL
*/
void AP_InertialSensor::set_accel(uint8_t instance, const Vector3f &accel)
{
if (instance < INS_MAX_INSTANCES) {
_accel[instance] = accel;
}
}
void AP_InertialSensor::set_gyro(uint8_t instance, const Vector3f &gyro)
{
if (instance < INS_MAX_INSTANCES) {
_gyro[instance] = gyro;
}
}

1
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -251,6 +251,7 @@ private:
#include "AP_InertialSensor_Backend.h"
#include "AP_InertialSensor_MPU6000.h"
#include "AP_InertialSensor_HIL.h"
#include "AP_InertialSensor_UserInteract_Stream.h"
#include "AP_InertialSensor_UserInteract_MAVLink.h"

141
libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp
View File

@ -1,132 +1,71 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if NOT_YET
#include "AP_InertialSensor_HIL.h"
#include <AP_HAL.h>
#include "AP_InertialSensor_HIL.h"
const extern AP_HAL::HAL& hal;
AP_InertialSensor_HIL::AP_InertialSensor_HIL() :
AP_InertialSensor(),
AP_InertialSensor_HIL::AP_InertialSensor_HIL(AP_InertialSensor &imu, Vector3f &gyro, Vector3f &accel) :
AP_InertialSensor_Backend(imu, gyro, accel),
_sample_period_usec(0),
_last_sample_usec(0)
{
_accel[0] = Vector3f(0, 0, -GRAVITY_MSS);
}
uint16_t AP_InertialSensor_HIL::_init_sensor( Sample_rate sample_rate ) {
/*
detect the sensor
*/
AP_InertialSensor_Backend *AP_InertialSensor_HIL::detect(AP_InertialSensor &_imu,
AP_InertialSensor::Sample_rate sample_rate,
Vector3f &gyro,
Vector3f &accel)
{
AP_InertialSensor_HIL *sensor = new AP_InertialSensor_HIL(_imu, gyro, accel);
if (sensor == NULL) {
return NULL;
}
if (!sensor->_init_sensor(sample_rate)) {
delete sensor;
return NULL;
}
return sensor;
}
bool AP_InertialSensor_HIL::_init_sensor(AP_InertialSensor::Sample_rate sample_rate)
{
switch (sample_rate) {
case RATE_50HZ:
case AP_InertialSensor::RATE_50HZ:
_sample_period_usec = 20000;
break;
case RATE_100HZ:
case AP_InertialSensor::RATE_100HZ:
_sample_period_usec = 10000;
break;
case RATE_200HZ:
case AP_InertialSensor::RATE_200HZ:
_sample_period_usec = 5000;
break;
case RATE_400HZ:
case AP_InertialSensor::RATE_400HZ:
_sample_period_usec = 2500;
break;
}
return AP_PRODUCT_ID_NONE;
}
/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
// grab the used instances
_imu.register_gyro();
_imu.register_accel();
bool AP_InertialSensor_HIL::update( void ) {
uint32_t now = hal.scheduler->micros();
_last_sample_usec = now;
return true;
}
float AP_InertialSensor_HIL::get_delta_time() const {
return _sample_period_usec * 1.0e-6f;
}
float AP_InertialSensor_HIL::get_gyro_drift_rate(void) {
// 0.5 degrees/second/minute
return ToRad(0.5/60);
bool AP_InertialSensor_HIL::update(void)
{
uint32_t now = hal.scheduler->micros();
while (now - _last_sample_usec > _sample_period_usec) {
_last_sample_usec += _sample_period_usec;
}
return true;
}
bool AP_InertialSensor_HIL::_sample_available()
{
uint32_t tnow = hal.scheduler->micros();
bool have_sample = false;
while (tnow - _last_sample_usec > _sample_period_usec) {
have_sample = true;
_last_sample_usec += _sample_period_usec;
}
return have_sample;
return (hal.scheduler->micros() - _last_sample_usec > _sample_period_usec);
}
bool AP_InertialSensor_HIL::wait_for_sample(uint16_t timeout_ms)
{
if (_sample_available()) {
return true;
}
uint32_t start = hal.scheduler->millis();
while ((hal.scheduler->millis() - start) < timeout_ms) {
uint32_t tnow = hal.scheduler->micros();
uint32_t tdelay = (_last_sample_usec + _sample_period_usec) - tnow;
if (tdelay < 100000) {
hal.scheduler->delay_microseconds(tdelay);
}
if (_sample_available()) {
return true;
}
}
return false;
}
void AP_InertialSensor_HIL::set_accel(uint8_t instance, const Vector3f &accel)
{
if (instance >= INS_MAX_INSTANCES) {
return;
}
_previous_accel[instance] = _accel[instance];
_accel[instance] = accel;
_last_accel_usec[instance] = hal.scheduler->micros();
}
void AP_InertialSensor_HIL::set_gyro(uint8_t instance, const Vector3f &gyro)
{
if (instance >= INS_MAX_INSTANCES) {
return;
}
_gyro[instance] = gyro;
_last_gyro_usec[instance] = hal.scheduler->micros();
}
bool AP_InertialSensor_HIL::get_gyro_health(uint8_t instance) const
{
if (instance >= INS_MAX_INSTANCES) {
return false;
}
return (hal.scheduler->micros() - _last_gyro_usec[instance]) < 40000;
}
bool AP_InertialSensor_HIL::get_accel_health(uint8_t instance) const
{
if (instance >= INS_MAX_INSTANCES) {
return false;
}
return (hal.scheduler->micros() - _last_accel_usec[instance]) < 40000;
}
uint8_t AP_InertialSensor_HIL::get_gyro_count(void) const
{
if (get_gyro_health(1)) {
return 2;
}
return 1;
}
uint8_t AP_InertialSensor_HIL::get_accel_count(void) const
{
if (get_accel_health(1)) {
return 2;
}
return 1;
}
#endif

42
libraries/AP_InertialSensor/AP_InertialSensor_HIL.h
View File

@ -1,36 +1,32 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef __AP_INERTIAL_SENSOR_STUB_H__
#define __AP_INERTIAL_SENSOR_STUB_H__
#ifndef __AP_INERTIALSENSOR_HIL_H__
#define __AP_INERTIALSENSOR_HIL_H__
#include <AP_Progmem.h>
#include "AP_InertialSensor.h"
class AP_InertialSensor_HIL : public AP_InertialSensor
class AP_InertialSensor_HIL : public AP_InertialSensor_Backend
{
public:
AP_InertialSensor_HIL(AP_InertialSensor &imu, Vector3f &gyro, Vector3f &accel);
AP_InertialSensor_HIL();
/* update accel and gyro state */
bool update();
/* Concrete implementation of AP_InertialSensor functions: */
bool update();
float get_delta_time() const;
float get_gyro_drift_rate();
bool wait_for_sample(uint16_t timeout_ms);
void set_accel(uint8_t instance, const Vector3f &accel);
void set_gyro(uint8_t instance, const Vector3f &gyro);
bool get_gyro_health(uint8_t instance) const;
bool get_accel_health(uint8_t instance) const;
uint8_t get_gyro_count(void) const;
uint8_t get_accel_count(void) const;
bool gyro_sample_available(void) { return _sample_available(); }
bool accel_sample_available(void) { return _sample_available(); }
// detect the sensor
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu,
AP_InertialSensor::Sample_rate sample_rate,
Vector3f &gyro,
Vector3f &accel);
private:
bool _sample_available();
uint16_t _init_sensor( Sample_rate sample_rate );
uint32_t _sample_period_usec;
uint32_t _last_sample_usec;
uint32_t _last_accel_usec[INS_MAX_INSTANCES];
uint32_t _last_gyro_usec[INS_MAX_INSTANCES];
bool _init_sensor(AP_InertialSensor::Sample_rate sample_rate);
bool _sample_available(void);
uint32_t _sample_period_usec;
uint32_t _last_sample_usec;
};
#endif // __AP_INERTIAL_SENSOR_STUB_H__
#endif // __AP_INERTIALSENSOR_HIL_H__