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

Use accel of the same instance or primary baro for gyro instances that do not have valid temperature readings in temperature calibration data, use primary baro for magnetometers without valid temperature.

Browse Source
This commit is contained in:
mcsauder
2023-06-07 12:07:29 -04:00
committed by Daniel Agar
parent b8ad9bdbe5
commit af44da25f0
19 changed files with 814 additions and 717 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/magnetometer/isentek/ist8308/IST8308.cpp
-19
View File
@@ -220,13 +220,6 @@ void IST8308::RunImpl()
perf_count(_bad_register_perf);
Reset();
}
} else {
// periodically update temperature (~1 Hz)
if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
UpdateTemperature();
_temperature_update_timestamp = now;
}
}
}
@@ -298,15 +291,3 @@ void IST8308::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t cle
RegisterWrite(reg, val);
}
}
void IST8308::UpdateTemperature()
{
// Isentek magnetometers do not have a temperature sensor, so we will to use the baro's value.
sensor_baro_s sensor_baro;
if (_sensor_baro_sub[0].update(&sensor_baro)) {
if (PX4_ISFINITE(sensor_baro.temperature)) {
_px4_mag.set_temperature(sensor_baro.temperature);
}
}
}
src/drivers/magnetometer/isentek/ist8308/IST8308.hpp
+7 -21
View File
@@ -46,10 +46,7 @@
#include <lib/drivers/device/i2c.h>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <lib/perf/perf_counter.h>
#include <lib/sensor_calibration/Barometer.hpp>
#include <px4_platform_common/i2c_spi_buses.h>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/sensor_baro.h>
using namespace iSentek_IST8308;
@@ -64,17 +61,9 @@ public:
void RunImpl();
int init() override;
void print_status() override;
private:
enum class STATE : uint8_t {
RESET,
WAIT_FOR_RESET,
CONFIGURE,
READ,
} _state{STATE::RESET};
// Sensor Configuration
struct register_config_t {
Register reg;
@@ -91,15 +80,9 @@ private:
bool RegisterCheck(const register_config_t &reg_cfg);
uint8_t RegisterRead(Register reg);
void RegisterWrite(Register reg, uint8_t value);
void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
void UpdateTemperature();
uORB::SubscriptionMultiArray<sensor_baro_s, calibration::Barometer::MAX_SENSOR_COUNT> _sensor_baro_sub{ORB_ID::sensor_baro};
PX4Magnetometer _px4_mag;
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
@@ -108,14 +91,17 @@ private:
hrt_abstime _reset_timestamp{0};
hrt_abstime _last_config_check_timestamp{0};
hrt_abstime _temperature_update_timestamp{0};
int _failure_count{0};
enum class STATE : uint8_t {
RESET,
WAIT_FOR_RESET,
CONFIGURE,
READ,
} _state{STATE::RESET};
uint8_t _checked_register{0};
static constexpr uint8_t size_register_cfg{6};
register_config_t _register_cfg[size_register_cfg] {
// Register | Set bits, Clear bits
{ Register::ACTR, 0, ACTR_BIT::SUSPEND_EN },
src/drivers/magnetometer/isentek/ist8310/IST8310.cpp
-20
View File
@@ -188,7 +188,6 @@ void IST8310::RunImpl()
_px4_mag.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf));
_px4_mag.update(now, x, y, z);
UpdateTemperature();
success = true;
@@ -223,13 +222,6 @@ void IST8310::RunImpl()
Reset();
return;
}
} else {
// periodically update temperature (~1 Hz)
if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
UpdateTemperature();
_temperature_update_timestamp = now;
}
}
// initiate next measurement
@@ -304,15 +296,3 @@ void IST8310::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t cle
RegisterWrite(reg, val);
}
}
void IST8310::UpdateTemperature()
{
// Isentek magnetometers do not have a temperature sensor, so we will to use the baro's value.
sensor_baro_s sensor_baro;
if (_sensor_baro_sub[0].update(&sensor_baro)) {
if (PX4_ISFINITE(sensor_baro.temperature)) {
_px4_mag.set_temperature(sensor_baro.temperature);
}
}
}
src/drivers/magnetometer/isentek/ist8310/IST8310.hpp
+8 -22
View File
@@ -46,10 +46,7 @@
#include <lib/drivers/device/i2c.h>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <lib/perf/perf_counter.h>
#include <lib/sensor_calibration/Barometer.hpp>
#include <px4_platform_common/i2c_spi_buses.h>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/sensor_baro.h>
using namespace iSentek_IST8310;
@@ -64,18 +61,9 @@ public:
void RunImpl();
int init() override;
void print_status() override;
private:
enum class STATE : uint8_t {
RESET,
WAIT_FOR_RESET,
CONFIGURE,
MEASURE,
READ,
} _state{STATE::RESET};
// Sensor Configuration
struct register_config_t {
Register reg;
@@ -92,15 +80,9 @@ private:
bool RegisterCheck(const register_config_t &reg_cfg);
uint8_t RegisterRead(Register reg);
void RegisterWrite(Register reg, uint8_t value);
void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
void UpdateTemperature();
uORB::SubscriptionMultiArray<sensor_baro_s, calibration::Barometer::MAX_SENSOR_COUNT> _sensor_baro_sub{ORB_ID::sensor_baro};
PX4Magnetometer _px4_mag;
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
@@ -109,14 +91,18 @@ private:
hrt_abstime _reset_timestamp{0};
hrt_abstime _last_config_check_timestamp{0};
hrt_abstime _temperature_update_timestamp{0};
int _failure_count{0};
enum class STATE : uint8_t {
RESET,
WAIT_FOR_RESET,
CONFIGURE,
MEASURE,
READ,
} _state{STATE::RESET};
uint8_t _checked_register{0};
static constexpr uint8_t size_register_cfg{4};
register_config_t _register_cfg[size_register_cfg] {
// Register | Set bits, Clear bits
{ Register::CNTL2, 0, CNTL2_BIT::SRST },