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

ist8310: move to PX4Magnetometer and cleanup

Browse Source
This commit is contained in:
Daniel Agar
2020-04-01 10:07:08 -04:00
parent 1c333cb664
commit cf12969a29
2 changed files with 41 additions and 368 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/magnetometer/ist8310/CMakeLists.txt
+1 -2
View File
@@ -33,10 +33,9 @@
px4_add_module(
MODULE drivers__ist8310
MAIN ist8310
COMPILE_FLAGS
-Wno-cast-align # TODO: fix and enable
SRCS
ist8310.cpp
DEPENDS
drivers_magnetometer
px4_work_queue
)
src/drivers/magnetometer/ist8310/ist8310.cpp
+40 -366
View File
@@ -46,40 +46,13 @@
#include <px4_platform_common/i2c_spi_buses.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <sys/types.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <nuttx/arch.h>
#include <nuttx/clock.h>
#include <board_config.h>
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <lib/perf/perf_counter.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/drv_device.h>
#include <uORB/uORB.h>
#include <float.h>
#include <lib/conversion/rotation.h>
/*
* IST8310 internal constants and data structures.
*/
@@ -188,9 +161,6 @@ public:
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Initialise the automatic measurement state machine and start it.
*
@@ -219,35 +189,22 @@ public:
*/
void RunImpl();
protected:
private:
int probe() override;
void print_status() override;
private:
PX4Magnetometer _px4_mag;
unsigned _measure_interval{IST8310_CONVERSION_INTERVAL};
ringbuffer::RingBuffer *_reports{nullptr};
struct mag_calibration_s _scale {};
float _range_scale{0.003f}; /* default range scale from counts to gauss */
bool _collect_phase{false};
int _class_instance{-1};
int _orb_class_instance{-1};
orb_advert_t _mag_topic{nullptr};
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _range_errors;
perf_counter_t _conf_errors;
enum Rotation _rotation;
sensor_mag_s _last_report{}; /**< used for info() */
uint8_t _ctl3_reg{0};
uint8_t _ctl4_reg{0};
@@ -258,7 +215,7 @@ private:
* cope with communication errors causing the configuration to
* change
*/
void check_conf(void);
void check_conf();
/**
* Write a register.
@@ -309,61 +266,28 @@ private:
* Collect the result of the most recent measurement.
*/
int collect();
/**
* Convert a big-endian signed 16-bit value to a float.
*
* @param in A signed 16-bit big-endian value.
* @return The floating-point representation of the value.
*/
float meas_to_float(uint8_t in[2]);
/**
* Place the device in self test mode
*
* @return 0 if mode is set, 1 else
*/
int set_selftest(unsigned enable);
/* this class has pointer data members, do not allow copying it */
IST8310(const IST8310 &);
IST8310 operator=(const IST8310 &);
};
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int ist8310_main(int argc, char *argv[]);
IST8310::IST8310(I2CSPIBusOption bus_option, int bus_number, int address, enum Rotation rotation, int bus_frequency) :
I2C("IST8310", nullptr, bus_number, address, bus_frequency),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus_number, address),
_sample_perf(perf_alloc(PC_ELAPSED, "ist8310_read")),
_comms_errors(perf_alloc(PC_COUNT, "ist8310_com_err")),
_range_errors(perf_alloc(PC_COUNT, "ist8310_rng_err")),
_conf_errors(perf_alloc(PC_COUNT, "ist8310_conf_err")),
_rotation(rotation)
_px4_mag(get_device_id(), external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT, rotation),
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")),
_range_errors(perf_alloc(PC_COUNT, MODULE_NAME": rng_err")),
_conf_errors(perf_alloc(PC_COUNT, MODULE_NAME": conf_err"))
{
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_IST8310;
set_device_type(DRV_MAG_DEVTYPE_IST8310);
// default scaling
_scale.x_offset = 0;
_scale.x_scale = 1.0f;
_scale.y_offset = 0;
_scale.y_scale = 1.0f;
_scale.z_offset = 0;
_scale.z_scale = 1.0f;
_px4_mag.set_device_type(DRV_MAG_DEVTYPE_IST8310);
_px4_mag.set_external(external());
// default range scale from counts to gauss
_px4_mag.set_scale(0.003f);
}
IST8310::~IST8310()
{
delete _reports;
if (_class_instance != -1) {
unregister_class_devname(MAG_BASE_DEVICE_PATH, _class_instance);
}
// free perf counters
perf_free(_sample_perf);
perf_free(_comms_errors);
@@ -371,37 +295,24 @@ IST8310::~IST8310()
perf_free(_conf_errors);
}
int
IST8310::init()
int IST8310::init()
{
int ret = PX4_ERROR;
ret = I2C::init();
int ret = I2C::init();
if (ret != OK) {
DEVICE_DEBUG("CDev init failed");
goto out;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_mag_s));
if (_reports == nullptr) {
DEVICE_DEBUG("I2C init failed");
goto out;
}
/* reset the device configuration */
reset();
_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
ret = OK;
out:
return ret;
}
int
IST8310::write(unsigned address, void *data, unsigned count)
int IST8310::write(unsigned address, void *data, unsigned count)
{
uint8_t buf[32];
@@ -415,20 +326,18 @@ IST8310::write(unsigned address, void *data, unsigned count)
return transfer(&buf[0], count + 1, nullptr, 0);
}
int
IST8310::read(unsigned address, void *data, unsigned count)
int IST8310::read(unsigned address, void *data, unsigned count)
{
uint8_t cmd = address;
return transfer(&cmd, 1, (uint8_t *)data, count);
}
/**
check that the configuration register has the right value. This is
done periodically to cope with I2C bus noise causing the
configuration of the compass to change.
*/
void IST8310::check_conf(void)
void IST8310::check_conf()
{
int ret;
@@ -466,155 +375,16 @@ void IST8310::check_conf(void)
}
}
ssize_t
IST8310::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(sensor_mag_s);
sensor_mag_s *mag_buf = reinterpret_cast<sensor_mag_s *>(buffer);
int ret = 0;
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is enabled */
if (_measure_interval > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
* Note that we may be pre-empted by the workq thread while we are doing this;
* we are careful to avoid racing with them.
*/
while (count--) {
if (_reports->get(mag_buf)) {
ret += sizeof(sensor_mag_s);
mag_buf++;
}
}
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
/* manual measurement - run one conversion */
/* XXX really it'd be nice to lock against other readers here */
do {
_reports->flush();
/* trigger a measurement */
if (OK != measure()) {
ret = -EIO;
break;
}
/* wait for it to complete */
usleep(IST8310_CONVERSION_INTERVAL);
/* run the collection phase */
if (OK != collect()) {
ret = -EIO;
break;
}
if (_reports->get(mag_buf)) {
ret = sizeof(sensor_mag_s);
}
} while (0);
return ret;
}
int
IST8310::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* zero would be bad */
case 0:
return -EINVAL;
/* set default polling rate */
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_interval == 0);
/* set interval for next measurement to minimum legal value */
_measure_interval = IST8310_CONVERSION_INTERVAL;
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_interval == 0);
/* convert hz to tick interval via microseconds */
unsigned interval = (1000000 / arg);
/* check against maximum rate */
if (interval < IST8310_CONVERSION_INTERVAL) {
return -EINVAL;
}
/* update interval for next measurement */
_measure_interval = interval;
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
}
}
case SENSORIOCRESET:
return reset();
case MAGIOCEXSTRAP:
return set_selftest(arg);
case MAGIOCSSCALE:
/* set new scale factors */
memcpy(&_scale, (struct mag_calibration_s *)arg, sizeof(_scale));
return 0;
case MAGIOCGSCALE:
/* copy out scale factors */
memcpy((struct mag_calibration_s *)arg, &_scale, sizeof(_scale));
return 0;
case MAGIOCGEXTERNAL:
return external();
default:
/* give it to the superclass */
return CDev::ioctl(filp, cmd, arg);
}
}
void
IST8310::start()
void IST8310::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
_reports->flush();
/* schedule a cycle to start things */
ScheduleNow();
}
int
IST8310::reset()
int IST8310::reset()
{
/* software reset */
write_reg(ADDR_CTRL2, CTRL2_SRST);
@@ -630,8 +400,7 @@ IST8310::reset()
return OK;
}
int
IST8310::probe()
int IST8310::probe()
{
uint8_t data[1] = {0};
@@ -652,8 +421,7 @@ IST8310::probe()
return OK;
}
void
IST8310::RunImpl()
void IST8310::RunImpl()
{
/* collection phase? */
if (_collect_phase) {
@@ -693,8 +461,7 @@ IST8310::RunImpl()
ScheduleDelayed(IST8310_CONVERSION_INTERVAL);
}
int
IST8310::measure()
int IST8310::measure()
{
/*
* Send the command to begin a measurement.
@@ -708,37 +475,28 @@ IST8310::measure()
return ret;
}
int
IST8310::collect()
int IST8310::collect()
{
#pragma pack(push, 1)
struct { /* status register and data as read back from the device */
uint8_t x[2];
uint8_t y[2];
uint8_t z[2];
} report_buffer;
#pragma pack(pop)
} report_buffer{};
struct {
int16_t x, y, z;
} report;
} report{};
int ret;
uint8_t check_counter;
perf_begin(_sample_perf);
sensor_mag_s new_report;
const bool sensor_is_external = external();
float xraw_f;
float yraw_f;
float zraw_f;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
new_report.timestamp = hrt_absolute_time();
new_report.is_external = sensor_is_external;
new_report.error_count = perf_event_count(_comms_errors);
new_report.scaling = _range_scale;
new_report.device_id = _device_id.devid;
_px4_mag.set_error_count(perf_event_count(_comms_errors));
/*
* @note We could read the status register here, which could tell us that
@@ -748,6 +506,7 @@ IST8310::collect()
*/
/* get measurements from the device */
const hrt_abstime timestamp_sample = hrt_absolute_time();
ret = read(ADDR_DATA_OUT_X_LSB, (uint8_t *)&report_buffer, sizeof(report_buffer));
if (ret != OK) {
@@ -761,7 +520,6 @@ IST8310::collect()
report.y = (((int16_t)report_buffer.y[1]) << 8) | (int16_t)report_buffer.y[0];
report.z = (((int16_t)report_buffer.z[1]) << 8) | (int16_t)report_buffer.z[0];
/*
* Check if value makes sense according to the FSR and Resolution of
* this sensor, discarding outliers
@@ -774,50 +532,17 @@ IST8310::collect()
goto out;
}
/* temperature measurement is not available on IST8310 */
new_report.temperature = 0;
/*
* raw outputs
*
* Sensor doesn't follow right hand rule, swap x and y to make it obey
* it.
*/
new_report.x_raw = report.y;
new_report.y_raw = report.x;
new_report.z_raw = report.z;
/* scale values for output */
xraw_f = report.y;
yraw_f = report.x;
zraw_f = report.z;
/* apply user specified rotation */
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
new_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale;
new_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale;
new_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale;
if (!(_pub_blocked)) {
if (_mag_topic != nullptr) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
} else {
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report,
&_orb_class_instance, sensor_is_external ? ORB_PRIO_MAX : ORB_PRIO_HIGH);
if (_mag_topic == nullptr) {
DEVICE_DEBUG("ADVERT FAIL");
}
}
}
_last_report = new_report;
/* post a report to the ring */
_reports->force(&new_report);
_px4_mag.update(timestamp_sample, xraw_f, yraw_f, zraw_f);
/*
periodically check the range register and configuration
@@ -839,46 +564,13 @@ out:
return ret;
}
int
IST8310::set_selftest(unsigned enable)
{
int ret;
uint8_t str;
/* arm the excitement strap */
ret = read_reg(ADDR_STR, str);
if (OK != ret) {
perf_count(_comms_errors);
}
str &= ~STR_SELF_TEST_ON; // reset previous test
if (enable > 0) {
str |= STR_SELF_TEST_ON;
}
ret = write_reg(ADDR_STR, str);
if (OK != ret) {
perf_count(_comms_errors);
}
uint8_t str_reg_ret = 0;
read_reg(ADDR_STR, str_reg_ret);
return !(str == str_reg_ret);
}
int
IST8310::write_reg(uint8_t reg, uint8_t val)
int IST8310::write_reg(uint8_t reg, uint8_t val)
{
uint8_t buf = val;
return write(reg, &buf, 1);
}
int
IST8310::read_reg(uint8_t reg, uint8_t &val)
int IST8310::read_reg(uint8_t reg, uint8_t &val)
{
uint8_t buf = 0;
int ret = read(reg, &buf, 1);
@@ -886,29 +578,13 @@ IST8310::read_reg(uint8_t reg, uint8_t &val)
return ret;
}
float
IST8310::meas_to_float(uint8_t in[2])
{
union {
uint8_t b[2];
int16_t w;
} u;
u.b[0] = in[1];
u.b[1] = in[0];
return (float) u.w;
}
void
IST8310::print_status()
void IST8310::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
printf("poll interval: %u interval\n", _measure_interval);
print_message(_last_report);
_reports->print_info("report queue");
_px4_mag.print_status();
}
I2CSPIDriverBase *
@@ -933,8 +609,7 @@ IST8310::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iter
return interface;
}
void
IST8310::print_usage()
void IST8310::print_usage()
{
PRINT_MODULE_USAGE_NAME("ist8310", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer");
@@ -945,8 +620,7 @@ IST8310::print_usage()
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
int
ist8310_main(int argc, char *argv[])
extern "C" __EXPORT int ist8310_main(int argc, char *argv[])
{
int ch;
using ThisDriver = IST8310;