AP_InertialSensor: fix code style problem, delete useless codes
This commit is contained in:
HeBin
Lucas De Marchi
parent
b3307edd8f
commit
c056076e85
@ -13,22 +13,11 @@
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
This is an INS driver for the combination L3G4200D gyro and ADXL345 accelerometer.
|
||||
This combination is available as a cheap 10DOF sensor on ebay
|
||||
|
||||
This sensor driver is an example only - it should not be used in any
|
||||
serious autopilot as the latencies on I2C prevent good timing at
|
||||
high sample rates. It is useful when doing an initial port of
|
||||
ardupilot to a board where only I2C is available, and a cheap sensor
|
||||
can be used.
|
||||
|
||||
Datasheets:
|
||||
ADXL345 Accelerometer http://www.analog.com/static/imported-files/data_sheets/ADXL345.pdf
|
||||
L3G4200D gyro http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/CD00265057.pdf
|
||||
*/
|
||||
IMU driver for Robsense PhenixPro Devkit board including i3g4250d and iis328dq
|
||||
*/
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
||||
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RST_ZYNQ
|
||||
#include "AP_InertialSensor_RST.h"
|
||||
#include <AP_Math/AP_Math.h>
|
||||
|
||||
@ -39,150 +28,151 @@ Datasheets:
|
||||
|
||||
const extern AP_HAL::HAL &hal;
|
||||
|
||||
#define ADDR_INCREMENT (1<<6)
|
||||
#define ADDR_INCREMENT (1<<6)
|
||||
|
||||
/************************************iis328dq register addresses *******************************************/
|
||||
#define ACCEL_WHO_AM_I 0x0F
|
||||
#define ACCEL_WHO_I_AM 0x32
|
||||
#define ACCEL_WHO_AM_I 0x0F
|
||||
#define ACCEL_WHO_I_AM 0x32
|
||||
|
||||
#define ACCEL_CTRL_REG1 0x20
|
||||
#define ACCEL_CTRL_REG1 0x20
|
||||
/* keep lowpass low to avoid noise issues */
|
||||
#define RATE_50HZ_LP_37HZ (0<<4) | (0<<3)
|
||||
#define RATE_100HZ_LP_74HZ (0<<4) | (1<<3)
|
||||
#define RATE_400HZ_LP_292HZ (1<<4) | (0<<3)
|
||||
#define RATE_1000HZ_LP_780HZ (1<<4) | (1<<3)
|
||||
#define RATE_50HZ_LP_37HZ (0<<4) | (0<<3)
|
||||
#define RATE_100HZ_LP_74HZ (0<<4) | (1<<3)
|
||||
#define RATE_400HZ_LP_292HZ (1<<4) | (0<<3)
|
||||
#define RATE_1000HZ_LP_780HZ (1<<4) | (1<<3)
|
||||
|
||||
#define ACCEL_CTRL_REG2 0x21
|
||||
#define ACCEL_CTRL_REG3 0x22
|
||||
#define ACCEL_CTRL_REG4 0x23
|
||||
#define ACCEL_CTRL_REG2 0x21
|
||||
#define ACCEL_CTRL_REG3 0x22
|
||||
#define ACCEL_CTRL_REG4 0x23
|
||||
|
||||
#define ACCEL_CTRL_REG5 0x24
|
||||
#define ACCEL_HP_FILTER_RESETE 0x25
|
||||
#define ACCEL_OUT_REFERENCE 0x26
|
||||
#define ACCEL_STATUS_REG 0x27
|
||||
#define ACCEL_OUT_X_L 0x28
|
||||
#define ACCEL_OUT_X_H 0x29
|
||||
#define ACCEL_OUT_Y_L 0x2A
|
||||
#define ACCEL_OUT_Y_H 0x2B
|
||||
#define ACCEL_OUT_Z_L 0x2C
|
||||
#define ACCEL_OUT_Z_H 0x2D
|
||||
#define ACCEL_INT1_CFG 0x30
|
||||
#define ACCEL_INT1_SRC 0x31
|
||||
#define ACCEL_INT1_TSH 0x32
|
||||
#define ACCEL_INT1_DURATION 0x33
|
||||
#define ACCEL_INT2_CFG 0x34
|
||||
#define ACCEL_INT2_SRC 0x35
|
||||
#define ACCEL_INT2_TSH 0x36
|
||||
#define ACCEL_INT2_DURATION 0x37
|
||||
#define ACCEL_CTRL_REG5 0x24
|
||||
#define ACCEL_HP_FILTER_RESETE 0x25
|
||||
#define ACCEL_OUT_REFERENCE 0x26
|
||||
#define ACCEL_STATUS_REG 0x27
|
||||
#define ACCEL_OUT_X_L 0x28
|
||||
#define ACCEL_OUT_X_H 0x29
|
||||
#define ACCEL_OUT_Y_L 0x2A
|
||||
#define ACCEL_OUT_Y_H 0x2B
|
||||
#define ACCEL_OUT_Z_L 0x2C
|
||||
#define ACCEL_OUT_Z_H 0x2D
|
||||
#define ACCEL_INT1_CFG 0x30
|
||||
#define ACCEL_INT1_SRC 0x31
|
||||
#define ACCEL_INT1_TSH 0x32
|
||||
#define ACCEL_INT1_DURATION 0x33
|
||||
#define ACCEL_INT2_CFG 0x34
|
||||
#define ACCEL_INT2_SRC 0x35
|
||||
#define ACCEL_INT2_TSH 0x36
|
||||
#define ACCEL_INT2_DURATION 0x37
|
||||
|
||||
|
||||
/* Internal configuration values */
|
||||
#define ACCEL_REG1_POWER_NORMAL ((0<<7) | (0<<6) | (1<<5))
|
||||
#define ACCEL_REG1_Z_ENABLE (1<<2)
|
||||
#define ACCEL_REG1_Y_ENABLE (1<<1)
|
||||
#define ACCEL_REG1_X_ENABLE (1<<0)
|
||||
#define ACCEL_REG1_POWER_NORMAL ((0<<7) | (0<<6) | (1<<5))
|
||||
#define ACCEL_REG1_Z_ENABLE (1<<2)
|
||||
#define ACCEL_REG1_Y_ENABLE (1<<1)
|
||||
#define ACCEL_REG1_X_ENABLE (1<<0)
|
||||
|
||||
#define ACCEL_REG4_BDU (1<<7)
|
||||
#define ACCEL_REG4_BLE (1<<6)
|
||||
#define ACCEL_REG4_FULL_SCALE_BITS ((1<<5) | (1<<4))
|
||||
#define ACCEL_REG4_FULL_SCALE_2G ((0<<5) | (0<<4))
|
||||
#define ACCEL_REG4_FULL_SCALE_4G ((0<<5) | (1<<4))
|
||||
#define ACCEL_REG4_FULL_SCALE_8G ((1<<5) | (1<<4))
|
||||
#define ACCEL_REG4_BDU (1<<7)
|
||||
#define ACCEL_REG4_BLE (1<<6)
|
||||
#define ACCEL_REG4_FULL_SCALE_BITS ((1<<5) | (1<<4))
|
||||
#define ACCEL_REG4_FULL_SCALE_2G ((0<<5) | (0<<4))
|
||||
#define ACCEL_REG4_FULL_SCALE_4G ((0<<5) | (1<<4))
|
||||
#define ACCEL_REG4_FULL_SCALE_8G ((1<<5) | (1<<4))
|
||||
|
||||
#define ACCEL_STATUS_ZYXOR (1<<7)
|
||||
#define ACCEL_STATUS_ZOR (1<<6)
|
||||
#define ACCEL_STATUS_YOR (1<<5)
|
||||
#define ACCEL_STATUS_XOR (1<<4)
|
||||
#define ACCEL_STATUS_ZYXDA (1<<3)
|
||||
#define ACCEL_STATUS_ZDA (1<<2)
|
||||
#define ACCEL_STATUS_YDA (1<<1)
|
||||
#define ACCEL_STATUS_XDA (1<<0)
|
||||
#define ACCEL_STATUS_ZYXOR (1<<7)
|
||||
#define ACCEL_STATUS_ZOR (1<<6)
|
||||
#define ACCEL_STATUS_YOR (1<<5)
|
||||
#define ACCEL_STATUS_XOR (1<<4)
|
||||
#define ACCEL_STATUS_ZYXDA (1<<3)
|
||||
#define ACCEL_STATUS_ZDA (1<<2)
|
||||
#define ACCEL_STATUS_YDA (1<<1)
|
||||
#define ACCEL_STATUS_XDA (1<<0)
|
||||
|
||||
#define ACCEL_DEFAULT_RANGE_G 8
|
||||
#define ACCEL_DEFAULT_RATE 1000
|
||||
#define ACCEL_DEFAULT_RATE 1000
|
||||
#define ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 780
|
||||
#define ACCEL_ONE_G 9.80665f
|
||||
|
||||
/************************************i3g4250d register addresses *******************************************/
|
||||
#define GYRO_WHO_AM_I 0x0F
|
||||
#define GYRO_WHO_I_AM 0xD3
|
||||
#define GYRO_WHO_AM_I 0x0F
|
||||
#define GYRO_WHO_I_AM 0xD3
|
||||
|
||||
#define GYRO_CTRL_REG1 0x20
|
||||
#define GYRO_CTRL_REG1 0x20
|
||||
/* keep lowpass low to avoid noise issues */
|
||||
#define RATE_100HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4))
|
||||
#define RATE_200HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4))
|
||||
#define RATE_200HZ_LP_50HZ ((0<<7) | (1<<6) | (1<<5) | (0<<4))
|
||||
#define RATE_200HZ_LP_70HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4))
|
||||
#define RATE_400HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4))
|
||||
#define RATE_400HZ_LP_25HZ ((1<<7) | (0<<6) | (0<<5) | (1<<4))
|
||||
#define RATE_400HZ_LP_50HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4))
|
||||
#define RATE_400HZ_LP_100HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4))
|
||||
#define RATE_800HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4))
|
||||
#define RATE_800HZ_LP_35HZ ((1<<7) | (1<<6) | (0<<5) | (1<<4))
|
||||
#define RATE_800HZ_LP_50HZ ((1<<7) | (1<<6) | (1<<5) | (0<<4))
|
||||
#define RATE_800HZ_LP_100HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4))
|
||||
#define RATE_100HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4))
|
||||
#define RATE_200HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4))
|
||||
#define RATE_200HZ_LP_50HZ ((0<<7) | (1<<6) | (1<<5) | (0<<4))
|
||||
#define RATE_200HZ_LP_70HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4))
|
||||
#define RATE_400HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4))
|
||||
#define RATE_400HZ_LP_25HZ ((1<<7) | (0<<6) | (0<<5) | (1<<4))
|
||||
#define RATE_400HZ_LP_50HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4))
|
||||
#define RATE_400HZ_LP_100HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4))
|
||||
#define RATE_800HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4))
|
||||
#define RATE_800HZ_LP_35HZ ((1<<7) | (1<<6) | (0<<5) | (1<<4))
|
||||
#define RATE_800HZ_LP_50HZ ((1<<7) | (1<<6) | (1<<5) | (0<<4))
|
||||
#define RATE_800HZ_LP_100HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4))
|
||||
|
||||
#define GYRO_CTRL_REG2 0x21
|
||||
#define GYRO_CTRL_REG3 0x22
|
||||
#define GYRO_CTRL_REG4 0x23
|
||||
#define RANGE_250DPS (0<<4)
|
||||
#define RANGE_500DPS (1<<4)
|
||||
#define RANGE_2000DPS (3<<4)
|
||||
#define GYRO_CTRL_REG2 0x21
|
||||
#define GYRO_CTRL_REG3 0x22
|
||||
#define GYRO_CTRL_REG4 0x23
|
||||
#define RANGE_250DPS (0<<4)
|
||||
#define RANGE_500DPS (1<<4)
|
||||
#define RANGE_2000DPS (3<<4)
|
||||
|
||||
#define GYRO_CTRL_REG5 0x24
|
||||
#define GYRO_REFERENCE 0x25
|
||||
#define GYRO_OUT_TEMP 0x26
|
||||
#define GYRO_STATUS_REG 0x27
|
||||
#define GYRO_OUT_X_L 0x28
|
||||
#define GYRO_OUT_X_H 0x29
|
||||
#define GYRO_OUT_Y_L 0x2A
|
||||
#define GYRO_OUT_Y_H 0x2B
|
||||
#define GYRO_OUT_Z_L 0x2C
|
||||
#define GYRO_OUT_Z_H 0x2D
|
||||
#define GYRO_FIFO_CTRL_REG 0x2E
|
||||
#define GYRO_FIFO_SRC_REG 0x2F
|
||||
#define GYRO_INT1_CFG 0x30
|
||||
#define GYRO_INT1_SRC 0x31
|
||||
#define GYRO_INT1_TSH_XH 0x32
|
||||
#define GYRO_INT1_TSH_XL 0x33
|
||||
#define GYRO_INT1_TSH_YH 0x34
|
||||
#define GYRO_INT1_TSH_YL 0x35
|
||||
#define GYRO_INT1_TSH_ZH 0x36
|
||||
#define GYRO_INT1_TSH_ZL 0x37
|
||||
#define GYRO_INT1_DURATION 0x38
|
||||
#define GYRO_LOW_ODR 0x39
|
||||
#define GYRO_CTRL_REG5 0x24
|
||||
#define GYRO_REFERENCE 0x25
|
||||
#define GYRO_OUT_TEMP 0x26
|
||||
#define GYRO_STATUS_REG 0x27
|
||||
#define GYRO_OUT_X_L 0x28
|
||||
#define GYRO_OUT_X_H 0x29
|
||||
#define GYRO_OUT_Y_L 0x2A
|
||||
#define GYRO_OUT_Y_H 0x2B
|
||||
#define GYRO_OUT_Z_L 0x2C
|
||||
#define GYRO_OUT_Z_H 0x2D
|
||||
#define GYRO_FIFO_CTRL_REG 0x2E
|
||||
#define GYRO_FIFO_SRC_REG 0x2F
|
||||
#define GYRO_INT1_CFG 0x30
|
||||
#define GYRO_INT1_SRC 0x31
|
||||
#define GYRO_INT1_TSH_XH 0x32
|
||||
#define GYRO_INT1_TSH_XL 0x33
|
||||
#define GYRO_INT1_TSH_YH 0x34
|
||||
#define GYRO_INT1_TSH_YL 0x35
|
||||
#define GYRO_INT1_TSH_ZH 0x36
|
||||
#define GYRO_INT1_TSH_ZL 0x37
|
||||
#define GYRO_INT1_DURATION 0x38
|
||||
#define GYRO_LOW_ODR 0x39
|
||||
|
||||
|
||||
/* Internal configuration values */
|
||||
#define GYRO_REG1_POWER_NORMAL (1<<3)
|
||||
#define GYRO_REG1_Z_ENABLE (1<<2)
|
||||
#define GYRO_REG1_Y_ENABLE (1<<1)
|
||||
#define GYRO_REG1_X_ENABLE (1<<0)
|
||||
#define GYRO_REG1_POWER_NORMAL (1<<3)
|
||||
#define GYRO_REG1_Z_ENABLE (1<<2)
|
||||
#define GYRO_REG1_Y_ENABLE (1<<1)
|
||||
#define GYRO_REG1_X_ENABLE (1<<0)
|
||||
|
||||
#define GYRO_REG4_BLE (1<<6)
|
||||
#define GYRO_REG4_BLE (1<<6)
|
||||
|
||||
#define GYRO_REG5_FIFO_ENABLE (1<<6)
|
||||
#define GYRO_REG5_REBOOT_MEMORY (1<<7)
|
||||
#define GYRO_REG5_FIFO_ENABLE (1<<6)
|
||||
#define GYRO_REG5_REBOOT_MEMORY (1<<7)
|
||||
|
||||
#define GYRO_STATUS_ZYXOR (1<<7)
|
||||
#define GYRO_STATUS_ZOR (1<<6)
|
||||
#define GYRO_STATUS_YOR (1<<5)
|
||||
#define GYRO_STATUS_XOR (1<<4)
|
||||
#define GYRO_STATUS_ZYXDA (1<<3)
|
||||
#define GYRO_STATUS_ZDA (1<<2)
|
||||
#define GYRO_STATUS_YDA (1<<1)
|
||||
#define GYRO_STATUS_XDA (1<<0)
|
||||
#define GYRO_STATUS_ZYXOR (1<<7)
|
||||
#define GYRO_STATUS_ZOR (1<<6)
|
||||
#define GYRO_STATUS_YOR (1<<5)
|
||||
#define GYRO_STATUS_XOR (1<<4)
|
||||
#define GYRO_STATUS_ZYXDA (1<<3)
|
||||
#define GYRO_STATUS_ZDA (1<<2)
|
||||
#define GYRO_STATUS_YDA (1<<1)
|
||||
#define GYRO_STATUS_XDA (1<<0)
|
||||
|
||||
#define GYRO_FIFO_CTRL_BYPASS_MODE (0<<5)
|
||||
#define GYRO_FIFO_CTRL_FIFO_MODE (1<<5)
|
||||
#define GYRO_FIFO_CTRL_STREAM_MODE (1<<6)
|
||||
#define GYRO_FIFO_CTRL_STREAM_TO_FIFO_MODE (3<<5)
|
||||
#define GYRO_FIFO_CTRL_BYPASS_MODE (0<<5)
|
||||
#define GYRO_FIFO_CTRL_FIFO_MODE (1<<5)
|
||||
#define GYRO_FIFO_CTRL_STREAM_MODE (1<<6)
|
||||
#define GYRO_FIFO_CTRL_STREAM_TO_FIFO_MODE (3<<5)
|
||||
#define GYRO_FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7)
|
||||
|
||||
#define GYRO_DEFAULT_RATE 800 //data output frequency
|
||||
//data output frequency
|
||||
#define GYRO_DEFAULT_RATE 800
|
||||
#define GYRO_DEFAULT_RANGE_DPS 2000
|
||||
#define GYRO_DEFAULT_FILTER_FREQ 35
|
||||
#define GYRO_TEMP_OFFSET_CELSIUS 40
|
||||
#define GYRO_DEFAULT_FILTER_FREQ 35
|
||||
#define GYRO_TEMP_OFFSET_CELSIUS 40
|
||||
|
||||
|
||||
// constructor
|
||||
@ -230,10 +220,9 @@ AP_InertialSensor_Backend *AP_InertialSensor_RST::probe(AP_InertialSensor &imu,
|
||||
*/
|
||||
bool AP_InertialSensor_RST::_init_gyro(void)
|
||||
{
|
||||
uint8_t whoami;
|
||||
|
||||
_gyro_spi_sem = _dev_gyro->get_semaphore();
|
||||
|
||||
if (!_gyro_spi_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
if (!_dev_gyro->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -242,10 +231,10 @@ bool AP_InertialSensor_RST::_init_gyro(void)
|
||||
|
||||
_dev_gyro->set_speed(AP_HAL::Device::SPEED_HIGH);
|
||||
|
||||
_dev_gyro->read_registers(GYRO_WHO_AM_I, &_gyro_whoami, sizeof(_gyro_whoami));
|
||||
if (_gyro_whoami != GYRO_WHO_I_AM) {
|
||||
hal.console->printf("RST: unexpected gyro WHOAMI 0x%x\n", (unsigned)_gyro_whoami);
|
||||
printf("RST: unexpected gyro WHOAMI 0x%x\n", (unsigned)_gyro_whoami);
|
||||
_dev_gyro->read_registers(GYRO_WHO_AM_I, &whoami, sizeof(whoami));
|
||||
if (whoami != GYRO_WHO_I_AM) {
|
||||
hal.console->printf("RST: unexpected gyro WHOAMI 0x%x\n", (unsigned)whoami);
|
||||
printf("RST: unexpected gyro WHOAMI 0x%x\n", (unsigned)whoami);
|
||||
goto fail_whoami;
|
||||
}
|
||||
|
||||
@ -256,32 +245,37 @@ bool AP_InertialSensor_RST::_init_gyro(void)
|
||||
|
||||
hal.scheduler->delay(100);
|
||||
|
||||
_dev_gyro->write_register(GYRO_CTRL_REG1,
|
||||
GYRO_REG1_POWER_NORMAL | GYRO_REG1_Z_ENABLE | GYRO_REG1_X_ENABLE | GYRO_REG1_Y_ENABLE | RATE_800HZ_LP_50HZ);
|
||||
_dev_gyro->write_register(GYRO_CTRL_REG1,
|
||||
GYRO_REG1_POWER_NORMAL |
|
||||
GYRO_REG1_Z_ENABLE | GYRO_REG1_X_ENABLE | GYRO_REG1_Y_ENABLE |
|
||||
RATE_800HZ_LP_50HZ);
|
||||
|
||||
_dev_gyro->write_register(GYRO_CTRL_REG2, 0); /* disable high-pass filters */
|
||||
_dev_gyro->write_register(GYRO_CTRL_REG3, 0x0); /* DRDY disable */
|
||||
_dev_gyro->write_register(GYRO_CTRL_REG4, RANGE_2000DPS);
|
||||
_dev_gyro->write_register(GYRO_CTRL_REG5, GYRO_REG5_FIFO_ENABLE); /* disable wake-on-interrupt */
|
||||
/* disable high-pass filters */
|
||||
_dev_gyro->write_register(GYRO_CTRL_REG2, 0);
|
||||
|
||||
/* disable FIFO. This makes things simpler and ensures we
|
||||
* aren't getting stale data. It means we must run the hrt
|
||||
* callback fast enough to not miss data. */
|
||||
_dev_gyro->write_register(GYRO_FIFO_CTRL_REG, GYRO_FIFO_CTRL_BYPASS_MODE);
|
||||
/* DRDY disable */
|
||||
_dev_gyro->write_register(GYRO_CTRL_REG3, 0x0);
|
||||
_dev_gyro->write_register(GYRO_CTRL_REG4, RANGE_2000DPS);
|
||||
|
||||
_gyro_scale = 70e-3f / 180.0f * M_PI;
|
||||
/* disable wake-on-interrupt */
|
||||
_dev_gyro->write_register(GYRO_CTRL_REG5, GYRO_REG5_FIFO_ENABLE);
|
||||
|
||||
/* disable FIFO. This makes things simpler and ensures we
|
||||
* aren't getting stale data. It means we must run the hrt
|
||||
* callback fast enough to not miss data. */
|
||||
_dev_gyro->write_register(GYRO_FIFO_CTRL_REG, GYRO_FIFO_CTRL_BYPASS_MODE);
|
||||
|
||||
_gyro_scale = 70e-3f / 180.0f * M_PI;
|
||||
|
||||
hal.scheduler->delay(100);
|
||||
|
||||
_gyro_spi_sem->give();
|
||||
_dev_gyro->get_semaphore()->give();
|
||||
|
||||
return true;
|
||||
|
||||
fail_whoami:
|
||||
_gyro_spi_sem->give();
|
||||
_dev_gyro->get_semaphore()->give();
|
||||
return false;
|
||||
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
@ -289,9 +283,9 @@ fail_whoami:
|
||||
*/
|
||||
bool AP_InertialSensor_RST::_init_accel(void)
|
||||
{
|
||||
_accel_spi_sem = _dev_accel->get_semaphore();
|
||||
uint8_t whoami;
|
||||
|
||||
if (!_accel_spi_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
if (!_dev_accel->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -299,27 +293,34 @@ bool AP_InertialSensor_RST::_init_accel(void)
|
||||
|
||||
_dev_accel->set_read_flag(0x80);
|
||||
|
||||
_dev_accel->read_registers(ACCEL_WHO_AM_I, &_accel_whoami, sizeof(_accel_whoami));
|
||||
if (_accel_whoami != ACCEL_WHO_I_AM) {
|
||||
hal.console->printf("RST: unexpected accel WHOAMI 0x%x\n", (unsigned)_accel_whoami);
|
||||
printf("RST: unexpected accel WHOAMI 0x%x\n", (unsigned)_accel_whoami);
|
||||
_dev_accel->read_registers(ACCEL_WHO_AM_I, &whoami, sizeof(whoami));
|
||||
if (whoami != ACCEL_WHO_I_AM) {
|
||||
hal.console->printf("RST: unexpected accel WHOAMI 0x%x\n", (unsigned)whoami);
|
||||
printf("RST: unexpected accel WHOAMI 0x%x\n", (unsigned)whoami);
|
||||
goto fail_whoami;
|
||||
}
|
||||
|
||||
_dev_accel->write_register(ACCEL_CTRL_REG1,
|
||||
ACCEL_REG1_POWER_NORMAL | ACCEL_REG1_Z_ENABLE | ACCEL_REG1_Y_ENABLE | ACCEL_REG1_X_ENABLE | RATE_1000HZ_LP_780HZ);
|
||||
_dev_accel->write_register(ACCEL_CTRL_REG2, 0); /* disable high-pass filters */
|
||||
_dev_accel->write_register(ACCEL_CTRL_REG3, 0x02); /* DRDY enable */
|
||||
_dev_accel->write_register(ACCEL_CTRL_REG4, ACCEL_REG4_BDU | ACCEL_REG4_FULL_SCALE_8G);
|
||||
_dev_accel->write_register(ACCEL_CTRL_REG1,
|
||||
ACCEL_REG1_POWER_NORMAL |
|
||||
ACCEL_REG1_Z_ENABLE | ACCEL_REG1_Y_ENABLE | ACCEL_REG1_X_ENABLE |
|
||||
RATE_1000HZ_LP_780HZ);
|
||||
|
||||
/* disable high-pass filters */
|
||||
_dev_accel->write_register(ACCEL_CTRL_REG2, 0);
|
||||
|
||||
/* DRDY enable */
|
||||
_dev_accel->write_register(ACCEL_CTRL_REG3, 0x02);
|
||||
_dev_accel->write_register(ACCEL_CTRL_REG4,
|
||||
ACCEL_REG4_BDU | ACCEL_REG4_FULL_SCALE_8G);
|
||||
|
||||
_accel_scale = 0.244e-3f * ACCEL_ONE_G;
|
||||
|
||||
_accel_spi_sem->give();
|
||||
_dev_accel->get_semaphore()->give();
|
||||
|
||||
return true;
|
||||
|
||||
fail_whoami:
|
||||
_accel_spi_sem->give();
|
||||
_dev_accel->get_semaphore()->give();
|
||||
return false;
|
||||
|
||||
|
||||
@ -327,8 +328,7 @@ fail_whoami:
|
||||
|
||||
bool AP_InertialSensor_RST::_init_sensor(void)
|
||||
{
|
||||
if(!_init_gyro() || !_init_accel())
|
||||
{
|
||||
if (!_init_gyro() || !_init_accel()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -368,64 +368,35 @@ void AP_InertialSensor_RST::gyro_measure(void)
|
||||
Vector3f gyro;
|
||||
uint8_t status = 0;
|
||||
int16_t raw_data[3];
|
||||
static uint64_t start = 0;
|
||||
uint64_t now = 0;
|
||||
static int count = 0;
|
||||
|
||||
_dev_gyro->read_registers(GYRO_STATUS_REG, &status, sizeof(status));
|
||||
|
||||
if((status & GYRO_STATUS_ZYXDA) == 0)
|
||||
if ((status & GYRO_STATUS_ZYXDA) == 0) {
|
||||
return;
|
||||
|
||||
count++;
|
||||
|
||||
now = AP_HAL::micros64();
|
||||
|
||||
if(now - start >= 1000000)
|
||||
{
|
||||
//printf("gyro count = %d\n", count);
|
||||
count = 0;
|
||||
start = now;
|
||||
}
|
||||
|
||||
if (_dev_gyro->read_registers(GYRO_OUT_X_L | ADDR_INCREMENT, (uint8_t *)raw_data, sizeof(raw_data)))
|
||||
{
|
||||
if (_dev_gyro->read_registers(GYRO_OUT_X_L | ADDR_INCREMENT, (uint8_t *)raw_data, sizeof(raw_data))) {
|
||||
gyro = Vector3f(raw_data[0], raw_data[1], raw_data[2]);
|
||||
gyro *= _gyro_scale;
|
||||
_rotate_and_correct_gyro(_gyro_instance, gyro);
|
||||
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// Accumulate values from accels
|
||||
// Accumulate values from accels
|
||||
void AP_InertialSensor_RST::accel_measure(void)
|
||||
{
|
||||
Vector3f accel;
|
||||
uint8_t status = 0;
|
||||
int16_t raw_data[3];
|
||||
static uint64_t start = 0;
|
||||
uint64_t now = 0;
|
||||
static int count = 0;
|
||||
|
||||
_dev_accel->read_registers(ACCEL_STATUS_REG, &status, sizeof(status));
|
||||
|
||||
if((status & ACCEL_STATUS_ZYXDA) == 0)
|
||||
if ((status & ACCEL_STATUS_ZYXDA) == 0) {
|
||||
return;
|
||||
|
||||
count++;
|
||||
|
||||
now = AP_HAL::micros64();
|
||||
|
||||
if(now - start >= 1000000)
|
||||
{
|
||||
//printf("accel count = %d\n", count);
|
||||
count = 0;
|
||||
start = now;
|
||||
}
|
||||
|
||||
if (_dev_accel->read_registers(ACCEL_OUT_X_L | ADDR_INCREMENT, (uint8_t *)raw_data, sizeof(raw_data)))
|
||||
{
|
||||
if (_dev_accel->read_registers(ACCEL_OUT_X_L | ADDR_INCREMENT, (uint8_t *)raw_data, sizeof(raw_data))) {
|
||||
accel = Vector3f(raw_data[0], raw_data[1], raw_data[2]);
|
||||
accel *= _accel_scale;
|
||||
_rotate_and_correct_accel(_accel_instance, accel);
|
||||
|
||||
@ -43,13 +43,6 @@ private:
|
||||
|
||||
AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev_gyro;//i3g4250d
|
||||
AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev_accel;//iis328dq
|
||||
AP_HAL::Semaphore *_gyro_spi_sem;
|
||||
AP_HAL::Semaphore *_accel_spi_sem;
|
||||
|
||||
// gyro whoami
|
||||
uint8_t _gyro_whoami;
|
||||
// accel whoami
|
||||
uint8_t _accel_whoami;
|
||||
|
||||
float _gyro_scale;
|
||||
float _accel_scale;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user