UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 21:09:41 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix(sensors): af9838 minor code style and architecture updates (#27370)

Browse Source 
* drivers: af9838 minor fixes and code style updates

* Update af9838_main.cpp

* Update af9838.cpp

* Update af9838.hpp
This commit is contained in:
Voltafield
2026-05-21 09:53:20 +08:00
committed by GitHub
parent ee002b1db6
commit 2cea093fdb
3 changed files with 3 additions and 85 deletions
Download Patch File Download Diff File Expand all files Collapse all files

64
src/drivers/magnetometer/voltafield/af9838/af9838.cpp
View File

@@ -31,63 +31,9 @@
****************************************************************************/
#include "af9838.hpp"
#ifdef VTC_DRIVER_MED_FILTER
void AF9838_Driver::af9838_median_filter(float *mag)
{
uint16_t i, j, k;
float mft_tmp[3][MED_FILTER_MAX_NUM];
float tmp;
if (_mdf_init_idx < _mdf_num) {
for (i = 0; i < 3; i++) {
_mdf_data[i][_mdf_init_idx] = mag[i];
}
_mdf_init_idx++;
return;
} else {
for (i = 0; i < 3; i++) {
_mdf_data[i][_mdf_data_idx] = mag[i];
}
}
if (++_mdf_data_idx >= _mdf_num) {
_mdf_data_idx = 0;
}
for (i = 0; i < 3; i++) {
for (j = 0; j < _mdf_num; j++) {
mft_tmp[i][j] = _mdf_data[i][j];
}
}
for (i = 0; i < 3; i++) {
for (j = 0; j < (_mdf_num - 1); j++) {
for (k = 0; k < (_mdf_num - 1 - j); k++) {
if (mft_tmp[i][k] < mft_tmp[i][k + 1]) {
tmp = mft_tmp[i][k];
mft_tmp[i][k] = mft_tmp[i][k + 1];
mft_tmp[i][k + 1] = tmp;
}
}
}
}
for (i = 0; i < 3; i++) {
mag[i] = mft_tmp[i][(uint16_t)((_mdf_num - 1) / 2)];
}
}
#endif
using namespace AF9838;
using namespace time_literals;
float AF9838::g_cli_rate_hz = 100.0f;
AF9838_Driver::AF9838_Driver(const I2CSPIDriverConfig &config)
: device::I2C(config),
I2CSPIDriver(config),
@@ -159,7 +105,7 @@ int AF9838_Driver::init()
return PX4_ERROR;
}
float hz = g_cli_rate_hz;
float hz = AF9838::DEFAULT_RATE_HZ;
if (hz < 1.f) { hz = 1.f; }
@@ -269,14 +215,6 @@ void AF9838_Driver::RunImpl()
my_uT = ry * AF9838::LSB_TO_uT;
mz_uT = rz * AF9838::LSB_TO_uT;
#ifdef VTC_DRIVER_MED_FILTER
float mag_uT[3] = { mx_uT, my_uT, mz_uT };
af9838_median_filter(mag_uT);
mx_uT = mag_uT[0];
my_uT = mag_uT[1];
mz_uT = mag_uT[2];
#endif
mx_G = mx_uT * AF9838::uT_TO_G;
my_G = my_uT * AF9838::uT_TO_G;
mz_G = mz_uT * AF9838::uT_TO_G;

13
src/drivers/magnetometer/voltafield/af9838/af9838.hpp
View File

@@ -53,7 +53,7 @@
namespace AF9838
{
extern float g_cli_rate_hz;
static constexpr float DEFAULT_RATE_HZ = 100.0f;
static constexpr float LSB_TO_uT = 0.1f; // micro-Tesla / LSB
static constexpr float uT_TO_G = 0.01f; // µT → Gauss
static constexpr float ONE_G = 9.80665f;
@@ -97,17 +97,6 @@ private:
bool _running{false};
bool _waiting_data{false};
hrt_abstime _last_trigger{0};
bool _algo_inited{false};
static constexpr uint16_t AF9838_MED_FILTER_MAX_NUM = 15;
static constexpr uint16_t AF9838_MED_FILTER_NUMBER = 5;
uint16_t _mdf_num = AF9838_MED_FILTER_NUMBER;
uint16_t _mdf_init_idx = 0;
uint16_t _mdf_data_idx = 0;
float _mdf_data[3][AF9838_MED_FILTER_MAX_NUM] {};
void af9838_median_filter(float mag[3]);
uORB::Subscription _accel_sub {ORB_ID(vehicle_acceleration)};
hrt_abstime _last_accel_ts{0};

11
src/drivers/magnetometer/voltafield/af9838/af9838_main.cpp
View File

@@ -42,7 +42,6 @@ void AF9838_Driver::print_usage()
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x0C);
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
PRINT_MODULE_USAGE_PARAM_FLOAT('r', 100.f, 1.f, 1000.f, "Output rate (Hz)", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
@@ -59,19 +58,11 @@ extern "C" __EXPORT int af9838_main(int argc, char *argv[])
cli.i2c_address = I2C_ADDRESS_DEFAULT;
cli.default_i2c_frequency = I2C_SPEED;
AF9838::g_cli_rate_hz = 100.0f;
while ((ch = cli.getOpt(argc, argv, "R:r:")) != EOF) {
while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
switch (ch) {
case 'R':
cli.rotation = (enum Rotation)atoi(cli.optArg());
break;
case 'r': {
const float rate = atof(cli.optArg());
AF9838::g_cli_rate_hz = rate;
break;
}
}
}