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

introduce sensor_gyro_control message for vehicle_angular_velocity (#12145)

Browse Source
This commit is contained in:
Daniel Agar
2019-08-16 13:53:59 -04:00
committed by GitHub
parent be3d09c700
commit dacaabe92e
7 changed files with 134 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
+94 -18
View File
@@ -103,6 +103,17 @@ VehicleAngularVelocity::SensorBiasUpdate(bool force)
bool
VehicleAngularVelocity::SensorCorrectionsUpdate(bool force)
{
if (_sensor_selection_sub.updated() || force) {
sensor_selection_s sensor_selection;
if (_sensor_selection_sub.copy(&sensor_selection)) {
if (_selected_sensor_device_id != sensor_selection.gyro_device_id) {
_selected_sensor_device_id = sensor_selection.gyro_device_id;
force = true;
}
}
}
// check if the selected sensor has updated
if (_sensor_correction_sub.updated() || force) {
@@ -131,12 +142,40 @@ VehicleAngularVelocity::SensorCorrectionsUpdate(bool force)
if ((_selected_sensor != corrections.selected_gyro_instance) || force) {
if (corrections.selected_gyro_instance < MAX_SENSOR_COUNT) {
// clear all registered callbacks
for (auto &sub : _sensor_control_sub) {
sub.unregister_callback();
}
for (auto &sub : _sensor_sub) {
sub.unregister_callback();
}
const int sensor_new = corrections.selected_gyro_instance;
// subscribe to sensor_gyro_control if available
// currently not all drivers (eg df_*) provide sensor_gyro_control
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
sensor_gyro_control_s report{};
_sensor_control_sub[i].copy(&report);
if ((report.device_id != 0) && (report.device_id == _selected_sensor_device_id)) {
if (_sensor_control_sub[i].register_callback()) {
PX4_DEBUG("selected sensor (control) changed %d -> %d", _selected_sensor, i);
_selected_sensor_control = i;
_sensor_control_available = true;
// record selected sensor (sensor_gyro orb index)
_selected_sensor = sensor_new;
return true;
}
}
}
// otherwise fallback to using sensor_gyro (legacy that will be removed)
_sensor_control_available = false;
if (_sensor_sub[sensor_new].register_callback()) {
PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor, sensor_new);
_selected_sensor = sensor_new;
@@ -183,32 +222,62 @@ VehicleAngularVelocity::Run()
// update corrections first to set _selected_sensor
SensorCorrectionsUpdate();
sensor_gyro_s sensor_data;
if (_sensor_control_available) {
// using sensor_gyro_control is preferred, but currently not all drivers (eg df_*) provide sensor_gyro_control
sensor_gyro_control_s sensor_data;
if (_sensor_sub[_selected_sensor].update(&sensor_data)) {
perf_set_elapsed(_sensor_latency_perf, hrt_elapsed_time(&sensor_data.timestamp));
if (_sensor_control_sub[_selected_sensor].update(&sensor_data)) {
perf_set_elapsed(_sensor_latency_perf, hrt_elapsed_time(&sensor_data.timestamp));
ParametersUpdate();
SensorBiasUpdate();
ParametersUpdate();
SensorBiasUpdate();
// get the sensor data and correct for thermal errors
const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
// get the sensor data and correct for thermal errors (apply offsets and scale)
Vector3f rates{(Vector3f{sensor_data.xyz} - _offset).emult(_scale)};
// apply offsets and scale
Vector3f rates{(val - _offset).emult(_scale)};
// rotate corrected measurements from sensor to body frame
rates = _board_rotation * rates;
// rotate corrected measurements from sensor to body frame
rates = _board_rotation * rates;
// correct for in-run bias errors
rates -= _bias;
// correct for in-run bias errors
rates -= _bias;
vehicle_angular_velocity_s angular_velocity;
angular_velocity.timestamp_sample = sensor_data.timestamp;
rates.copyTo(angular_velocity.xyz);
angular_velocity.timestamp = hrt_absolute_time();
vehicle_angular_velocity_s angular_velocity;
angular_velocity.timestamp_sample = sensor_data.timestamp;
rates.copyTo(angular_velocity.xyz);
angular_velocity.timestamp = hrt_absolute_time();
_vehicle_angular_velocity_pub.publish(angular_velocity);
}
_vehicle_angular_velocity_pub.publish(angular_velocity);
} else {
// otherwise fallback to using sensor_gyro (legacy that will be removed)
sensor_gyro_s sensor_data;
if (_sensor_sub[_selected_sensor].update(&sensor_data)) {
perf_set_elapsed(_sensor_latency_perf, hrt_elapsed_time(&sensor_data.timestamp));
ParametersUpdate();
SensorBiasUpdate();
// get the sensor data and correct for thermal errors
const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
// apply offsets and scale
Vector3f rates{(val - _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_data.timestamp;
rates.copyTo(angular_velocity.xyz);
angular_velocity.timestamp = hrt_absolute_time();
_vehicle_angular_velocity_pub.publish(angular_velocity);
}
}
perf_end(_cycle_perf);
@@ -219,6 +288,13 @@ VehicleAngularVelocity::PrintStatus()
{
PX4_INFO("selected sensor: %d", _selected_sensor);
if (_selected_sensor_device_id != 0) {
PX4_INFO("using sensor_gyro_control: %d (%d)", _selected_sensor_device_id, _selected_sensor_control);
} else {
PX4_WARN("sensor_gyro_control unavailable for selected sensor: %d (%d)", _selected_sensor_device_id, _selected_sensor);
}
perf_print_counter(_cycle_perf);
perf_print_counter(_interval_perf);
perf_print_counter(_sensor_latency_perf);
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
+11
View File
@@ -50,6 +50,7 @@
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/sensor_gyro_control.h>
#include <uORB/topics/vehicle_angular_velocity.h>
class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem
@@ -89,12 +90,19 @@ private:
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; /**< sensor thermal correction subscription */
uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; /**< selected primary sensor subscription */
uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { /**< sensor data subscription */
{this, ORB_ID(sensor_gyro), 0},
{this, ORB_ID(sensor_gyro), 1},
{this, ORB_ID(sensor_gyro), 2}
};
uORB::SubscriptionCallbackWorkItem _sensor_control_sub[MAX_SENSOR_COUNT] { /**< sensor control data subscription */
{this, ORB_ID(sensor_gyro_control), 0},
{this, ORB_ID(sensor_gyro_control), 1},
{this, ORB_ID(sensor_gyro_control), 2}
};
matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
matrix::Vector3f _offset;
@@ -105,6 +113,9 @@ private:
perf_counter_t _interval_perf;
perf_counter_t _sensor_latency_perf;
uint32_t _selected_sensor_device_id{0};
uint8_t _selected_sensor{0};
uint8_t _selected_sensor_control{0};
bool _sensor_control_available{false};
};