From e57aaaaa5ea03686e1e1df3671ccbacd8837090a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 22 Mar 2021 12:01:12 -0400 Subject: [PATCH] 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) --- boards/nxp/fmurt1062-v1/default.cmake | 4 +- msg/sensor_accel_fifo.msg | 2 - msg/sensor_gyro_fifo.msg | 2 - src/lib/conversion/rotation.cpp | 201 +------------- src/lib/conversion/rotation.h | 259 +++++++++++++++++- .../accelerometer/PX4Accelerometer.cpp | 121 ++++---- .../accelerometer/PX4Accelerometer.hpp | 4 +- src/lib/drivers/gyroscope/PX4Gyroscope.cpp | 88 +++--- src/lib/drivers/gyroscope/PX4Gyroscope.hpp | 4 +- src/lib/mathlib/math/Functions.hpp | 13 + src/lib/sensor_calibration/Gyroscope.hpp | 5 + .../VehicleAngularVelocity.cpp | 72 +++-- .../VehicleAngularVelocity.hpp | 3 + 13 files changed, 419 insertions(+), 359 deletions(-) diff --git a/boards/nxp/fmurt1062-v1/default.cmake b/boards/nxp/fmurt1062-v1/default.cmake index c91fa23414..0cf400bc0a 100644 --- a/boards/nxp/fmurt1062-v1/default.cmake +++ b/boards/nxp/fmurt1062-v1/default.cmake @@ -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 diff --git a/msg/sensor_accel_fifo.msg b/msg/sensor_accel_fifo.msg index 0f79bf5018..1eae5dd149 100644 --- a/msg/sensor_accel_fifo.msg +++ b/msg/sensor_accel_fifo.msg @@ -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) diff --git a/msg/sensor_gyro_fifo.msg b/msg/sensor_gyro_fifo.msg index 957336e5e2..2e77ef07eb 100644 --- a/msg/sensor_gyro_fifo.msg +++ b/msg/sensor_gyro_fifo.msg @@ -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 diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index a19842c915..19f9967880 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -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; - } -} diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index decb9f4b6a..80a9910d8b 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -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 @@ -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 +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); + } + } +} diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp index 1fc77a3e41..6eb7031053 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp @@ -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); } diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index 4a8fc17e8a..50fdbfa2f7 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -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_pub{ORB_ID(sensor_accel)}; diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index 7f647b39c1..a5002e70ca 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -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); diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index 6f851d28d8..5e09001379 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -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_pub{ORB_ID(sensor_gyro)}; uORB::PublicationMulti _sensor_fifo_pub{ORB_ID(sensor_gyro_fifo)}; diff --git a/src/lib/mathlib/math/Functions.hpp b/src/lib/mathlib/math/Functions.hpp index 610900581a..346555687b 100644 --- a/src/lib/mathlib/math/Functions.hpp +++ b/src/lib/mathlib/math/Functions.hpp @@ -210,4 +210,17 @@ const T lerp(const T &a, const T &b, const T &s) return (static_cast(1) - s) * a + s * b; } +template +constexpr T negate(T value) +{ + static_assert(sizeof(T) > 2, "implement for T"); + return -value; +} + +template<> +constexpr int16_t negate(int16_t value) +{ + return (value == INT16_MIN) ? INT16_MAX : -value; +} + } /* namespace math */ diff --git a/src/lib/sensor_calibration/Gyroscope.hpp b/src/lib/sensor_calibration/Gyroscope.hpp index 163960377f..0d249eedb6 100644 --- a/src/lib/sensor_calibration/Gyroscope.hpp +++ b/src/lib/sensor_calibration/Gyroscope.hpp @@ -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(); diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 009157c645..97372a7e62 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -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(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() diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 8bd7636e0e..2b73ceaedb 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -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_pub{ORB_ID(vehicle_angular_acceleration)};