mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
9c6bd38e91
Due to the way the headers are organized changing a single change in an inertial sensor driver would trigger a rebuild for most of the files in the project. Time could be saved by using ccache (since most of the things didn't change) but we can do better, i.e. re-organize the headers so we don't have to re-build everything. With this patch only AP_InertialSensor/AP_InertialSensor.h is exposed to most users. There are some corner cases to integrate with some example code, but most of the places now depend only on this header and this header doesn't depend on the specific backends. Now changing a single header, e.g. AP_InertialSensor_L3G4200D.h triggers a rebuild only of these files: $ waf copter 'copter' finished successfully (0.000s) Waf: Entering directory `/home/lucas/p/dronecode/ardupilot/build/minlure' [ 80/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor.cpp [ 84/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp [310/370] Linking build/minlure/ArduCopter/libArduCopter_libs.a [370/370] Linking build/minlure/bin/arducopter Waf: Leaving directory `/home/lucas/p/dronecode/ardupilot/build/minlure'
73 lines
2.2 KiB
C++
73 lines
2.2 KiB
C++
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
#pragma once
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
|
|
|
#include "AP_InertialSensor.h"
|
|
#include "AP_InertialSensor_Backend.h"
|
|
#include <drivers/drv_accel.h>
|
|
#include <drivers/drv_gyro.h>
|
|
#include <uORB/uORB.h>
|
|
#include <uORB/topics/sensor_combined.h>
|
|
|
|
#include <Filter/Filter.h>
|
|
#include <Filter/LowPassFilter2p.h>
|
|
|
|
class AP_InertialSensor_PX4 : public AP_InertialSensor_Backend
|
|
{
|
|
public:
|
|
|
|
AP_InertialSensor_PX4(AP_InertialSensor &imu);
|
|
|
|
/* update accel and gyro state */
|
|
bool update();
|
|
|
|
// detect the sensor
|
|
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
|
|
|
|
// accumulate more samples
|
|
void accumulate(void) override { _get_sample(); }
|
|
|
|
private:
|
|
bool _init_sensor(void);
|
|
void _get_sample(void);
|
|
uint64_t _last_accel_timestamp[INS_MAX_INSTANCES];
|
|
uint64_t _last_gyro_timestamp[INS_MAX_INSTANCES];
|
|
float _accel_sample_time[INS_MAX_INSTANCES];
|
|
float _gyro_sample_time[INS_MAX_INSTANCES];
|
|
|
|
void _new_accel_sample(uint8_t i, accel_report &accel_report);
|
|
void _new_gyro_sample(uint8_t i, gyro_report &gyro_report);
|
|
|
|
bool _get_gyro_sample(uint8_t i, struct gyro_report &gyro_report);
|
|
bool _get_accel_sample(uint8_t i, struct accel_report &accel_report);
|
|
|
|
// calculate right queue depth for a sensor
|
|
uint8_t _queue_depth(uint16_t sensor_sample_rate) const;
|
|
|
|
// accelerometer and gyro driver handles
|
|
uint8_t _num_accel_instances;
|
|
uint8_t _num_gyro_instances;
|
|
|
|
int _accel_fd[INS_MAX_INSTANCES];
|
|
int _gyro_fd[INS_MAX_INSTANCES];
|
|
|
|
// indexes in frontend object. Note that these could be different
|
|
// from the backend indexes
|
|
uint8_t _accel_instance[INS_MAX_INSTANCES];
|
|
uint8_t _gyro_instance[INS_MAX_INSTANCES];
|
|
|
|
#ifdef AP_INERTIALSENSOR_PX4_DEBUG
|
|
uint32_t _gyro_meas_count[INS_MAX_INSTANCES];
|
|
uint32_t _accel_meas_count[INS_MAX_INSTANCES];
|
|
|
|
uint32_t _gyro_meas_count_start_us[INS_MAX_INSTANCES];
|
|
uint32_t _accel_meas_count_start_us[INS_MAX_INSTANCES];
|
|
|
|
float _gyro_dt_max[INS_MAX_INSTANCES];
|
|
float _accel_dt_max[INS_MAX_INSTANCES];
|
|
#endif // AP_INERTIALSENSOR_PX4_DEBUG
|
|
};
|
|
#endif
|