forked from Archive/PX4-Autopilot
rotate accel/gyro FIFO before publish and fix angular velocity filter resets
- rotates accel & gyro FIFO data before publication both to simplify downstream usage (including log review) and fix other issues
- to best handle int16_t data rotations are now either performed with swaps if possible, otherwise promoted to float, rotated using the full rotation matrix, then rounded back to int16_t
- fix sensors/vehicle_angular_velocity filter reset both with proper rotation and new calibration uncorrect helper
- in FIFO case filtering is done before calibration is applied, but we need to handle a possible reset from a completely different sensor (vehicle body angular velocity -> sensor frame uncorrected data)
This commit is contained in:
Daniel Agar
GitHub
parent
d4dd019578
commit
e57aaaaa5e
|
|
@ -71,7 +71,7 @@ px4_add_board(
|
|||
navigator
|
||||
rc_update
|
||||
sensors
|
||||
sih
|
||||
#sih
|
||||
temperature_compensation
|
||||
#vmount
|
||||
SYSTEMCMDS
|
||||
|
|
@ -94,6 +94,7 @@ px4_add_board(
|
|||
reboot
|
||||
reflect
|
||||
sd_bench
|
||||
#serial_test
|
||||
system_time
|
||||
top
|
||||
topic_listener
|
||||
|
|
@ -102,7 +103,6 @@ px4_add_board(
|
|||
usb_connected
|
||||
ver
|
||||
work_queue
|
||||
serial_test
|
||||
EXAMPLES
|
||||
## fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
|
||||
# hello
|
||||
|
|
|
|||
|
|
@ -11,5 +11,3 @@ uint8 samples # number of valid samples
|
|||
int16[32] x # acceleration in the FRD board frame X-axis in m/s^2
|
||||
int16[32] y # acceleration in the FRD board frame Y-axis in m/s^2
|
||||
int16[32] z # acceleration in the FRD board frame Z-axis in m/s^2
|
||||
|
||||
uint8 rotation # Direction the sensor faces (see Rotation enum)
|
||||
|
|
|
|||
|
|
@ -12,6 +12,4 @@ int16[32] x # angular velocity in the FRD board frame X-axis in ra
|
|||
int16[32] y # angular velocity in the FRD board frame Y-axis in rad/s
|
||||
int16[32] z # angular velocity in the FRD board frame Z-axis in rad/s
|
||||
|
||||
uint8 rotation # Direction the sensor faces (see Rotation enum)
|
||||
|
||||
uint8 ORB_QUEUE_LENGTH = 4
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2013-2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -56,202 +56,3 @@ get_rot_quaternion(enum Rotation rot)
|
|||
math::radians((float)rot_lookup[rot].pitch),
|
||||
math::radians((float)rot_lookup[rot].yaw)}};
|
||||
}
|
||||
|
||||
__EXPORT void
|
||||
rotate_3f(enum Rotation rot, float &x, float &y, float &z)
|
||||
{
|
||||
switch (rot) {
|
||||
case ROTATION_NONE:
|
||||
break;
|
||||
|
||||
case ROTATION_YAW_90: {
|
||||
float tmp = x;
|
||||
x = -y;
|
||||
y = tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_YAW_180: {
|
||||
x = -x;
|
||||
y = -y;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_YAW_270: {
|
||||
float tmp = x;
|
||||
x = y;
|
||||
y = -tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_180: {
|
||||
y = -y;
|
||||
z = -z;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_180_YAW_90:
|
||||
|
||||
// FALLTHROUGH
|
||||
case ROTATION_PITCH_180_YAW_270: {
|
||||
float tmp = x;
|
||||
x = y;
|
||||
y = tmp;
|
||||
z = -z;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_PITCH_180: {
|
||||
x = -x;
|
||||
z = -z;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_180_YAW_270:
|
||||
|
||||
// FALLTHROUGH
|
||||
case ROTATION_PITCH_180_YAW_90: {
|
||||
float tmp = x;
|
||||
x = -y;
|
||||
y = -tmp;
|
||||
z = -z;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_90: {
|
||||
float tmp = z;
|
||||
z = y;
|
||||
y = -tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_90_YAW_90: {
|
||||
float tmp = x;
|
||||
x = z;
|
||||
z = y;
|
||||
y = tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_270: {
|
||||
float tmp = z;
|
||||
z = -y;
|
||||
y = tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_270_YAW_90: {
|
||||
float tmp = x;
|
||||
x = -z;
|
||||
z = -y;
|
||||
y = tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_PITCH_90: {
|
||||
float tmp = z;
|
||||
z = -x;
|
||||
x = tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_PITCH_270: {
|
||||
float tmp = z;
|
||||
z = x;
|
||||
x = -tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_180_PITCH_270: {
|
||||
float tmp = z;
|
||||
z = x;
|
||||
x = tmp;
|
||||
y = -y;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_90_YAW_270: {
|
||||
float tmp = x;
|
||||
x = -z;
|
||||
z = y;
|
||||
y = -tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_90_PITCH_90: {
|
||||
float tmp = x;
|
||||
x = y;
|
||||
y = -z;
|
||||
z = -tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_180_PITCH_90: {
|
||||
float tmp = x;
|
||||
x = -z;
|
||||
y = -y;
|
||||
z = -tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_270_PITCH_90: {
|
||||
float tmp = x;
|
||||
x = -y;
|
||||
y = z;
|
||||
z = -tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_90_PITCH_180: {
|
||||
float tmp = y;
|
||||
x = -x;
|
||||
y = -z;
|
||||
z = -tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_270_PITCH_180: {
|
||||
float tmp = y;
|
||||
x = -x;
|
||||
y = z;
|
||||
z = tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_90_PITCH_270: {
|
||||
float tmp = x;
|
||||
x = -y;
|
||||
y = -z;
|
||||
z = tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_270_PITCH_270: {
|
||||
float tmp = x;
|
||||
x = y;
|
||||
y = z;
|
||||
z = tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_90_PITCH_180_YAW_90: {
|
||||
float tmp = x;
|
||||
x = z;
|
||||
z = -y;
|
||||
y = -tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
|
||||
// otherwise use full rotation matrix for valid rotations
|
||||
if (rot < ROTATION_MAX) {
|
||||
const matrix::Vector3f r{get_rot_matrix(rot) *matrix::Vector3f{x, y, z}};
|
||||
x = r(0);
|
||||
y = r(1);
|
||||
z = r(2);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013-2020 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2013-2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -37,8 +37,7 @@
|
|||
* Vector rotation library
|
||||
*/
|
||||
|
||||
#ifndef ROTATION_H_
|
||||
#define ROTATION_H_
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
|
|
@ -149,20 +148,256 @@ static constexpr rot_lookup_t rot_lookup[ROTATION_MAX] = {
|
|||
/**
|
||||
* Get the rotation matrix
|
||||
*/
|
||||
__EXPORT matrix::Dcmf
|
||||
get_rot_matrix(enum Rotation rot);
|
||||
__EXPORT matrix::Dcmf get_rot_matrix(enum Rotation rot);
|
||||
|
||||
/**
|
||||
* Get the rotation quaternion
|
||||
*/
|
||||
__EXPORT matrix::Quatf
|
||||
get_rot_quaternion(enum Rotation rot);
|
||||
__EXPORT matrix::Quatf get_rot_quaternion(enum Rotation rot);
|
||||
|
||||
template<typename T>
|
||||
static constexpr bool rotate_3(enum Rotation rot, T &x, T &y, T &z)
|
||||
{
|
||||
switch (rot) {
|
||||
case ROTATION_NONE:
|
||||
return true;
|
||||
|
||||
case ROTATION_YAW_90: {
|
||||
T tmp = x;
|
||||
x = math::negate(y);
|
||||
y = tmp;
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_YAW_180: {
|
||||
x = math::negate(x);
|
||||
y = math::negate(y);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_YAW_270: {
|
||||
T tmp = x;
|
||||
x = y;
|
||||
y = math::negate(tmp);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_180: {
|
||||
y = math::negate(y);
|
||||
z = math::negate(z);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_180_YAW_90:
|
||||
|
||||
// FALLTHROUGH
|
||||
case ROTATION_PITCH_180_YAW_270: {
|
||||
T tmp = x;
|
||||
x = y;
|
||||
y = tmp;
|
||||
z = math::negate(z);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_PITCH_180: {
|
||||
x = math::negate(x);
|
||||
z = math::negate(z);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_180_YAW_270:
|
||||
|
||||
// FALLTHROUGH
|
||||
case ROTATION_PITCH_180_YAW_90: {
|
||||
T tmp = x;
|
||||
x = math::negate(y);
|
||||
y = math::negate(tmp);
|
||||
z = math::negate(z);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_90: {
|
||||
T tmp = z;
|
||||
z = y;
|
||||
y = math::negate(tmp);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_90_YAW_90: {
|
||||
T tmp = x;
|
||||
x = z;
|
||||
z = y;
|
||||
y = tmp;
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_270: {
|
||||
T tmp = z;
|
||||
z = math::negate(y);
|
||||
y = tmp;
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_270_YAW_90: {
|
||||
T tmp = x;
|
||||
x = math::negate(z);
|
||||
z = math::negate(y);
|
||||
y = tmp;
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_PITCH_90: {
|
||||
T tmp = z;
|
||||
z = math::negate(x);
|
||||
x = tmp;
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_PITCH_270: {
|
||||
T tmp = z;
|
||||
z = x;
|
||||
x = math::negate(tmp);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_180_PITCH_270: {
|
||||
T tmp = z;
|
||||
z = x;
|
||||
x = tmp;
|
||||
y = math::negate(y);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_90_YAW_270: {
|
||||
T tmp = x;
|
||||
x = math::negate(z);
|
||||
z = y;
|
||||
y = math::negate(tmp);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_90_PITCH_90: {
|
||||
T tmp = x;
|
||||
x = y;
|
||||
y = math::negate(z);
|
||||
z = math::negate(tmp);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_180_PITCH_90: {
|
||||
T tmp = x;
|
||||
x = math::negate(z);
|
||||
y = math::negate(y);
|
||||
z = math::negate(tmp);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_270_PITCH_90: {
|
||||
T tmp = x;
|
||||
x = math::negate(y);
|
||||
y = z;
|
||||
z = math::negate(tmp);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_90_PITCH_180: {
|
||||
T tmp = y;
|
||||
x = math::negate(x);
|
||||
y = math::negate(z);
|
||||
z = math::negate(tmp);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_270_PITCH_180: {
|
||||
T tmp = y;
|
||||
x = math::negate(x);
|
||||
y = z;
|
||||
z = tmp;
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_90_PITCH_270: {
|
||||
T tmp = x;
|
||||
x = math::negate(y);
|
||||
y = math::negate(z);
|
||||
z = tmp;
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_270_PITCH_270: {
|
||||
T tmp = x;
|
||||
x = y;
|
||||
y = z;
|
||||
z = tmp;
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_90_PITCH_180_YAW_90: {
|
||||
T tmp = x;
|
||||
x = z;
|
||||
z = math::negate(y);
|
||||
y = math::negate(tmp);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* rotate a 3 element int16_t vector in-place
|
||||
*/
|
||||
__EXPORT inline void rotate_3i(enum Rotation rot, int16_t &x, int16_t &y, int16_t &z)
|
||||
{
|
||||
if (!rotate_3(rot, x, y, z)) {
|
||||
// otherwise use full rotation matrix for valid rotations
|
||||
if (rot < ROTATION_MAX) {
|
||||
const matrix::Vector3f r{get_rot_matrix(rot) *matrix::Vector3f{(float)x, (float)y, (float)z}};
|
||||
x = math::constrain(roundf(r(0)), (float)INT16_MIN, (float)INT16_MAX);
|
||||
y = math::constrain(roundf(r(1)), (float)INT16_MIN, (float)INT16_MAX);
|
||||
z = math::constrain(roundf(r(2)), (float)INT16_MIN, (float)INT16_MAX);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* rotate a 3 element float vector in-place
|
||||
*/
|
||||
__EXPORT void
|
||||
rotate_3f(enum Rotation rot, float &x, float &y, float &z);
|
||||
|
||||
|
||||
#endif /* ROTATION_H_ */
|
||||
__EXPORT inline void rotate_3f(enum Rotation rot, float &x, float &y, float &z)
|
||||
{
|
||||
if (!rotate_3(rot, x, y, z)) {
|
||||
// otherwise use full rotation matrix for valid rotations
|
||||
if (rot < ROTATION_MAX) {
|
||||
const matrix::Vector3f r{get_rot_matrix(rot) *matrix::Vector3f{x, y, z}};
|
||||
x = r(0);
|
||||
y = r(1);
|
||||
z = r(2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018-2020 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2018-2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -40,7 +40,7 @@
|
|||
using namespace time_literals;
|
||||
using matrix::Vector3f;
|
||||
|
||||
static constexpr int32_t sum(const int16_t samples[16], uint8_t len)
|
||||
static constexpr int32_t sum(const int16_t samples[], uint8_t len)
|
||||
{
|
||||
int32_t sum = 0;
|
||||
|
||||
|
|
@ -51,7 +51,7 @@ static constexpr int32_t sum(const int16_t samples[16], uint8_t len)
|
|||
return sum;
|
||||
}
|
||||
|
||||
static constexpr uint8_t clipping(const int16_t samples[16], int16_t clip_limit, uint8_t len)
|
||||
static constexpr uint8_t clipping(const int16_t samples[], int16_t clip_limit, uint8_t len)
|
||||
{
|
||||
unsigned clip_count = 0;
|
||||
|
||||
|
|
@ -94,67 +94,11 @@ void PX4Accelerometer::set_device_type(uint8_t devtype)
|
|||
}
|
||||
|
||||
void PX4Accelerometer::update(const hrt_abstime ×tamp_sample, float x, float y, float z)
|
||||
{
|
||||
// clipping
|
||||
uint8_t clip_count[3];
|
||||
clip_count[0] = (fabsf(x) >= _clip_limit);
|
||||
clip_count[1] = (fabsf(y) >= _clip_limit);
|
||||
clip_count[2] = (fabsf(z) >= _clip_limit);
|
||||
|
||||
// publish
|
||||
Publish(timestamp_sample, x, y, z, clip_count);
|
||||
}
|
||||
|
||||
void PX4Accelerometer::updateFIFO(sensor_accel_fifo_s &sample)
|
||||
{
|
||||
// publish fifo
|
||||
sample.device_id = _device_id;
|
||||
sample.scale = _scale;
|
||||
sample.rotation = _rotation;
|
||||
|
||||
sample.timestamp = hrt_absolute_time();
|
||||
_sensor_fifo_pub.publish(sample);
|
||||
|
||||
{
|
||||
// trapezoidal integration (equally spaced, scaled by dt later)
|
||||
const uint8_t N = sample.samples;
|
||||
const Vector3f integral{
|
||||
(0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1)),
|
||||
(0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1)),
|
||||
(0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1)),
|
||||
};
|
||||
|
||||
_last_sample[0] = sample.x[N - 1];
|
||||
_last_sample[1] = sample.y[N - 1];
|
||||
_last_sample[2] = sample.z[N - 1];
|
||||
|
||||
// clipping
|
||||
uint8_t clip_count[3] {
|
||||
clipping(sample.x, _clip_limit, N),
|
||||
clipping(sample.y, _clip_limit, N),
|
||||
clipping(sample.z, _clip_limit, N),
|
||||
};
|
||||
|
||||
const float x = integral(0) / (float)N;
|
||||
const float y = integral(1) / (float)N;
|
||||
const float z = integral(2) / (float)N;
|
||||
|
||||
// publish
|
||||
Publish(sample.timestamp_sample, x, y, z, clip_count, N);
|
||||
}
|
||||
}
|
||||
|
||||
void PX4Accelerometer::Publish(const hrt_abstime ×tamp_sample, float x, float y, float z, uint8_t clip_count[3],
|
||||
uint8_t samples)
|
||||
{
|
||||
// Apply rotation (before scaling)
|
||||
rotate_3f(_rotation, x, y, z);
|
||||
|
||||
float clipping_x = clip_count[0];
|
||||
float clipping_y = clip_count[1];
|
||||
float clipping_z = clip_count[2];
|
||||
rotate_3f(_rotation, clipping_x, clipping_y, clipping_z);
|
||||
|
||||
// publish
|
||||
sensor_accel_s report;
|
||||
|
||||
report.timestamp_sample = timestamp_sample;
|
||||
|
|
@ -164,10 +108,57 @@ void PX4Accelerometer::Publish(const hrt_abstime ×tamp_sample, float x, flo
|
|||
report.x = x * _scale;
|
||||
report.y = y * _scale;
|
||||
report.z = z * _scale;
|
||||
report.clip_counter[0] = fabsf(roundf(clipping_x));
|
||||
report.clip_counter[1] = fabsf(roundf(clipping_y));
|
||||
report.clip_counter[2] = fabsf(roundf(clipping_z));
|
||||
report.samples = samples;
|
||||
report.clip_counter[0] = (fabsf(x) >= _clip_limit);
|
||||
report.clip_counter[1] = (fabsf(y) >= _clip_limit);
|
||||
report.clip_counter[2] = (fabsf(z) >= _clip_limit);
|
||||
report.samples = 1;
|
||||
report.timestamp = hrt_absolute_time();
|
||||
|
||||
_sensor_pub.publish(report);
|
||||
}
|
||||
|
||||
void PX4Accelerometer::updateFIFO(sensor_accel_fifo_s &sample)
|
||||
{
|
||||
// rotate all raw samples and publish fifo
|
||||
const uint8_t N = sample.samples;
|
||||
|
||||
for (int n = 0; n < N; n++) {
|
||||
rotate_3i(_rotation, sample.x[n], sample.y[n], sample.z[n]);
|
||||
}
|
||||
|
||||
sample.device_id = _device_id;
|
||||
sample.scale = _scale;
|
||||
sample.timestamp = hrt_absolute_time();
|
||||
_sensor_fifo_pub.publish(sample);
|
||||
|
||||
|
||||
// trapezoidal integration (equally spaced, scaled by dt later)
|
||||
const Vector3f integral{
|
||||
(0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1)),
|
||||
(0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1)),
|
||||
(0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1)),
|
||||
};
|
||||
|
||||
_last_sample[0] = sample.x[N - 1];
|
||||
_last_sample[1] = sample.y[N - 1];
|
||||
_last_sample[2] = sample.z[N - 1];
|
||||
|
||||
|
||||
const float scale = _scale / (float)N;
|
||||
|
||||
// publish
|
||||
sensor_accel_s report;
|
||||
report.timestamp_sample = sample.timestamp_sample;
|
||||
report.device_id = _device_id;
|
||||
report.temperature = _temperature;
|
||||
report.error_count = _error_count;
|
||||
report.x = integral(0) * scale;
|
||||
report.y = integral(1) * scale;
|
||||
report.z = integral(2) * scale;
|
||||
report.clip_counter[0] = clipping(sample.x, _clip_limit, N);
|
||||
report.clip_counter[1] = clipping(sample.y, _clip_limit, N);
|
||||
report.clip_counter[2] = clipping(sample.z, _clip_limit, N);
|
||||
report.samples = N;
|
||||
report.timestamp = hrt_absolute_time();
|
||||
|
||||
_sensor_pub.publish(report);
|
||||
|
|
@ -176,5 +167,5 @@ void PX4Accelerometer::Publish(const hrt_abstime ×tamp_sample, float x, flo
|
|||
void PX4Accelerometer::UpdateClipLimit()
|
||||
{
|
||||
// 99.9% of potential max
|
||||
_clip_limit = fmaxf((_range / _scale) * 0.999f, INT16_MAX);
|
||||
_clip_limit = math::constrain((_range / _scale) * 0.999f, 0.f, (float)INT16_MAX);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018-2020 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2018-2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -65,8 +65,6 @@ public:
|
|||
int get_instance() { return _sensor_pub.get_instance(); };
|
||||
|
||||
private:
|
||||
void Publish(const hrt_abstime ×tamp_sample, float x, float y, float z, uint8_t clip_count[3],
|
||||
uint8_t samples = 1);
|
||||
void UpdateClipLimit();
|
||||
|
||||
uORB::PublicationMulti<sensor_accel_s> _sensor_pub{ORB_ID(sensor_accel)};
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018-2020 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2018-2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -40,7 +40,7 @@
|
|||
using namespace time_literals;
|
||||
using matrix::Vector3f;
|
||||
|
||||
static constexpr int32_t sum(const int16_t samples[16], uint8_t len)
|
||||
static constexpr int32_t sum(const int16_t samples[], uint8_t len)
|
||||
{
|
||||
int32_t sum = 0;
|
||||
|
||||
|
|
@ -81,44 +81,6 @@ void PX4Gyroscope::set_device_type(uint8_t devtype)
|
|||
}
|
||||
|
||||
void PX4Gyroscope::update(const hrt_abstime ×tamp_sample, float x, float y, float z)
|
||||
{
|
||||
// publish
|
||||
Publish(timestamp_sample, x, y, z);
|
||||
}
|
||||
|
||||
void PX4Gyroscope::updateFIFO(sensor_gyro_fifo_s &sample)
|
||||
{
|
||||
// publish fifo
|
||||
sample.device_id = _device_id;
|
||||
sample.scale = _scale;
|
||||
sample.rotation = _rotation;
|
||||
|
||||
sample.timestamp = hrt_absolute_time();
|
||||
_sensor_fifo_pub.publish(sample);
|
||||
|
||||
{
|
||||
// trapezoidal integration (equally spaced, scaled by dt later)
|
||||
const uint8_t N = sample.samples;
|
||||
const Vector3f integral{
|
||||
(0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1)),
|
||||
(0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1)),
|
||||
(0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1)),
|
||||
};
|
||||
|
||||
_last_sample[0] = sample.x[N - 1];
|
||||
_last_sample[1] = sample.y[N - 1];
|
||||
_last_sample[2] = sample.z[N - 1];
|
||||
|
||||
const float x = integral(0) / (float)N;
|
||||
const float y = integral(1) / (float)N;
|
||||
const float z = integral(2) / (float)N;
|
||||
|
||||
// publish
|
||||
Publish(sample.timestamp_sample, x, y, z, N);
|
||||
}
|
||||
}
|
||||
|
||||
void PX4Gyroscope::Publish(const hrt_abstime ×tamp_sample, float x, float y, float z, uint8_t samples)
|
||||
{
|
||||
// Apply rotation (before scaling)
|
||||
rotate_3f(_rotation, x, y, z);
|
||||
|
|
@ -132,7 +94,51 @@ void PX4Gyroscope::Publish(const hrt_abstime ×tamp_sample, float x, float y
|
|||
report.x = x * _scale;
|
||||
report.y = y * _scale;
|
||||
report.z = z * _scale;
|
||||
report.samples = samples;
|
||||
report.samples = 1;
|
||||
report.timestamp = hrt_absolute_time();
|
||||
|
||||
_sensor_pub.publish(report);
|
||||
}
|
||||
|
||||
void PX4Gyroscope::updateFIFO(sensor_gyro_fifo_s &sample)
|
||||
{
|
||||
// rotate all raw samples and publish fifo
|
||||
const uint8_t N = sample.samples;
|
||||
|
||||
for (int n = 0; n < N; n++) {
|
||||
rotate_3i(_rotation, sample.x[n], sample.y[n], sample.z[n]);
|
||||
}
|
||||
|
||||
sample.device_id = _device_id;
|
||||
sample.scale = _scale;
|
||||
sample.timestamp = hrt_absolute_time();
|
||||
_sensor_fifo_pub.publish(sample);
|
||||
|
||||
|
||||
// trapezoidal integration (equally spaced, scaled by dt later)
|
||||
const Vector3f integral{
|
||||
(0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1)),
|
||||
(0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1)),
|
||||
(0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1)),
|
||||
};
|
||||
|
||||
_last_sample[0] = sample.x[N - 1];
|
||||
_last_sample[1] = sample.y[N - 1];
|
||||
_last_sample[2] = sample.z[N - 1];
|
||||
|
||||
|
||||
const float scale = _scale / N;
|
||||
|
||||
sensor_gyro_s report;
|
||||
|
||||
report.timestamp_sample = sample.timestamp_sample;
|
||||
report.device_id = _device_id;
|
||||
report.temperature = _temperature;
|
||||
report.error_count = _error_count;
|
||||
report.x = integral(0) * scale;
|
||||
report.y = integral(1) * scale;
|
||||
report.z = integral(2) * scale;
|
||||
report.samples = N;
|
||||
report.timestamp = hrt_absolute_time();
|
||||
|
||||
_sensor_pub.publish(report);
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018-2020 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2018-2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -65,8 +65,6 @@ public:
|
|||
int get_instance() { return _sensor_pub.get_instance(); };
|
||||
|
||||
private:
|
||||
void Publish(const hrt_abstime ×tamp_sample, float x, float y, float z, uint8_t samples = 1);
|
||||
|
||||
uORB::PublicationMulti<sensor_gyro_s> _sensor_pub{ORB_ID(sensor_gyro)};
|
||||
uORB::PublicationMulti<sensor_gyro_fifo_s> _sensor_fifo_pub{ORB_ID(sensor_gyro_fifo)};
|
||||
|
||||
|
|
|
|||
|
|
@ -210,4 +210,17 @@ const T lerp(const T &a, const T &b, const T &s)
|
|||
return (static_cast<T>(1) - s) * a + s * b;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
constexpr T negate(T value)
|
||||
{
|
||||
static_assert(sizeof(T) > 2, "implement for T");
|
||||
return -value;
|
||||
}
|
||||
|
||||
template<>
|
||||
constexpr int16_t negate<int16_t>(int16_t value)
|
||||
{
|
||||
return (value == INT16_MIN) ? INT16_MAX : -value;
|
||||
}
|
||||
|
||||
} /* namespace math */
|
||||
|
|
|
|||
|
|
@ -82,6 +82,11 @@ public:
|
|||
return _rotation * matrix::Vector3f{data - _thermal_offset - _offset};
|
||||
}
|
||||
|
||||
inline matrix::Vector3f Uncorrect(const matrix::Vector3f &corrected_data) const
|
||||
{
|
||||
return (_rotation.I() * corrected_data) + _thermal_offset + _offset;
|
||||
}
|
||||
|
||||
bool ParametersSave();
|
||||
void ParametersUpdate();
|
||||
|
||||
|
|
|
|||
|
|
@ -330,6 +330,18 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
|
|||
}
|
||||
}
|
||||
|
||||
Vector3f VehicleAngularVelocity::GetResetAngularVelocity() const
|
||||
{
|
||||
if (_fifo_available && (_last_scale > 0.f)) {
|
||||
return _calibration.Uncorrect(_angular_velocity + _bias) / _last_scale;
|
||||
|
||||
} else if (!_fifo_available) {
|
||||
return _angular_velocity;
|
||||
}
|
||||
|
||||
return Vector3f{0.f, 0.f, 0.f};
|
||||
}
|
||||
|
||||
void VehicleAngularVelocity::DisableDynamicNotchEscRpm()
|
||||
{
|
||||
#if !defined(CONSTRAINED_FLASH)
|
||||
|
|
@ -395,10 +407,12 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
|
|||
}
|
||||
|
||||
// only reset if there's sufficient change (> 1%)
|
||||
if ((change_percent > 0.01f) && (_last_scale > 0.f)) {
|
||||
if (change_percent > 0.01f) {
|
||||
const Vector3f reset_angular_velocity{GetResetAngularVelocity()};
|
||||
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
auto &dnf = _dynamic_notch_filter_esc_rpm[i][harmonic][axis];
|
||||
dnf.reset(_angular_velocity(axis) / _last_scale);
|
||||
dnf.reset(reset_angular_velocity(axis));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -451,9 +465,9 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force)
|
|||
// peak frequency changed by at least 0.1%
|
||||
dnf.setParameters(_filter_sample_rate_hz, peak_freq, sensor_gyro_fft.resolution_hz);
|
||||
|
||||
// only reset if there's sufficient change (> 1%)
|
||||
if ((peak_diff_abs > sensor_gyro_fft.resolution_hz) && (_last_scale > 0.f)) {
|
||||
dnf.reset(_angular_velocity(axis) / _last_scale);
|
||||
// only reset if there's sufficient change
|
||||
if (peak_diff_abs > sensor_gyro_fft.resolution_hz) {
|
||||
dnf.reset(GetResetAngularVelocity()(axis));
|
||||
}
|
||||
|
||||
perf_count(_dynamic_notch_filter_fft_update_perf);
|
||||
|
|
@ -500,15 +514,16 @@ void VehicleAngularVelocity::Run()
|
|||
|
||||
const int N = sensor_fifo_data.samples;
|
||||
const float dt_s = sensor_fifo_data.dt * 1e-6f;
|
||||
const enum Rotation fifo_rotation = static_cast<enum Rotation>(sensor_fifo_data.rotation);
|
||||
|
||||
if (_reset_filters || (fabsf(sensor_fifo_data.scale - _last_scale) > FLT_EPSILON)) {
|
||||
if (UpdateSampleRate()) {
|
||||
// in FIFO mode the unscaled raw data is filtered
|
||||
_angular_velocity_prev = _angular_velocity / sensor_fifo_data.scale;
|
||||
ResetFilters(_angular_velocity_prev, _angular_acceleration / sensor_fifo_data.scale);
|
||||
|
||||
_last_scale = sensor_fifo_data.scale;
|
||||
|
||||
_angular_velocity_prev = GetResetAngularVelocity();
|
||||
Vector3f angular_acceleration{_calibration.rotation().I() *_angular_acceleration / _last_scale};
|
||||
|
||||
ResetFilters(_angular_velocity_prev, angular_acceleration);
|
||||
}
|
||||
|
||||
if (_reset_filters) {
|
||||
|
|
@ -592,16 +607,13 @@ void VehicleAngularVelocity::Run()
|
|||
}
|
||||
|
||||
// Angular velocity: rotate sensor frame to board, scale raw data to SI, apply calibration, and remove in-run estimated bias
|
||||
rotate_3f(fifo_rotation, angular_velocity_unscaled(0), angular_velocity_unscaled(1), angular_velocity_unscaled(2));
|
||||
_angular_velocity = _calibration.Correct(angular_velocity_unscaled * sensor_fifo_data.scale) - _bias;
|
||||
|
||||
// Angular acceleration: rotate sensor frame to board, scale raw data to SI, apply any additional configured rotation
|
||||
rotate_3f(fifo_rotation, angular_acceleration_unscaled(0), angular_acceleration_unscaled(1),
|
||||
angular_acceleration_unscaled(2));
|
||||
_angular_acceleration = _calibration.rotation() * angular_acceleration_unscaled * sensor_fifo_data.scale;
|
||||
|
||||
// Publish
|
||||
if (!_sensor_fifo_sub.updated() && (sensor_fifo_data.timestamp_sample - _last_publish >= _publish_interval_min_us)) {
|
||||
if (!_sensor_fifo_sub.updated()) {
|
||||
Publish(sensor_fifo_data.timestamp_sample);
|
||||
}
|
||||
}
|
||||
|
|
@ -683,7 +695,7 @@ void VehicleAngularVelocity::Run()
|
|||
_angular_velocity = angular_velocity;
|
||||
|
||||
// Publish
|
||||
if (!_sensor_sub.updated() && (sensor_data.timestamp_sample - _last_publish >= _publish_interval_min_us)) {
|
||||
if (!_sensor_sub.updated()) {
|
||||
Publish(sensor_data.timestamp_sample);
|
||||
}
|
||||
}
|
||||
|
|
@ -692,23 +704,25 @@ void VehicleAngularVelocity::Run()
|
|||
|
||||
void VehicleAngularVelocity::Publish(const hrt_abstime ×tamp_sample)
|
||||
{
|
||||
// Publish vehicle_angular_acceleration
|
||||
vehicle_angular_acceleration_s v_angular_acceleration;
|
||||
v_angular_acceleration.timestamp_sample = timestamp_sample;
|
||||
_angular_acceleration.copyTo(v_angular_acceleration.xyz);
|
||||
v_angular_acceleration.timestamp = hrt_absolute_time();
|
||||
_vehicle_angular_acceleration_pub.publish(v_angular_acceleration);
|
||||
if (timestamp_sample >= _last_publish + _publish_interval_min_us) {
|
||||
// Publish vehicle_angular_acceleration
|
||||
vehicle_angular_acceleration_s v_angular_acceleration;
|
||||
v_angular_acceleration.timestamp_sample = timestamp_sample;
|
||||
_angular_acceleration.copyTo(v_angular_acceleration.xyz);
|
||||
v_angular_acceleration.timestamp = hrt_absolute_time();
|
||||
_vehicle_angular_acceleration_pub.publish(v_angular_acceleration);
|
||||
|
||||
// Publish vehicle_angular_velocity
|
||||
vehicle_angular_velocity_s v_angular_velocity;
|
||||
v_angular_velocity.timestamp_sample = timestamp_sample;
|
||||
_angular_velocity.copyTo(v_angular_velocity.xyz);
|
||||
v_angular_velocity.timestamp = hrt_absolute_time();
|
||||
_vehicle_angular_velocity_pub.publish(v_angular_velocity);
|
||||
// Publish vehicle_angular_velocity
|
||||
vehicle_angular_velocity_s v_angular_velocity;
|
||||
v_angular_velocity.timestamp_sample = timestamp_sample;
|
||||
_angular_velocity.copyTo(v_angular_velocity.xyz);
|
||||
v_angular_velocity.timestamp = hrt_absolute_time();
|
||||
_vehicle_angular_velocity_pub.publish(v_angular_velocity);
|
||||
|
||||
// shift last publish time forward, but don't let it get further behind than the interval
|
||||
_last_publish = math::constrain(_last_publish + _publish_interval_min_us,
|
||||
timestamp_sample - _publish_interval_min_us, timestamp_sample);
|
||||
// shift last publish time forward, but don't let it get further behind than the interval
|
||||
_last_publish = math::constrain(_last_publish + _publish_interval_min_us,
|
||||
timestamp_sample - _publish_interval_min_us, timestamp_sample);
|
||||
}
|
||||
}
|
||||
|
||||
void VehicleAngularVelocity::PrintStatus()
|
||||
|
|
|
|||
|
|
@ -86,6 +86,9 @@ private:
|
|||
void UpdateDynamicNotchFFT(bool force = false);
|
||||
bool UpdateSampleRate();
|
||||
|
||||
// scaled appropriately for current FIFO mode
|
||||
matrix::Vector3f GetResetAngularVelocity() const;
|
||||
|
||||
static constexpr int MAX_SENSOR_COUNT = 4;
|
||||
|
||||
uORB::Publication<vehicle_angular_acceleration_s> _vehicle_angular_acceleration_pub{ORB_ID(vehicle_angular_acceleration)};
|
||||
|
|
|
|||
Loading…
Reference in New Issue