Browse Source
* split out filtered sensor_gyro aggregation from mc_att_control and move to wq:rate_ctrlsbg
Daniel Agar
6 years ago
committed by
GitHub
34 changed files with 778 additions and 376 deletions
@ -0,0 +1,8 @@
@@ -0,0 +1,8 @@
|
||||
|
||||
uint64 timestamp # time since system start (microseconds) |
||||
|
||||
uint64 timestamp_sample # the timestamp of the raw data (microseconds) |
||||
|
||||
float32[3] xyz # Bias corrected angular velocity about X, Y, Z body axis in rad/s |
||||
|
||||
# TOPICS vehicle_angular_velocity vehicle_angular_velocity_groundtruth |
||||
@ -0,0 +1,37 @@
@@ -0,0 +1,37 @@
|
||||
############################################################################ |
||||
# |
||||
# Copyright (c) 2019 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 |
||||
# are met: |
||||
# |
||||
# 1. Redistributions of source code must retain the above copyright |
||||
# notice, this list of conditions and the following disclaimer. |
||||
# 2. Redistributions in binary form must reproduce the above copyright |
||||
# notice, this list of conditions and the following disclaimer in |
||||
# the documentation and/or other materials provided with the |
||||
# distribution. |
||||
# 3. Neither the name PX4 nor the names of its contributors may be |
||||
# used to endorse or promote products derived from this software |
||||
# without specific prior written permission. |
||||
# |
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
# POSSIBILITY OF SUCH DAMAGE. |
||||
# |
||||
############################################################################ |
||||
|
||||
px4_add_library(sensors__vehicle_angular_velocity |
||||
VehicleAngularVelocity.cpp |
||||
) |
||||
target_link_libraries(sensors__vehicle_angular_velocity PRIVATE px4_work_queue) |
||||
@ -0,0 +1,226 @@
@@ -0,0 +1,226 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2019 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 |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#include "VehicleAngularVelocity.hpp" |
||||
|
||||
#include <px4_log.h> |
||||
|
||||
using namespace time_literals; |
||||
using namespace matrix; |
||||
|
||||
VehicleAngularVelocity::VehicleAngularVelocity() : |
||||
ModuleParams(nullptr), |
||||
WorkItem(px4::wq_configurations::rate_ctrl), |
||||
_cycle_perf(perf_alloc(PC_ELAPSED, "vehicle_angular_velocity: cycle time")), |
||||
_interval_perf(perf_alloc(PC_INTERVAL, "vehicle_angular_velocity: interval")), |
||||
_sensor_gyro_latency_perf(perf_alloc(PC_ELAPSED, "vehicle_angular_velocity: sensor gyro latency")) |
||||
{ |
||||
} |
||||
|
||||
VehicleAngularVelocity::~VehicleAngularVelocity() |
||||
{ |
||||
Stop(); |
||||
|
||||
perf_free(_cycle_perf); |
||||
perf_free(_interval_perf); |
||||
perf_free(_sensor_gyro_latency_perf); |
||||
} |
||||
|
||||
bool |
||||
VehicleAngularVelocity::Start() |
||||
{ |
||||
// initialize thermal corrections as we might not immediately get a topic update (only non-zero values)
|
||||
_scale = Vector3f{1.0f, 1.0f, 1.0f}; |
||||
_offset.zero(); |
||||
_bias.zero(); |
||||
|
||||
// force initial updates
|
||||
ParametersUpdate(true); |
||||
SensorBiasUpdate(true); |
||||
|
||||
_sensor_correction_sub.register_callback(); |
||||
|
||||
return SensorCorrectionsUpdate(true); |
||||
} |
||||
|
||||
void |
||||
VehicleAngularVelocity::Stop() |
||||
{ |
||||
Deinit(); |
||||
|
||||
// clear all registered callbacks
|
||||
for (auto sub : _sensor_gyro_sub) { |
||||
sub.unregister_callback(); |
||||
} |
||||
|
||||
_sensor_correction_sub.unregister_callback(); |
||||
} |
||||
|
||||
void |
||||
VehicleAngularVelocity::SensorBiasUpdate(bool force) |
||||
{ |
||||
if (_sensor_bias_sub.updated() || force) { |
||||
sensor_bias_s bias; |
||||
|
||||
if (_sensor_bias_sub.copy(&bias)) { |
||||
// TODO: should be checking device ID
|
||||
_bias(0) = bias.gyro_x_bias; |
||||
_bias(1) = bias.gyro_y_bias; |
||||
_bias(2) = bias.gyro_z_bias; |
||||
} |
||||
} |
||||
} |
||||
|
||||
bool |
||||
VehicleAngularVelocity::SensorCorrectionsUpdate(bool force) |
||||
{ |
||||
// check if the selected gyro has updated
|
||||
if (_sensor_correction_sub.updated() || force) { |
||||
|
||||
sensor_correction_s corrections{}; |
||||
_sensor_correction_sub.copy(&corrections); |
||||
|
||||
// TODO: should be checking device ID
|
||||
if (_selected_gyro == 0) { |
||||
_offset = Vector3f{corrections.gyro_offset_0}; |
||||
_scale = Vector3f{corrections.gyro_scale_0}; |
||||
|
||||
} else if (_selected_gyro == 1) { |
||||
_offset = Vector3f{corrections.gyro_offset_1}; |
||||
_scale = Vector3f{corrections.gyro_scale_1}; |
||||
|
||||
} else if (_selected_gyro == 2) { |
||||
_offset = Vector3f{corrections.gyro_offset_2}; |
||||
_scale = Vector3f{corrections.gyro_scale_2}; |
||||
|
||||
} else { |
||||
_offset = Vector3f{0.0f, 0.0f, 0.0f}; |
||||
_scale = Vector3f{1.0f, 1.0f, 1.0f}; |
||||
} |
||||
|
||||
// update the latest gyro selection
|
||||
if (_selected_gyro != corrections.selected_gyro_instance) { |
||||
if (corrections.selected_gyro_instance < MAX_GYRO_COUNT) { |
||||
// clear all registered callbacks
|
||||
for (auto sub : _sensor_gyro_sub) { |
||||
sub.unregister_callback(); |
||||
} |
||||
|
||||
const int gyro_new = corrections.selected_gyro_instance; |
||||
|
||||
if (_sensor_gyro_sub[gyro_new].register_callback()) { |
||||
PX4_DEBUG("selected gyro changed %d -> %d", _selected_gyro, gyro_new); |
||||
_selected_gyro = gyro_new; |
||||
|
||||
return true; |
||||
} |
||||
} |
||||
} |
||||
} |
||||
|
||||
return false; |
||||
} |
||||
|
||||
void |
||||
VehicleAngularVelocity::ParametersUpdate(bool force) |
||||
{ |
||||
// Check if parameters have changed
|
||||
if (_params_sub.updated() || force) { |
||||
// clear update
|
||||
parameter_update_s param_update; |
||||
_params_sub.copy(¶m_update); |
||||
|
||||
updateParams(); |
||||
|
||||
// get transformation matrix from sensor/board to body frame
|
||||
const matrix::Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get()); |
||||
|
||||
// fine tune the rotation
|
||||
const Dcmf board_rotation_offset(Eulerf( |
||||
math::radians(_param_sens_board_x_off.get()), |
||||
math::radians(_param_sens_board_y_off.get()), |
||||
math::radians(_param_sens_board_z_off.get()))); |
||||
|
||||
_board_rotation = board_rotation_offset * board_rotation; |
||||
} |
||||
} |
||||
|
||||
void |
||||
VehicleAngularVelocity::Run() |
||||
{ |
||||
perf_begin(_cycle_perf); |
||||
perf_count(_interval_perf); |
||||
|
||||
// update corrections first to set _selected_gyro
|
||||
SensorCorrectionsUpdate(); |
||||
|
||||
sensor_gyro_s sensor_gyro; |
||||
|
||||
if (_sensor_gyro_sub[_selected_gyro].update(&sensor_gyro)) { |
||||
perf_set_elapsed(_sensor_gyro_latency_perf, hrt_elapsed_time(&sensor_gyro.timestamp)); |
||||
|
||||
ParametersUpdate(); |
||||
SensorBiasUpdate(); |
||||
|
||||
// get the raw gyro data and correct for thermal errors
|
||||
const Vector3f gyro{sensor_gyro.x, sensor_gyro.y, sensor_gyro.z}; |
||||
|
||||
// apply offsets and scale
|
||||
Vector3f rates{(gyro - _offset).emult(_scale)}; |
||||
|
||||
// rotate corrected measurements from sensor to body frame
|
||||
rates = _board_rotation * rates; |
||||
|
||||
// correct for in-run bias errors
|
||||
rates -= _bias; |
||||
|
||||
vehicle_angular_velocity_s angular_velocity; |
||||
angular_velocity.timestamp_sample = sensor_gyro.timestamp; |
||||
rates.copyTo(angular_velocity.xyz); |
||||
angular_velocity.timestamp = hrt_absolute_time(); |
||||
|
||||
_vehicle_angular_velocity_pub.publish(angular_velocity); |
||||
} |
||||
|
||||
perf_end(_cycle_perf); |
||||
} |
||||
|
||||
void |
||||
VehicleAngularVelocity::PrintStatus() |
||||
{ |
||||
PX4_INFO("selected gyro: %d", _selected_gyro); |
||||
|
||||
perf_print_counter(_cycle_perf); |
||||
perf_print_counter(_interval_perf); |
||||
perf_print_counter(_sensor_gyro_latency_perf); |
||||
} |
||||
@ -0,0 +1,110 @@
@@ -0,0 +1,110 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2019 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 |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#pragma once |
||||
|
||||
#include <lib/perf/perf_counter.h> |
||||
#include <px4_config.h> |
||||
#include <px4_getopt.h> |
||||
#include <px4_log.h> |
||||
#include <px4_module_params.h> |
||||
#include <lib/matrix/matrix/math.hpp> |
||||
#include <lib/mathlib/math/Limits.hpp> |
||||
#include <lib/mathlib/math/Functions.hpp> |
||||
#include <lib/conversion/rotation.h> |
||||
#include <px4_work_queue/WorkItem.hpp> |
||||
#include <uORB/Publication.hpp> |
||||
#include <uORB/Subscription.hpp> |
||||
#include <uORB/SubscriptionCallback.hpp> |
||||
#include <uORB/topics/parameter_update.h> |
||||
#include <uORB/topics/sensor_bias.h> |
||||
#include <uORB/topics/sensor_correction.h> |
||||
#include <uORB/topics/sensor_gyro.h> |
||||
#include <uORB/topics/vehicle_angular_velocity.h> |
||||
|
||||
#define MAX_GYRO_COUNT 3 |
||||
|
||||
class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem |
||||
{ |
||||
public: |
||||
|
||||
VehicleAngularVelocity(); |
||||
virtual ~VehicleAngularVelocity(); |
||||
|
||||
void Run() override; |
||||
|
||||
bool Start(); |
||||
void Stop(); |
||||
|
||||
void PrintStatus(); |
||||
|
||||
private: |
||||
|
||||
void ParametersUpdate(bool force = false); |
||||
void SensorBiasUpdate(bool force = false); |
||||
bool SensorCorrectionsUpdate(bool force = false); |
||||
|
||||
|
||||
DEFINE_PARAMETERS( |
||||
(ParamInt<px4::params::SENS_BOARD_ROT>) _param_sens_board_rot, |
||||
|
||||
(ParamFloat<px4::params::SENS_BOARD_X_OFF>) _param_sens_board_x_off, |
||||
(ParamFloat<px4::params::SENS_BOARD_Y_OFF>) _param_sens_board_y_off, |
||||
(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _param_sens_board_z_off |
||||
) |
||||
|
||||
uORB::Publication<vehicle_angular_velocity_s> _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)}; |
||||
|
||||
uORB::Subscription _params_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ |
||||
uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; /**< sensor in-run bias correction subscription */ |
||||
|
||||
uORB::SubscriptionCallbackWorkItem _sensor_correction_sub{this, ORB_ID(sensor_correction)}; /**< sensor thermal correction subscription */ |
||||
uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub[MAX_GYRO_COUNT] { /**< gyro data subscription */ |
||||
{this, ORB_ID(sensor_gyro), 0}, |
||||
{this, ORB_ID(sensor_gyro), 1}, |
||||
{this, ORB_ID(sensor_gyro), 2} |
||||
}; |
||||
|
||||
matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ |
||||
|
||||
matrix::Vector3f _offset; |
||||
matrix::Vector3f _scale; |
||||
matrix::Vector3f _bias; |
||||
|
||||
perf_counter_t _cycle_perf; |
||||
perf_counter_t _interval_perf; |
||||
perf_counter_t _sensor_gyro_latency_perf; |
||||
|
||||
int _selected_gyro{-1}; |
||||
|
||||
}; |
||||
Loading…
Reference in new issue