aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: use wait_pin() to wait for DRDY pin if available

Browse Source
This commit is contained in:
Andrew Tridgell 2020-04-21 08:34:47 +10:00
parent 450871cde1
commit 5c6749ee54
3 changed files with 30 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp
View File

@ -44,8 +44,8 @@
#define TIMING_DEBUG 0
#if TIMING_DEBUG
#define DEBUG_SET_PIN(n,v) hal.gpio->write(55-n, v)
#define DEBUG_TOGGLE_PIN(n) hal.gpio->toggle(55-n)
#define DEBUG_SET_PIN(n,v) hal.gpio->write(52+n, v)
#define DEBUG_TOGGLE_PIN(n) hal.gpio->toggle(52+n)
#else
#define DEBUG_SET_PIN(n,v)
#define DEBUG_TOGGLE_PIN(n)
@ -54,23 +54,26 @@
extern const AP_HAL::HAL& hal;
AP_InertialSensor_ADIS1647x::AP_InertialSensor_ADIS1647x(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::Device> _dev,
enum Rotation _rotation)
AP_HAL::OwnPtr<AP_HAL::Device> _dev,
enum Rotation _rotation,
uint8_t drdy_gpio)
: AP_InertialSensor_Backend(imu)
, dev(std::move(_dev))
, rotation(_rotation)
, drdy_pin(drdy_gpio)
{
}
AP_InertialSensor_Backend *
AP_InertialSensor_ADIS1647x::probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::Device> dev,
enum Rotation rotation)
AP_HAL::OwnPtr<AP_HAL::Device> dev,
enum Rotation rotation,
uint8_t drdy_gpio)
{
if (!dev) {
return nullptr;
}
auto sensor = new AP_InertialSensor_ADIS1647x(imu, std::move(dev), rotation);
auto sensor = new AP_InertialSensor_ADIS1647x(imu, std::move(dev), rotation, drdy_gpio);
if (!sensor) {
return nullptr;
@ -305,10 +308,22 @@ void AP_InertialSensor_ADIS1647x::loop(void)
// we deliberately set the period a bit fast to ensure we
// don't lose a sample
const uint32_t period_us = 480;
bool wait_ok = false;
if (drdy_pin != 0) {
// when we have a DRDY pin then wait for it to go high
DEBUG_SET_PIN(0, 1);
wait_ok = hal.gpio->wait_pin(drdy_pin, AP_HAL::GPIO::INTERRUPT_RISING, 1000);
DEBUG_SET_PIN(0, 0);
}
read_sensor();
uint32_t dt = AP_HAL::micros() - tstart;
if (dt < period_us) {
hal.scheduler->delay_microseconds(period_us - dt);
uint32_t wait_us = period_us - dt;
if (!wait_ok || wait_us > period_us/2) {
DEBUG_SET_PIN(3, 1);
hal.scheduler->delay_microseconds(wait_us);
DEBUG_SET_PIN(3, 0);
}
}
}
}

7
libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.h
View File

@ -27,7 +27,8 @@ class AP_InertialSensor_ADIS1647x : public AP_InertialSensor_Backend {
public:
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::Device> dev,
enum Rotation rotation);
enum Rotation rotation,
uint8_t drdy_gpio);
/**
* Configure the sensors and start reading routine.
@ -38,7 +39,8 @@ public:
private:
AP_InertialSensor_ADIS1647x(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::Device> dev,
enum Rotation rotation);
enum Rotation rotation,
uint8_t drdy_gpio);
/*
initialise driver
@ -59,6 +61,7 @@ private:
uint8_t accel_instance;
uint8_t gyro_instance;
enum Rotation rotation;
uint8_t drdy_pin;
uint16_t last_counter;
bool done_first_read;

4
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -64,8 +64,8 @@ void AP_InertialSensor_Backend::_update_sensor_rate(uint16_t &count, uint32_t &s
count++;
if (now - start_us > 1000000UL) {
float observed_rate_hz = count * 1.0e6f / (now - start_us);
#if SENSOR_RATE_DEBUG
printf("RATE: %.1f should be %.1f\n", observed_rate_hz, rate_hz);
#if 0
printf("IMU RATE: %.1f should be %.1f\n", observed_rate_hz, rate_hz);
#endif
float filter_constant = 0.98f;
float upper_limit = 1.05f;