zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_InertialSensor: fix code style problem, delete useless codes

master
HeBin 7 years ago committed by Lucas De Marchi
parent
b3307edd8f
commit
c056076e85
2 changed files with 179 additions and 215 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 383
      libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp
  2. 7
      libraries/AP_InertialSensor/AP_InertialSensor_RST.h

383
libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp
Unescape View File

@ -13,22 +13,11 @@ @@ -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: @@ -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 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 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_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_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_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_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 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 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_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_REG4_BLE (1<<6)
#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_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_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_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_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, @@ -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) @@ -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;
}
@ -257,31 +246,36 @@ bool AP_InertialSensor_RST::_init_gyro(void) @@ -257,31 +246,36 @@ 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);
GYRO_REG1_POWER_NORMAL |
GYRO_REG1_Z_ENABLE | GYRO_REG1_X_ENABLE | GYRO_REG1_Y_ENABLE |
RATE_800HZ_LP_50HZ);
/* disable high-pass filters */
_dev_gyro->write_register(GYRO_CTRL_REG2, 0);
/* DRDY disable */
_dev_gyro->write_register(GYRO_CTRL_REG3, 0x0);
_dev_gyro->write_register(GYRO_CTRL_REG4, RANGE_2000DPS);
_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 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);
/* 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;
_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: @@ -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) @@ -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: @@ -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,34 +368,19 @@ void AP_InertialSensor_RST::gyro_measure(void) @@ -368,34 +368,19 @@ 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
@ -404,28 +389,14 @@ void AP_InertialSensor_RST::accel_measure(void) @@ -404,28 +389,14 @@ 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);

7
libraries/AP_InertialSensor/AP_InertialSensor_RST.h
Unescape View File

@ -43,13 +43,6 @@ private: @@ -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;

Write Preview
Loading…
Cancel
Save