fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors: complete move to uORB::Subscription

Browse Source
This commit is contained in:
Daniel Agar 2020-03-12 18:27:02 -04:00
parent 7c8ab49eb9
commit aa2f3a6624
4 changed files with 33 additions and 113 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Tools/uorb_graph/create.py
View File

@ -220,7 +220,6 @@ class Graph(object):
# (the expectation is that the previous matching ORB_ID() will be passed
# to this, so that we can ignore it)
special_cases_sub = [
('sensors', r'voted_sensors_update\.cpp$', r'\binitSensorClass\b\(([^,)]+)', r'^meta$'),
('listener', r'.*', None, r'^(id)$'),
('logger', r'.*', None, r'^(topic|sub\.metadata|_polling_topic_meta)$'),

1
src/modules/sensors/sensors.cpp
View File

@ -441,7 +441,6 @@ void Sensors::Run()
sub.unregisterCallback();
}
_voted_sensors_update.deinit();
exit_and_cleanup();
return;
}

101
src/modules/sensors/voted_sensors_update.cpp
View File

@ -50,6 +50,7 @@
using namespace sensors;
using namespace matrix;
using math::radians;
VotedSensorsUpdate::VotedSensorsUpdate(const Parameters &parameters, bool hil_enabled)
: ModuleParams(nullptr), _parameters(parameters), _hil_enabled(hil_enabled), _mag_compensator(this)
@ -86,33 +87,15 @@ int VotedSensorsUpdate::init(sensor_combined_s &raw)
void VotedSensorsUpdate::initializeSensors()
{
initSensorClass(ORB_ID(sensor_gyro_integrated), _gyro, GYRO_COUNT_MAX);
initSensorClass(ORB_ID(sensor_mag), _mag, MAG_COUNT_MAX);
initSensorClass(ORB_ID(sensor_accel_integrated), _accel, ACCEL_COUNT_MAX);
}
void VotedSensorsUpdate::deinit()
{
for (int i = 0; i < _gyro.subscription_count; i++) {
orb_unsubscribe(_gyro.subscription[i]);
}
for (int i = 0; i < _accel.subscription_count; i++) {
orb_unsubscribe(_accel.subscription[i]);
}
for (int i = 0; i < _mag.subscription_count; i++) {
orb_unsubscribe(_mag.subscription[i]);
}
initSensorClass(_gyro, GYRO_COUNT_MAX);
initSensorClass(_accel, ACCEL_COUNT_MAX);
initSensorClass(_mag, MAG_COUNT_MAX);
}
void VotedSensorsUpdate::parametersUpdate()
{
/* fine tune board offset */
Dcmf board_rotation_offset = Eulerf(
M_DEG_TO_RAD_F * _parameters.board_offset[0],
M_DEG_TO_RAD_F * _parameters.board_offset[1],
M_DEG_TO_RAD_F * _parameters.board_offset[2]);
const Dcmf board_rotation_offset{Eulerf{radians(_parameters.board_offset[0]), radians(_parameters.board_offset[1]), radians(_parameters.board_offset[2])}};
_board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)_parameters.board_rotation);
@ -344,7 +327,7 @@ void VotedSensorsUpdate::parametersUpdate()
sensor_mag_s report{};
if (orb_copy(ORB_ID(sensor_mag), _mag.subscription[topic_instance], &report) != 0) {
if (!_mag.subscription[topic_instance].copy(&report)) {
continue;
}
@ -486,27 +469,14 @@ void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw)
float *scales[] = {_corrections.accel_scale_0, _corrections.accel_scale_1, _corrections.accel_scale_2 };
for (int uorb_index = 0; uorb_index < _accel.subscription_count; uorb_index++) {
bool accel_updated;
orb_check(_accel.subscription[uorb_index], &accel_updated);
if (accel_updated) {
sensor_accel_integrated_s accel_report;
sensor_accel_integrated_s accel_report;
int ret = orb_copy(ORB_ID(sensor_accel_integrated), _accel.subscription[uorb_index], &accel_report);
if (ret != PX4_OK || accel_report.timestamp == 0) {
continue; //ignore invalid data
}
if (!_accel.enabled[uorb_index]) {
continue;
}
if (_accel.enabled[uorb_index] && _accel.subscription[uorb_index].update(&accel_report)) {
// First publication with data
if (_accel.priority[uorb_index] == 0) {
int32_t priority = 0;
orb_priority(_accel.subscription[uorb_index], &priority);
_accel.priority[uorb_index] = (uint8_t)priority;
_accel.priority[uorb_index] = _accel.subscription[uorb_index].get_priority();
}
_accel_device_id[uorb_index] = accel_report.device_id;
@ -567,27 +537,13 @@ void VotedSensorsUpdate::gyroPoll(struct sensor_combined_s &raw)
float *scales[] = {_corrections.gyro_scale_0, _corrections.gyro_scale_1, _corrections.gyro_scale_2 };
for (int uorb_index = 0; uorb_index < _gyro.subscription_count; uorb_index++) {
bool gyro_updated;
orb_check(_gyro.subscription[uorb_index], &gyro_updated);
sensor_gyro_integrated_s gyro_report;
if (gyro_updated) {
sensor_gyro_integrated_s gyro_report;
int ret = orb_copy(ORB_ID(sensor_gyro_integrated), _gyro.subscription[uorb_index], &gyro_report);
if (ret != PX4_OK || gyro_report.timestamp == 0) {
continue; //ignore invalid data
}
if (!_gyro.enabled[uorb_index]) {
continue;
}
if (_gyro.enabled[uorb_index] && _gyro.subscription[uorb_index].update(&gyro_report)) {
// First publication with data
if (_gyro.priority[uorb_index] == 0) {
int32_t priority = 0;
orb_priority(_gyro.subscription[uorb_index], &priority);
_gyro.priority[uorb_index] = (uint8_t)priority;
_gyro.priority[uorb_index] = _gyro.subscription[uorb_index].get_priority();
}
_gyro_device_id[uorb_index] = gyro_report.device_id;
@ -646,27 +602,13 @@ void VotedSensorsUpdate::gyroPoll(struct sensor_combined_s &raw)
void VotedSensorsUpdate::magPoll(vehicle_magnetometer_s &magnetometer)
{
for (int uorb_index = 0; uorb_index < _mag.subscription_count; uorb_index++) {
bool mag_updated;
orb_check(_mag.subscription[uorb_index], &mag_updated);
sensor_mag_s mag_report;
if (mag_updated) {
sensor_mag_s mag_report{};
int ret = orb_copy(ORB_ID(sensor_mag), _mag.subscription[uorb_index], &mag_report);
if (ret != PX4_OK || mag_report.timestamp == 0) {
continue; //ignore invalid data
}
if (!_mag.enabled[uorb_index]) {
continue;
}
if (_mag.enabled[uorb_index] && _mag.subscription[uorb_index].update(&mag_report)) {
// First publication with data
if (_mag.priority[uorb_index] == 0) {
int32_t priority = 0;
orb_priority(_mag.subscription[uorb_index], &priority);
_mag.priority[uorb_index] = (uint8_t)priority;
_mag.priority[uorb_index] = _mag.subscription[uorb_index].get_priority();
/* force a scale and offset update the first time we get data */
parametersUpdate();
@ -678,7 +620,6 @@ void VotedSensorsUpdate::magPoll(vehicle_magnetometer_s &magnetometer)
*/
continue;
}
}
Vector3f vect(mag_report.x, mag_report.y, mag_report.z);
@ -781,25 +722,21 @@ bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_na
return false;
}
void VotedSensorsUpdate::initSensorClass(const struct orb_metadata *meta, SensorData &sensor_data,
uint8_t sensor_count_max)
void VotedSensorsUpdate::initSensorClass(SensorData &sensor_data, uint8_t sensor_count_max)
{
int max_sensor_index = -1;
for (unsigned i = 0; i < sensor_count_max; i++) {
if (orb_exists(meta, i) != 0) {
continue;
}
max_sensor_index = i;
if (sensor_data.subscription[i] < 0) {
sensor_data.subscription[i] = orb_subscribe_multi(meta, i);
if (!sensor_data.advertised[i] && sensor_data.subscription[i].advertised()) {
sensor_data.advertised[i] = true;
if (i > 0) {
/* the first always exists, but for each further sensor, add a new validator */
if (!sensor_data.voter.add_new_validator()) {
PX4_ERR("failed to add validator for sensor %s %i", meta->o_name, i);
PX4_ERR("failed to add validator for sensor %s %i", sensor_data.subscription[i].get_topic()->o_name, i);
}
}
}

43
src/modules/sensors/voted_sensors_update.h
View File

@ -95,11 +95,6 @@ public:
*/
void initializeSensors();
/**
* deinitialize the object (we cannot use the destructor because it is called on the wrong thread)
*/
void deinit();
void printStatus();
/**
@ -159,31 +154,21 @@ public:
private:
struct SensorData {
SensorData()
: last_best_vote(0),
subscription_count(0),
voter(1),
last_failover_count(0)
{
for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
enabled[i] = true;
subscription[i] = -1;
priority[i] = 0;
}
}
SensorData() = delete;
explicit SensorData(ORB_ID meta) : subscription{{meta, 0}, {meta, 1}, {meta, 2}, {meta, 3}} {}
bool enabled[SENSOR_COUNT_MAX];
int subscription[SENSOR_COUNT_MAX]; /**< raw sensor data subscription */
uint8_t priority[SENSOR_COUNT_MAX]; /**< sensor priority */
uint8_t last_best_vote; /**< index of the latest best vote */
int subscription_count;
DataValidatorGroup voter;
unsigned int last_failover_count;
uORB::Subscription subscription[SENSOR_COUNT_MAX]; /**< raw sensor data subscription */
DataValidatorGroup voter{1};
unsigned int last_failover_count{0};
uint8_t priority[SENSOR_COUNT_MAX] {}; /**< sensor priority */
uint8_t last_best_vote{0}; /**< index of the latest best vote */
uint8_t subscription_count{0};
bool enabled[SENSOR_COUNT_MAX] {true, true, true, true};
bool advertised[SENSOR_COUNT_MAX] {false, false, false, false};
matrix::Vector3f power_compensation[SENSOR_COUNT_MAX];
};
void initSensorClass(const orb_metadata *meta, SensorData &sensor_data, uint8_t sensor_count_max);
void initSensorClass(SensorData &sensor_data, uint8_t sensor_count_max);
/**
* Poll the accelerometer for updated data.
@ -215,9 +200,9 @@ private:
*/
bool checkFailover(SensorData &sensor, const char *sensor_name, const uint64_t type);
SensorData _accel {};
SensorData _gyro {};
SensorData _mag {};
SensorData _accel{ORB_ID::sensor_accel_integrated};
SensorData _gyro{ORB_ID::sensor_gyro_integrated};
SensorData _mag{ORB_ID::sensor_mag};
orb_advert_t _mavlink_log_pub{nullptr};