mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-03 16:48:52 +08:00
450 lines
12 KiB
C++
450 lines
12 KiB
C++
/****************************************************************************
|
||
*
|
||
* Copyright (c) 2017-2019 PX4 Development Team. All rights reserved.
|
||
*
|
||
* Redistribution and use in source and binary forms, with or without
|
||
* modification, are permitted provided that the following conditions
|
||
* are met:
|
||
*
|
||
* 1. Redistributions of source code must retain the above copyright
|
||
* notice, this list of conditions and the following disclaimer.
|
||
* 2. Redistributions in binary form must reproduce the above copyright
|
||
* notice, this list of conditions and the following disclaimer in
|
||
* the documentation and/or other materials provided with the
|
||
* distribution.
|
||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||
* used to endorse or promote products derived from this software
|
||
* without specific prior written permission.
|
||
*
|
||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||
* POSSIBILITY OF SUCH DAMAGE.
|
||
*
|
||
****************************************************************************/
|
||
|
||
/**
|
||
* @file fxos8701cq.cpp
|
||
* Driver for the NXP FXOS8701CQ 6-axis sensor with integrated linear accelerometer and
|
||
* magnetometer connected via SPI.
|
||
*/
|
||
|
||
#include "FXOS8701CQ.hpp"
|
||
|
||
using namespace time_literals;
|
||
|
||
/*
|
||
list of registers that will be checked in check_registers(). Note
|
||
that ADDR_WHO_AM_I must be first in the list.
|
||
*/
|
||
const uint8_t FXOS8701CQ::_checked_registers[FXOS8701C_NUM_CHECKED_REGISTERS] = {
|
||
FXOS8701CQ_WHOAMI,
|
||
FXOS8701CQ_XYZ_DATA_CFG,
|
||
FXOS8701CQ_CTRL_REG1,
|
||
FXOS8701CQ_M_CTRL_REG1,
|
||
FXOS8701CQ_M_CTRL_REG2,
|
||
};
|
||
|
||
FXOS8701CQ::FXOS8701CQ(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
|
||
spi_mode_e spi_mode) :
|
||
SPI(DRV_ACC_DEVTYPE_FXOS8701C, MODULE_NAME, bus, device, spi_mode, bus_frequency),
|
||
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
|
||
_px4_accel(get_device_id(), ORB_PRIO_LOW, rotation),
|
||
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
|
||
_px4_mag(get_device_id(), ORB_PRIO_LOW, rotation),
|
||
_mag_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": mag read")),
|
||
#endif
|
||
_accel_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": acc read")),
|
||
_bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad reg")),
|
||
_accel_duplicates(perf_alloc(PC_COUNT, MODULE_NAME": acc dupe"))
|
||
{
|
||
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
|
||
_px4_mag.set_scale(0.001f);
|
||
#endif
|
||
}
|
||
|
||
FXOS8701CQ::~FXOS8701CQ()
|
||
{
|
||
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
|
||
perf_free(_mag_sample_perf);
|
||
#endif
|
||
|
||
// delete the perf counter
|
||
perf_free(_accel_sample_perf);
|
||
perf_free(_bad_registers);
|
||
perf_free(_accel_duplicates);
|
||
}
|
||
|
||
int
|
||
FXOS8701CQ::init()
|
||
{
|
||
// do SPI init (and probe) first
|
||
int ret = SPI::init();
|
||
|
||
if (ret != OK) {
|
||
PX4_ERR("SPI init failed");
|
||
return ret;
|
||
}
|
||
|
||
reset();
|
||
|
||
start();
|
||
|
||
return PX4_OK;
|
||
}
|
||
|
||
void
|
||
FXOS8701CQ::reset()
|
||
{
|
||
// enable accel set it To Standby
|
||
write_checked_reg(FXOS8701CQ_CTRL_REG1, 0);
|
||
write_checked_reg(FXOS8701CQ_XYZ_DATA_CFG, 0);
|
||
|
||
// Use hybird mode to read Accel and Mag
|
||
write_checked_reg(FXOS8701CQ_M_CTRL_REG1, M_CTRL_REG1_HMS_AM | M_CTRL_REG1_OS(7));
|
||
|
||
// Use the hybird auto increment mode to read all the data at the same time
|
||
write_checked_reg(FXOS8701CQ_M_CTRL_REG2, CTRL_REG2_AUTO_INC);
|
||
|
||
accel_set_range(FXOS8701C_ACCEL_DEFAULT_RANGE_G);
|
||
accel_set_samplerate(FXOS8701C_ACCEL_DEFAULT_RATE);
|
||
|
||
// enable set it To Standby mode at 800 Hz which becomes 400 Hz due to hybird mode
|
||
write_checked_reg(FXOS8701CQ_CTRL_REG1, CTRL_REG1_DR(0) | CTRL_REG1_ACTIVE);
|
||
}
|
||
|
||
int
|
||
FXOS8701CQ::probe()
|
||
{
|
||
// verify that the device is attached and functioning
|
||
uint8_t whoami = read_reg(FXOS8701CQ_WHOAMI);
|
||
bool success = (whoami == FXOS8700CQ_WHOAMI_VAL) || (whoami == FXOS8701CQ_WHOAMI_VAL);
|
||
|
||
if (success) {
|
||
_checked_values[0] = whoami;
|
||
return OK;
|
||
}
|
||
|
||
return -EIO;
|
||
}
|
||
|
||
uint8_t
|
||
FXOS8701CQ::read_reg(unsigned reg)
|
||
{
|
||
uint8_t cmd[3];
|
||
|
||
cmd[0] = DIR_READ(reg);
|
||
cmd[1] = ADDR_7(reg);
|
||
cmd[2] = 0;
|
||
|
||
transfer(cmd, cmd, sizeof(cmd));
|
||
|
||
return cmd[2];
|
||
}
|
||
|
||
void
|
||
FXOS8701CQ::write_reg(unsigned reg, uint8_t value)
|
||
{
|
||
uint8_t cmd[3];
|
||
|
||
cmd[0] = DIR_WRITE(reg);
|
||
cmd[1] = ADDR_7(reg);
|
||
cmd[2] = value;
|
||
|
||
transfer(cmd, nullptr, sizeof(cmd));
|
||
}
|
||
|
||
void
|
||
FXOS8701CQ::write_checked_reg(unsigned reg, uint8_t value)
|
||
{
|
||
write_reg(reg, value);
|
||
|
||
for (uint8_t i = 0; i < FXOS8701C_NUM_CHECKED_REGISTERS; i++) {
|
||
if (reg == _checked_registers[i]) {
|
||
_checked_values[i] = value;
|
||
}
|
||
}
|
||
}
|
||
|
||
void
|
||
FXOS8701CQ::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
|
||
{
|
||
uint8_t val = read_reg(reg);
|
||
val &= ~clearbits;
|
||
val |= setbits;
|
||
write_checked_reg(reg, val);
|
||
}
|
||
|
||
int
|
||
FXOS8701CQ::accel_set_range(unsigned max_g)
|
||
{
|
||
uint8_t setbits = 0;
|
||
float lsb_per_g;
|
||
|
||
if (max_g == 0 || max_g > 8) {
|
||
max_g = 8;
|
||
}
|
||
|
||
if (max_g > 4) { // 8g
|
||
setbits = XYZ_DATA_CFG_FS_8G;
|
||
lsb_per_g = 1024;
|
||
//max_accel_g = 8;
|
||
|
||
} else if (max_g > 2) { // 4g
|
||
setbits = XYZ_DATA_CFG_FS_4G;
|
||
lsb_per_g = 2048;
|
||
//max_accel_g = 4;
|
||
|
||
} else { // 2g
|
||
setbits = XYZ_DATA_CFG_FS_2G;
|
||
lsb_per_g = 4096;
|
||
//max_accel_g = 2;
|
||
}
|
||
|
||
float accel_range_scale = (CONSTANTS_ONE_G / lsb_per_g);
|
||
|
||
modify_reg(FXOS8701CQ_XYZ_DATA_CFG, XYZ_DATA_CFG_FS_MASK, setbits);
|
||
|
||
_px4_accel.set_scale(accel_range_scale);
|
||
|
||
return OK;
|
||
}
|
||
|
||
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
|
||
int
|
||
FXOS8701CQ::mag_set_range(unsigned max_ga)
|
||
{
|
||
// mag_range_ga = 12;
|
||
float mag_range_scale = 0.001f;
|
||
|
||
_px4_mag.set_scale(mag_range_scale);
|
||
|
||
return OK;
|
||
}
|
||
#endif
|
||
|
||
int
|
||
FXOS8701CQ::accel_set_samplerate(unsigned frequency)
|
||
{
|
||
uint8_t setbits = 0;
|
||
|
||
// The selected ODR is reduced by a factor of two when the device is operated in hybrid mode.
|
||
uint8_t active = read_reg(FXOS8701CQ_CTRL_REG1) & CTRL_REG1_ACTIVE;
|
||
|
||
if (frequency == 0) {
|
||
frequency = FXOS8701C_ACCEL_DEFAULT_RATE;
|
||
}
|
||
|
||
if (frequency <= 25) {
|
||
setbits = CTRL_REG1_DR(4); // Use 50 as it is 50 / 2
|
||
_accel_samplerate = 25;
|
||
|
||
} else if (frequency <= 50) {
|
||
setbits = CTRL_REG1_DR(3); // Use 100 as it is 100 / 2
|
||
_accel_samplerate = 50;
|
||
|
||
} else if (frequency <= 100) {
|
||
setbits = CTRL_REG1_DR(2); // Use 200 as it is 200 / 2
|
||
_accel_samplerate = 100;
|
||
|
||
} else if (frequency <= 200) {
|
||
setbits = CTRL_REG1_DR(1); // Use 400 as it is 400 / 2;
|
||
_accel_samplerate = 200;
|
||
|
||
} else if (frequency <= 400) {
|
||
setbits = CTRL_REG1_DR(0); // Use 800 as it is 800 / 2;
|
||
_accel_samplerate = 400;
|
||
|
||
} else {
|
||
return -EINVAL;
|
||
}
|
||
|
||
modify_reg(FXOS8701CQ_CTRL_REG1, CTRL_REG1_ACTIVE, 0);
|
||
modify_reg(FXOS8701CQ_CTRL_REG1, CTRL_REG1_DR_MASK, setbits);
|
||
modify_reg(FXOS8701CQ_CTRL_REG1, 0, active);
|
||
|
||
return OK;
|
||
}
|
||
|
||
void
|
||
FXOS8701CQ::start()
|
||
{
|
||
// start polling at the specified rate
|
||
ScheduleOnInterval(1000000 / (FXOS8701C_ACCEL_DEFAULT_RATE) - FXOS8701C_TIMER_REDUCTION, 10000);
|
||
}
|
||
|
||
void
|
||
FXOS8701CQ::check_registers(void)
|
||
{
|
||
uint8_t v;
|
||
|
||
if ((v = read_reg(_checked_registers[_checked_next])) != _checked_values[_checked_next]) {
|
||
/*
|
||
if we get the wrong value then we know the SPI bus
|
||
or sensor is very sick. We set _register_wait to 20
|
||
and wait until we have seen 20 good values in a row
|
||
before we consider the sensor to be OK again.
|
||
*/
|
||
perf_count(_bad_registers);
|
||
|
||
/*
|
||
try to fix the bad register value. We only try to
|
||
fix one per loop to prevent a bad sensor hogging the
|
||
bus. We skip zero as that is the WHO_AM_I, which
|
||
is not writeable
|
||
*/
|
||
if (_checked_next != 0) {
|
||
write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]);
|
||
}
|
||
|
||
_register_wait = 20;
|
||
}
|
||
|
||
_checked_next = (_checked_next + 1) % FXOS8701C_NUM_CHECKED_REGISTERS;
|
||
}
|
||
|
||
void
|
||
FXOS8701CQ::RunImpl()
|
||
{
|
||
// start the performance counter
|
||
perf_begin(_accel_sample_perf);
|
||
|
||
// status register and data as read back from the device
|
||
#pragma pack(push, 1)
|
||
struct {
|
||
uint8_t cmd[2];
|
||
uint8_t status;
|
||
int16_t x;
|
||
int16_t y;
|
||
int16_t z;
|
||
int16_t mx;
|
||
int16_t my;
|
||
int16_t mz;
|
||
} raw_accel_mag_report{};
|
||
#pragma pack(pop)
|
||
|
||
check_registers();
|
||
|
||
if (_register_wait != 0) {
|
||
// we are waiting for some good transfers before using
|
||
// the sensor again.
|
||
_register_wait--;
|
||
perf_end(_accel_sample_perf);
|
||
return;
|
||
}
|
||
|
||
/* fetch data from the sensor */
|
||
raw_accel_mag_report.cmd[0] = DIR_READ(FXOS8701CQ_DR_STATUS);
|
||
raw_accel_mag_report.cmd[1] = ADDR_7(FXOS8701CQ_DR_STATUS);
|
||
const hrt_abstime timestamp_sample = hrt_absolute_time();
|
||
transfer((uint8_t *)&raw_accel_mag_report, (uint8_t *)&raw_accel_mag_report, sizeof(raw_accel_mag_report));
|
||
|
||
if (!(raw_accel_mag_report.status & DR_STATUS_ZYXDR)) {
|
||
perf_end(_accel_sample_perf);
|
||
perf_count(_accel_duplicates);
|
||
return;
|
||
}
|
||
|
||
/*
|
||
* Eight-bit 2’s complement sensor temperature value with 0.96 °C/LSB sensitivity.
|
||
* Temperature data is only valid between –40 °C and 125 °C. The temperature sensor
|
||
* output is only valid when M_CTRL_REG1[m_hms] > 0b00. Please note that the
|
||
* temperature sensor is uncalibrated and its output for a given temperature will vary from
|
||
* one device to the next
|
||
*/
|
||
float temperature = (read_reg(FXOS8701CQ_TEMP)) * 0.96f;
|
||
|
||
_px4_accel.set_temperature(temperature);
|
||
|
||
// report the error count as the sum of the number of bad
|
||
// register reads and bad values. This allows the higher level
|
||
// code to decide if it should use this sensor based on
|
||
// whether it has had failures
|
||
_px4_accel.set_error_count(perf_event_count(_bad_registers));
|
||
|
||
int16_t x = swap16RightJustify14(raw_accel_mag_report.x);
|
||
int16_t y = swap16RightJustify14(raw_accel_mag_report.y);
|
||
int16_t z = swap16RightJustify14(raw_accel_mag_report.z);
|
||
_px4_accel.update(timestamp_sample, x, y, z);
|
||
|
||
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
|
||
|
||
if (hrt_elapsed_time(&_mag_last_measure) >= 10_ms) {
|
||
int16_t mag_x = swap16(raw_accel_mag_report.mx);
|
||
int16_t mag_y = swap16(raw_accel_mag_report.my);
|
||
int16_t mag_z = swap16(raw_accel_mag_report.mz);
|
||
_px4_mag.update(timestamp_sample, mag_x, mag_y, mag_z);
|
||
}
|
||
|
||
#endif
|
||
|
||
// stop the perf counter
|
||
perf_end(_accel_sample_perf);
|
||
}
|
||
|
||
void
|
||
FXOS8701CQ::print_status()
|
||
{
|
||
I2CSPIDriverBase::print_status();
|
||
perf_print_counter(_accel_sample_perf);
|
||
|
||
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
|
||
perf_print_counter(_mag_sample_perf);
|
||
#endif
|
||
perf_print_counter(_bad_registers);
|
||
perf_print_counter(_accel_duplicates);
|
||
|
||
::printf("checked_next: %u\n", _checked_next);
|
||
|
||
for (uint8_t i = 0; i < FXOS8701C_NUM_CHECKED_REGISTERS; i++) {
|
||
uint8_t v = read_reg(_checked_registers[i]);
|
||
|
||
if (v != _checked_values[i]) {
|
||
::printf("reg %02x:%02x should be %02x\n",
|
||
(unsigned)_checked_registers[i],
|
||
(unsigned)v,
|
||
(unsigned)_checked_values[i]);
|
||
}
|
||
}
|
||
}
|
||
|
||
void
|
||
FXOS8701CQ::print_registers()
|
||
{
|
||
const struct {
|
||
uint8_t reg;
|
||
const char *name;
|
||
} regmap[] = {
|
||
DEF_REG(FXOS8701CQ_DR_STATUS),
|
||
DEF_REG(FXOS8701CQ_OUT_X_MSB),
|
||
DEF_REG(FXOS8701CQ_XYZ_DATA_CFG),
|
||
DEF_REG(FXOS8701CQ_WHOAMI),
|
||
DEF_REG(FXOS8701CQ_CTRL_REG1),
|
||
DEF_REG(FXOS8701CQ_CTRL_REG2),
|
||
DEF_REG(FXOS8701CQ_M_DR_STATUS),
|
||
DEF_REG(FXOS8701CQ_M_OUT_X_MSB),
|
||
DEF_REG(FXOS8701CQ_M_CTRL_REG1),
|
||
DEF_REG(FXOS8701CQ_M_CTRL_REG2),
|
||
DEF_REG(FXOS8701CQ_M_CTRL_REG3),
|
||
};
|
||
|
||
for (uint8_t i = 0; i < sizeof(regmap) / sizeof(regmap[0]); i++) {
|
||
printf("0x%02x %s\n", read_reg(regmap[i].reg), regmap[i].name);
|
||
}
|
||
}
|
||
|
||
void
|
||
FXOS8701CQ::test_error()
|
||
{
|
||
// trigger an error
|
||
write_reg(FXOS8701CQ_CTRL_REG1, 0);
|
||
}
|