fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 04:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

invensense icm20602 improvements

Browse Source 
- checked register mechanism and simple watchdog
    - driver checks for errors gradually and can reconfigure itself
 - respect IMU_GYRO_RATEMAX at the driver level
 - fixed sensor INT16_MIN and INT16_MAX handling (y & z axis are flipped before publishing)
 - increased sensor_gyro_fifo max size (enables running the driver much slower, but still transferring all raw data)
 - PX4Accelerometer/PX4Gyroscope remove unnecessary memsets
This commit is contained in:
Daniel Agar
2020-02-18 19:21:20 -05:00
parent 40921241e7
commit 22499effb9
12 changed files with 423 additions and 177 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/drivers/accelerometer/PX4Accelerometer.cpp
+13 -10
View File
@@ -50,12 +50,12 @@ static inline int32_t sum(const int16_t samples[16], uint8_t len)
return sum;
}
static inline unsigned clipping(const int16_t samples[16], int16_t clip_limit, uint8_t len)
static constexpr unsigned clipping(const int16_t samples[16], int16_t clip_limit, uint8_t len)
{
unsigned clip_count = 0;
for (int n = 0; n < len; n++) {
if (abs(samples[n]) > clip_limit) {
if (abs(samples[n]) >= clip_limit) {
clip_count++;
}
}
@@ -120,7 +120,10 @@ void PX4Accelerometer::set_device_type(uint8_t devtype)
void PX4Accelerometer::set_update_rate(uint16_t rate)
{
_update_rate = rate;
const uint32_t update_interval = 1000000 / rate;
// TODO: set this intelligently
_integrator_reset_samples = 4000 / update_interval;
}
@@ -144,7 +147,7 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
// publish raw data immediately
{
sensor_accel_s report{};
sensor_accel_s report;
report.timestamp_sample = timestamp_sample;
report.device_id = _device_id;
@@ -166,7 +169,7 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
if (_integrator.put(timestamp_sample, val_calibrated, delta_velocity, integral_dt)) {
// fill sensor_accel_integrated and publish
sensor_accel_integrated_s report{};
sensor_accel_integrated_s report;
report.timestamp_sample = timestamp_sample;
report.error_count = _error_count;
@@ -208,7 +211,7 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
// Apply range scale and the calibrating offset/scale
const Vector3f val_calibrated{((Vector3f{x, y, z} * _scale) - _calibration_offset).emult(_calibration_scale)};
sensor_accel_s report{};
sensor_accel_s report;
report.timestamp_sample = sample.timestamp_sample;
report.device_id = _device_id;
@@ -269,7 +272,7 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
delta_velocity *= 1e-6f * dt;
// fill sensor_accel_integrated and publish
sensor_accel_integrated_s report{};
sensor_accel_integrated_s report;
report.timestamp_sample = sample.timestamp_sample;
report.error_count = _error_count;
@@ -316,13 +319,13 @@ void PX4Accelerometer::PublishStatus()
{
// publish sensor status
if (hrt_elapsed_time(&_status_last_publish) >= 100_ms) {
sensor_accel_status_s status{};
sensor_accel_status_s status;
status.device_id = _device_id;
status.error_count = _error_count;
status.full_scale_range = _range;
status.rotation = _rotation;
status.measure_rate = _update_rate;
status.measure_rate_hz = _update_rate;
status.temperature = _temperature;
status.vibration_metric = _vibration_metric;
status.clipping[0] = _clipping[0];
@@ -347,8 +350,8 @@ void PX4Accelerometer::ResetIntegrator()
void PX4Accelerometer::UpdateClipLimit()
{
// 95% of potential max
_clip_limit = (_range / _scale) * 0.95f;
// 99.9% of potential max
_clip_limit = fmaxf((_range / _scale) * 0.999f, INT16_MAX);
}
void PX4Accelerometer::UpdateVibrationMetrics(const Vector3f &delta_velocity)
src/lib/drivers/gyroscope/PX4Gyroscope.cpp
+16 -11
View File
@@ -50,12 +50,12 @@ static inline int32_t sum(const int16_t samples[16], uint8_t len)
return sum;
}
static inline unsigned clipping(const int16_t samples[16], int16_t clip_limit, uint8_t len)
static constexpr unsigned clipping(const int16_t samples[16], int16_t clip_limit, uint8_t len)
{
unsigned clip_count = 0;
for (int n = 0; n < len; n++) {
if (abs(samples[n]) > clip_limit) {
if (abs(samples[n]) >= clip_limit) {
clip_count++;
}
}
@@ -65,6 +65,7 @@ static inline unsigned clipping(const int16_t samples[16], int16_t clip_limit, u
PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation rotation) :
CDev(nullptr),
ModuleParams(nullptr),
_sensor_pub{ORB_ID(sensor_gyro), priority},
_sensor_fifo_pub{ORB_ID(sensor_gyro_fifo), priority},
_sensor_integrated_pub{ORB_ID(sensor_gyro_integrated), priority},
@@ -74,6 +75,8 @@ PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation r
_rotation_dcm{get_rot_matrix(rotation)}
{
_class_device_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);
updateParams();
}
PX4Gyroscope::~PX4Gyroscope()
@@ -119,7 +122,10 @@ void PX4Gyroscope::set_device_type(uint8_t devtype)
void PX4Gyroscope::set_update_rate(uint16_t rate)
{
_update_rate = rate;
const uint32_t update_interval = 1000000 / rate;
// TODO: set this intelligently
_integrator_reset_samples = 4000 / update_interval;
}
@@ -143,7 +149,7 @@ void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float
// publish raw data immediately
{
sensor_gyro_s report{};
sensor_gyro_s report;
report.timestamp_sample = timestamp_sample;
report.device_id = _device_id;
@@ -165,7 +171,7 @@ void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float
if (_integrator.put(timestamp_sample, val_calibrated, delta_angle, integral_dt)) {
// fill sensor_gyro_integrated and publish
sensor_gyro_integrated_s report{};
sensor_gyro_integrated_s report;
report.timestamp_sample = timestamp_sample;
report.error_count = _error_count;
@@ -207,7 +213,7 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
// Apply range scale and the calibration offset
const Vector3f val_calibrated{(Vector3f{x, y, z} * _scale) - _calibration_offset};
sensor_gyro_s report{};
sensor_gyro_s report;
report.timestamp_sample = sample.timestamp_sample;
report.device_id = _device_id;
@@ -268,7 +274,7 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
delta_angle *= 1e-6f * dt;
// fill sensor_gyro_integrated and publish
sensor_gyro_integrated_s report{};
sensor_gyro_integrated_s report;
report.timestamp_sample = sample.timestamp_sample;
report.error_count = _error_count;
@@ -315,13 +321,13 @@ void PX4Gyroscope::PublishStatus()
{
// publish sensor status
if (hrt_elapsed_time(&_status_last_publish) >= 100_ms) {
sensor_gyro_status_s status{};
sensor_gyro_status_s status;
status.device_id = _device_id;
status.error_count = _error_count;
status.full_scale_range = _range;
status.rotation = _rotation;
status.measure_rate = _update_rate;
status.measure_rate_hz = _update_rate;
status.temperature = _temperature;
status.vibration_metric = _vibration_metric;
status.coning_vibration = _coning_vibration;
@@ -347,11 +353,10 @@ void PX4Gyroscope::ResetIntegrator()
void PX4Gyroscope::UpdateClipLimit()
{
// 95% of potential max
_clip_limit = (_range / _scale) * 0.95f;
// 99.9% of potential max
_clip_limit = fmaxf((_range / _scale) * 0.999f, INT16_MAX);
}
void PX4Gyroscope::UpdateVibrationMetrics(const Vector3f &delta_angle)
{
// Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
src/lib/drivers/gyroscope/PX4Gyroscope.hpp
+10 -4
View File
@@ -38,6 +38,7 @@
#include <lib/cdev/CDev.hpp>
#include <lib/conversion/rotation.h>
#include <lib/drivers/device/integrator.h>
#include <px4_platform_common/module_params.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/PublicationQueuedMulti.hpp>
#include <uORB/topics/sensor_gyro.h>
@@ -45,7 +46,7 @@
#include <uORB/topics/sensor_gyro_integrated.h>
#include <uORB/topics/sensor_gyro_status.h>
class PX4Gyroscope : public cdev::CDev
class PX4Gyroscope : public cdev::CDev, public ModuleParams
{
public:
PX4Gyroscope(uint32_t device_id, uint8_t priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
@@ -55,6 +56,8 @@ public:
uint32_t get_device_id() const { return _device_id; }
float get_max_rate_hz() const { return _param_imu_gyro_rate_max.get(); }
void set_device_id(uint32_t device_id) { _device_id = device_id; }
void set_device_type(uint8_t devtype);
void set_error_count(uint64_t error_count) { _error_count += error_count; }
@@ -72,9 +75,9 @@ public:
uint8_t samples; // number of samples
float dt; // in microseconds
int16_t x[16];
int16_t y[16];
int16_t z[16];
int16_t x[32];
int16_t y[32];
int16_t z[32];
};
static_assert(sizeof(FIFOSample::x) == sizeof(sensor_gyro_fifo_s::x), "FIFOSample.x invalid size");
static_assert(sizeof(FIFOSample::y) == sizeof(sensor_gyro_fifo_s::y), "FIFOSample.y invalid size");
@@ -131,4 +134,7 @@ private:
uint8_t _integrator_fifo_samples{0};
uint8_t _integrator_clipping{0};
DEFINE_PARAMETERS(
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max
)
};