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 @@ |
|||||||
|
|
||||||
|
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 @@ |
|||||||
|
############################################################################ |
||||||
|
# |
||||||
|
# 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 @@ |
|||||||
|
/****************************************************************************
|
||||||
|
* |
||||||
|
* 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 @@ |
|||||||
|
/****************************************************************************
|
||||||
|
* |
||||||
|
* 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