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

sensors: allow up to 4 accels, gyros, and baros and add configurable rotations for accel & gyro

Browse Source
This commit is contained in:
Daniel Agar
2020-10-08 19:01:44 -04:00
committed by GitHub
parent 28956e0399
commit eecf2e7a1e
47 changed files with 2881 additions and 1163 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/sensor_calibration/Accelerometer.cpp
+38 -10
View File
@@ -110,6 +110,9 @@ void Accelerometer::SensorCorrectionsUpdate(bool force)
case 2:
_thermal_offset = Vector3f{corrections.accel_offset_2};
return;
case 3:
_thermal_offset = Vector3f{corrections.accel_offset_2};
return;
}
}
}
@@ -120,6 +123,12 @@ void Accelerometer::SensorCorrectionsUpdate(bool force)
}
}
void Accelerometer::set_rotation(Rotation rotation)
{
_rotation_enum = rotation;
_rotation = get_rot_matrix(rotation);
}
void Accelerometer::ParametersUpdate()
{
if (_device_id == 0) {
@@ -131,12 +140,30 @@ void Accelerometer::ParametersUpdate()
if (_calibration_index >= 0) {
if (!_external) {
_rotation = GetBoardRotation();
// CAL_ACCx_ROT
int32_t rotation_value = GetCalibrationParam(SensorString(), "ROT", _calibration_index);
if (_external) {
if ((rotation_value >= ROTATION_MAX) || (rotation_value < 0)) {
PX4_ERR("External %s %d (%d) invalid rotation %d, resetting to rotation none",
SensorString(), _device_id, _calibration_index, rotation_value);
rotation_value = ROTATION_NONE;
SetCalibrationParam(SensorString(), "ROT", _calibration_index, rotation_value);
}
_rotation_enum = static_cast<Rotation>(rotation_value);
_rotation = get_rot_matrix(_rotation_enum);
} else {
// TODO: per sensor external rotation
_rotation.setIdentity();
// internal, CAL_ACCx_ROT -1
if (rotation_value != -1) {
PX4_ERR("Internal %s %d (%d) invalid rotation %d, resetting",
SensorString(), _device_id, _calibration_index, rotation_value);
SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1);
}
_rotation = GetBoardRotation();
_rotation_enum = ROTATION_NONE;
}
// CAL_ACCx_PRIO
@@ -144,7 +171,7 @@ void Accelerometer::ParametersUpdate()
if ((_priority < 0) || (_priority > 100)) {
// reset to default, -1 is the uninitialized parameter value
int32_t new_priority = DEFAULT_PRIORITY;
int32_t new_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
if (_priority != -1) {
PX4_ERR("%s %d (%d) invalid priority %d, resetting to %d", SensorString(), _device_id, _calibration_index, _priority,
@@ -185,6 +212,7 @@ void Accelerometer::ParametersUpdate()
void Accelerometer::Reset()
{
_rotation.setIdentity();
_rotation_enum = ROTATION_NONE;
_offset.zero();
_scale = Vector3f{1.f, 1.f, 1.f};
_thermal_offset.zero();
@@ -206,12 +234,12 @@ bool Accelerometer::ParametersSave()
success &= SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset);
success &= SetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index, _scale);
// if (_external) {
// SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum);
if (_external) {
success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum);
// } else {
// SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1);
// }
} else {
success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1);
}
return success;
}
src/lib/sensor_calibration/Accelerometer.hpp
+6 -2
View File
@@ -45,10 +45,10 @@ namespace calibration
class Accelerometer
{
public:
static constexpr int MAX_SENSOR_COUNT = 3;
static constexpr int MAX_SENSOR_COUNT = 4;
static constexpr uint8_t DEFAULT_PRIORITY = 50;
static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 25;
static constexpr const char *SensorString() { return "ACC"; }
@@ -64,6 +64,7 @@ public:
void set_external(bool external = true);
void set_offset(const matrix::Vector3f &offset) { _offset = offset; }
void set_scale(const matrix::Vector3f &scale) { _scale = scale; }
void set_rotation(Rotation rotation);
uint8_t calibration_count() const { return _calibration_count; }
uint32_t device_id() const { return _device_id; }
@@ -71,6 +72,7 @@ public:
bool external() const { return _external; }
const int32_t &priority() const { return _priority; }
const matrix::Dcmf &rotation() const { return _rotation; }
const Rotation &rotation_enum() const { return _rotation_enum; }
// apply offsets and scale
// rotate corrected measurements from sensor to body frame
@@ -89,6 +91,8 @@ public:
private:
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
Rotation _rotation_enum{ROTATION_NONE};
matrix::Dcmf _rotation;
matrix::Vector3f _offset;
matrix::Vector3f _scale;
src/lib/sensor_calibration/Gyroscope.cpp
+38 -10
View File
@@ -110,6 +110,9 @@ void Gyroscope::SensorCorrectionsUpdate(bool force)
case 2:
_thermal_offset = Vector3f{corrections.gyro_offset_2};
return;
case 3:
_thermal_offset = Vector3f{corrections.gyro_offset_3};
return;
}
}
}
@@ -120,6 +123,12 @@ void Gyroscope::SensorCorrectionsUpdate(bool force)
}
}
void Gyroscope::set_rotation(Rotation rotation)
{
_rotation_enum = rotation;
_rotation = get_rot_matrix(rotation);
}
void Gyroscope::ParametersUpdate()
{
if (_device_id == 0) {
@@ -131,12 +140,30 @@ void Gyroscope::ParametersUpdate()
if (_calibration_index >= 0) {
if (!_external) {
_rotation = GetBoardRotation();
// CAL_GYROx_ROT
int32_t rotation_value = GetCalibrationParam(SensorString(), "ROT", _calibration_index);
if (_external) {
if ((rotation_value >= ROTATION_MAX) || (rotation_value < 0)) {
PX4_ERR("External %s %d (%d) invalid rotation %d, resetting to rotation none",
SensorString(), _device_id, _calibration_index, rotation_value);
rotation_value = ROTATION_NONE;
SetCalibrationParam(SensorString(), "ROT", _calibration_index, rotation_value);
}
_rotation_enum = static_cast<Rotation>(rotation_value);
_rotation = get_rot_matrix(_rotation_enum);
} else {
// TODO: per sensor external rotation
_rotation.setIdentity();
// internal, CAL_GYROx_ROT -1
if (rotation_value != -1) {
PX4_ERR("Internal %s %d (%d) invalid rotation %d, resetting",
SensorString(), _device_id, _calibration_index, rotation_value);
SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1);
}
_rotation = GetBoardRotation();
_rotation_enum = ROTATION_NONE;
}
// CAL_GYROx_PRIO
@@ -144,7 +171,7 @@ void Gyroscope::ParametersUpdate()
if ((_priority < 0) || (_priority > 100)) {
// reset to default, -1 is the uninitialized parameter value
int32_t new_priority = DEFAULT_PRIORITY;
int32_t new_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
if (_priority != -1) {
PX4_ERR("%s %d (%d) invalid priority %d, resetting to %d", SensorString(), _device_id, _calibration_index, _priority,
@@ -177,6 +204,7 @@ void Gyroscope::ParametersUpdate()
void Gyroscope::Reset()
{
_rotation.setIdentity();
_rotation_enum = ROTATION_NONE;
_offset.zero();
_thermal_offset.zero();
@@ -196,12 +224,12 @@ bool Gyroscope::ParametersSave()
success &= SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority);
success &= SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset);
// if (_external) {
// SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum);
if (_external) {
success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum);
// } else {
// SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1);
// }
} else {
success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1);
}
return success;
}
src/lib/sensor_calibration/Gyroscope.hpp
+6 -2
View File
@@ -45,10 +45,10 @@ namespace calibration
class Gyroscope
{
public:
static constexpr int MAX_SENSOR_COUNT = 3;
static constexpr int MAX_SENSOR_COUNT = 4;
static constexpr uint8_t DEFAULT_PRIORITY = 50;
static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 25;
static constexpr const char *SensorString() { return "GYRO"; }
@@ -63,6 +63,7 @@ public:
void set_device_id(uint32_t device_id, bool external = false);
void set_external(bool external = true);
void set_offset(const matrix::Vector3f &offset) { _offset = offset; }
void set_rotation(Rotation rotation);
uint8_t calibration_count() const { return _calibration_count; }
uint32_t device_id() const { return _device_id; }
@@ -70,6 +71,7 @@ public:
bool external() const { return _external; }
const int32_t &priority() const { return _priority; }
const matrix::Dcmf &rotation() const { return _rotation; }
const Rotation &rotation_enum() const { return _rotation_enum; }
// apply offsets and scale
// rotate corrected measurements from sensor to body frame
@@ -88,6 +90,8 @@ public:
private:
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
Rotation _rotation_enum{ROTATION_NONE};
matrix::Dcmf _rotation;
matrix::Vector3f _offset;
matrix::Vector3f _thermal_offset;