Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

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

Merged
merged 1 commit into from Oct 8, 2020
Merged

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

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
301 changes: 293 additions & 8 deletions Tools/process_sensor_caldata.py
View file
Open in desktop

Large diffs are not rendered by default.

9 changes: 6 additions & 3 deletions msg/sensor_correction.msg
View file
Open in desktop
Expand Up @@ -6,21 +6,24 @@ uint64 timestamp # time since system start (microseconds)

# Corrections for gyro angular rate outputs where corrected_rate = raw_rate * gyro_scale + gyro_offset
# Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame
uint32[3] gyro_device_ids
uint32[4] gyro_device_ids
float32[3] gyro_offset_0 # gyro 0 XYZ offsets in the sensor frame in rad/s
float32[3] gyro_offset_1 # gyro 1 XYZ offsets in the sensor frame in rad/s
float32[3] gyro_offset_2 # gyro 2 XYZ offsets in the sensor frame in rad/s
float32[3] gyro_offset_3 # gyro 3 XYZ offsets in the sensor frame in rad/s

# Corrections for acceleromter acceleration outputs where corrected_accel = raw_accel * accel_scale + accel_offset
# Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame
uint32[3] accel_device_ids
uint32[4] accel_device_ids
float32[3] accel_offset_0 # accelerometer 0 XYZ offsets in the sensor frame in m/s/s
float32[3] accel_offset_1 # accelerometer 1 XYZ offsets in the sensor frame in m/s/s
float32[3] accel_offset_2 # accelerometer 2 XYZ offsets in the sensor frame in m/s/s
float32[3] accel_offset_3 # accelerometer 3 XYZ offsets in the sensor frame in m/s/s

# Corrections for barometric pressure outputs where corrected_pressure = raw_pressure * pressure_scale + pressure_offset
# Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame
uint32[3] baro_device_ids
uint32[4] baro_device_ids
float32 baro_offset_0 # barometric pressure 0 offsets in the sensor frame in m/s/s
float32 baro_offset_1 # barometric pressure 1 offsets in the sensor frame in m/s/s
float32 baro_offset_2 # barometric pressure 2 offsets in the sensor frame in m/s/s
float32 baro_offset_3 # barometric pressure 3 offsets in the sensor frame in m/s/s
48 changes: 38 additions & 10 deletions src/lib/sensor_calibration/Accelerometer.cpp
View file
Open in desktop
Expand Up @@ -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;
}
}
}
Expand All @@ -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) {
Expand All @@ -131,20 +140,38 @@ 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
_priority = GetCalibrationParam(SensorString(), "PRIO", _calibration_index);

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,
Expand Down Expand Up @@ -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();
Expand All @@ -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;
}
Expand Down
8 changes: 6 additions & 2 deletions src/lib/sensor_calibration/Accelerometer.hpp
View file
Open in desktop
Expand Up @@ -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"; }

Expand All @@ -64,13 +64,15 @@ class Accelerometer
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; }
bool enabled() const { return (_priority > 0); }
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
Expand All @@ -89,6 +91,8 @@ class Accelerometer
private:
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};

Rotation _rotation_enum{ROTATION_NONE};

matrix::Dcmf _rotation;
matrix::Vector3f _offset;
matrix::Vector3f _scale;
Expand Down
48 changes: 38 additions & 10 deletions src/lib/sensor_calibration/Gyroscope.cpp
View file
Open in desktop
Expand Up @@ -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;
}
}
}
Expand All @@ -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) {
Expand All @@ -131,20 +140,38 @@ 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
_priority = GetCalibrationParam(SensorString(), "PRIO", _calibration_index);

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,
Expand Down Expand Up @@ -177,6 +204,7 @@ void Gyroscope::ParametersUpdate()
void Gyroscope::Reset()
{
_rotation.setIdentity();
_rotation_enum = ROTATION_NONE;
_offset.zero();
_thermal_offset.zero();

Expand All @@ -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;
}
Expand Down
8 changes: 6 additions & 2 deletions src/lib/sensor_calibration/Gyroscope.hpp
View file
Open in desktop
Expand Up @@ -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"; }

Expand All @@ -63,13 +63,15 @@ class Gyroscope
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; }
bool enabled() const { return (_priority > 0); }
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
Expand All @@ -88,6 +90,8 @@ class Gyroscope
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;
Expand Down
6 changes: 3 additions & 3 deletions src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp
View file
Open in desktop
Expand Up @@ -47,13 +47,13 @@
using namespace time_literals;

static constexpr unsigned max_mandatory_gyro_count = 1;
static constexpr unsigned max_optional_gyro_count = 3;
static constexpr unsigned max_optional_gyro_count = 4;
static constexpr unsigned max_mandatory_accel_count = 1;
static constexpr unsigned max_optional_accel_count = 3;
static constexpr unsigned max_optional_accel_count = 4;
static constexpr unsigned max_mandatory_mag_count = 1;
static constexpr unsigned max_optional_mag_count = 4;
static constexpr unsigned max_mandatory_baro_count = 1;
static constexpr unsigned max_optional_baro_count = 1;
static constexpr unsigned max_optional_baro_count = 4;

bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
vehicle_status_flags_s &status_flags, const bool checkGNSS, bool reportFailures, const bool prearm,
Expand Down
9 changes: 8 additions & 1 deletion src/modules/commander/accelerometer_calibration.cpp
View file
Open in desktop
Expand Up @@ -149,7 +149,7 @@ using namespace matrix;
using namespace time_literals;

static constexpr char sensor_name[] {"accel"};
static constexpr unsigned MAX_ACCEL_SENS = 3;
static constexpr unsigned MAX_ACCEL_SENS = 4;

/// Data passed to calibration worker routine
typedef struct {
Expand All @@ -176,6 +176,7 @@ static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS
{ORB_ID(sensor_accel), 0, 0},
{ORB_ID(sensor_accel), 0, 1},
{ORB_ID(sensor_accel), 0, 2},
{ORB_ID(sensor_accel), 0, 3},
};

/* use the first sensor to pace the readout, but do per-sensor counts */
Expand All @@ -202,6 +203,9 @@ static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS
case 2:
offset = Vector3f{sensor_correction.accel_offset_2};
break;
case 3:
offset = Vector3f{sensor_correction.accel_offset_3};
break;
}
}
}
Expand Down Expand Up @@ -473,6 +477,9 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
case 2:
offset = Vector3f{sensor_correction.accel_offset_2};
break;
case 3:
offset = Vector3f{sensor_correction.accel_offset_3};
break;
}
}
}
Expand Down
6 changes: 5 additions & 1 deletion src/modules/commander/gyro_calibration.cpp
View file
Open in desktop
Expand Up @@ -59,7 +59,7 @@
#include <uORB/topics/sensor_gyro.h>

static constexpr char sensor_name[] {"gyro"};
static constexpr unsigned MAX_GYROS = 3;
static constexpr unsigned MAX_GYROS = 4;

using matrix::Vector3f;

Expand Down Expand Up @@ -101,6 +101,7 @@ static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data)
{ORB_ID(sensor_gyro), 0, 0},
{ORB_ID(sensor_gyro), 0, 1},
{ORB_ID(sensor_gyro), 0, 2},
{ORB_ID(sensor_gyro), 0, 3},
};

memset(&worker_data.last_sample_0_x, 0, sizeof(worker_data.last_sample_0_x));
Expand Down Expand Up @@ -148,6 +149,9 @@ static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data)
case 2:
offset = Vector3f{sensor_correction.gyro_offset_2};
break;
case 3:
offset = Vector3f{sensor_correction.gyro_offset_3};
break;
}
}
}
Expand Down