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

sensors (accel/gyro/mag) determine if external with device id

Browse Source
This commit is contained in:
Daniel Agar
2022-01-10 10:31:07 -05:00
committed by GitHub
parent 45040be669
commit e731fcdbc0
36 changed files with 187 additions and 205 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/mag_calibration.cpp
+2 -2
View File
@@ -540,7 +540,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
uORB::SubscriptionData<sensor_mag_s> mag_sub{ORB_ID(sensor_mag), cur_mag};
if (mag_sub.advertised() && (mag_sub.get().device_id != 0) && (mag_sub.get().timestamp > 0)) {
worker_data.calibration[cur_mag].set_device_id(mag_sub.get().device_id, mag_sub.get().is_external);
worker_data.calibration[cur_mag].set_device_id(mag_sub.get().device_id);
}
// reset calibration index to match uORB numbering
@@ -1016,7 +1016,7 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian
if (mag_sub.advertised() && (mag.timestamp != 0) && (mag.device_id != 0)) {
calibration::Magnetometer cal{mag.device_id, mag.is_external};
calibration::Magnetometer cal{mag.device_id};
// force calibration index to uORB index
cal.set_calibration_index(cur_mag);
src/modules/mag_bias_estimator/MagBiasEstimator.cpp
+1 -1
View File
@@ -170,7 +170,7 @@ void MagBiasEstimator::Run()
updated = true;
// apply existing mag calibration
_calibration[mag_index].set_device_id(sensor_mag.device_id, sensor_mag.is_external);
_calibration[mag_index].set_device_id(sensor_mag.device_id);
const Vector3f mag_calibrated = _calibration[mag_index].Correct(Vector3f{sensor_mag.x, sensor_mag.y, sensor_mag.z});
src/modules/mavlink/streams/MAG_CAL_REPORT.hpp
+1 -1
View File
@@ -73,7 +73,7 @@ private:
sensor_mag_s sensor_mag;
if (_sensor_mag_subs[mag].update(&sensor_mag) && (sensor_mag.device_id != 0)) {
calibration::Magnetometer calibration{sensor_mag.device_id, sensor_mag.is_external};
calibration::Magnetometer calibration{sensor_mag.device_id};
if (calibration.calibrated()) {
mavlink_mag_cal_report_t msg{};
src/modules/sensors/sensors.cpp
+14 -16
View File
@@ -325,41 +325,39 @@ int Sensors::parameters_update()
uint32_t device_id_mag = calibration::GetCalibrationParamInt32("MAG", "ID", i);
if (device_id_accel != 0) {
bool external_accel = (calibration::GetCalibrationParamInt32("ACC", "ROT", i) >= 0);
calibration::Accelerometer accel_cal(device_id_accel, external_accel);
calibration::Accelerometer accel_cal(device_id_accel);
}
if (device_id_gyro != 0) {
bool external_gyro = (calibration::GetCalibrationParamInt32("GYRO", "ROT", i) >= 0);
calibration::Gyroscope gyro_cal(device_id_gyro, external_gyro);
calibration::Gyroscope gyro_cal(device_id_gyro);
}
if (device_id_mag != 0) {
bool external_mag = (calibration::GetCalibrationParamInt32("MAG", "ROT", i) >= 0);
calibration::Magnetometer mag_cal(device_id_mag, external_mag);
calibration::Magnetometer mag_cal(device_id_mag);
}
}
// ensure calibration slots are active for the number of sensors currently available
// this to done to eliminate differences in the active set of parameters before and after sensor calibration
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if (orb_exists(ORB_ID(sensor_accel), i) == PX4_OK) {
bool external = (calibration::GetCalibrationParamInt32("ACC", "ROT", i) >= 0);
calibration::Accelerometer cal{0, external};
for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
uORB::SubscriptionData<sensor_accel_s> sensor_accel_sub{ORB_ID(sensor_accel), i};
uORB::SubscriptionData<sensor_gyro_s> sensor_gyro_sub{ORB_ID(sensor_gyro), i};
uORB::SubscriptionData<sensor_mag_s> sensor_mag_sub{ORB_ID(sensor_mag), i};
if (sensor_accel_sub.get().device_id != 0) {
calibration::Accelerometer cal{sensor_accel_sub.get().device_id};
cal.set_calibration_index(i);
cal.ParametersUpdate();
}
if (orb_exists(ORB_ID(sensor_gyro), i) == PX4_OK) {
bool external = (calibration::GetCalibrationParamInt32("GYRO", "ROT", i) >= 0);
calibration::Gyroscope cal{0, external};
if (sensor_gyro_sub.get().device_id != 0) {
calibration::Gyroscope cal{sensor_gyro_sub.get().device_id};
cal.set_calibration_index(i);
cal.ParametersUpdate();
}
if (orb_exists(ORB_ID(sensor_mag), i) == PX4_OK) {
bool external = (calibration::GetCalibrationParamInt32("MAG", "ROT", i) >= 0);
calibration::Magnetometer cal{0, external};
if (sensor_mag_sub.get().device_id != 0) {
calibration::Magnetometer cal{sensor_mag_sub.get().device_id};
cal.set_calibration_index(i);
cal.ParametersUpdate();
}
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
+1 -1
View File
@@ -432,7 +432,7 @@ void VehicleMagnetometer::Run()
while (_sensor_sub[uorb_index].update(&report)) {
if (_calibration[uorb_index].device_id() != report.device_id) {
_calibration[uorb_index].set_device_id(report.device_id, report.is_external);
_calibration[uorb_index].set_device_id(report.device_id);
_priority[uorb_index] = _calibration[uorb_index].priority();
}
src/modules/simulator/sensor_mag_sim/SensorMagSim.cpp
-1
View File
@@ -43,7 +43,6 @@ SensorMagSim::SensorMagSim() :
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
{
_px4_mag.set_device_type(DRV_MAG_DEVTYPE_MAGSIM);
_px4_mag.set_external(false);
}
SensorMagSim::~SensorMagSim()