Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp

408 lines
13 KiB
C++
Raw Blame History

/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
IMU driver for Robsense PhenixPro Devkit board including i3g4250d and iis328dq
*/
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RST_ZYNQ
#include "AP_InertialSensor_RST.h"
#include <AP_Math/AP_Math.h>
#include <inttypes.h>
#include <utility>
#include <math.h>
#include <stdio.h>
const extern AP_HAL::HAL &hal;
#define ADDR_INCREMENT (1<<6)
/************************************iis328dq register addresses *******************************************/
#define ACCEL_WHO_AM_I 0x0F
#define ACCEL_WHO_I_AM 0x32
#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
/* 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_DEFAULT_RANGE_G 8
#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_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
/* 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_FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7)
//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
// constructor
AP_InertialSensor_RST::AP_InertialSensor_RST(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_gyro,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_accel,
enum Rotation rotation_g,
enum Rotation rotation_a)
: AP_InertialSensor_Backend(imu)
, _dev_gyro(std::move(dev_gyro))
, _dev_accel(std::move(dev_accel))
, _rotation_g(rotation_g)
, _rotation_a(rotation_a)
{
}
AP_InertialSensor_RST::~AP_InertialSensor_RST()
{
}
/*
detect the sensor
*/
AP_InertialSensor_Backend *AP_InertialSensor_RST::probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_gyro,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_accel,
enum Rotation rotation_g,
enum Rotation rotation_a)
{
if (!dev_gyro && !dev_accel) {
return nullptr;
}
AP_InertialSensor_RST *sensor
= new AP_InertialSensor_RST(imu, std::move(dev_gyro), std::move(dev_accel), rotation_g, rotation_a);
if (!sensor || !sensor->_init_sensor()) {
delete sensor;
return nullptr;
}
return sensor;
}
/*
* init gyro
*/
bool AP_InertialSensor_RST::_init_gyro(void)
{
uint8_t whoami;
if (!_dev_gyro->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return false;
}
// set flag for reading registers
_dev_gyro->set_read_flag(0x80);
_dev_gyro->set_speed(AP_HAL::Device::SPEED_HIGH);
_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;
}
printf("detect i3g4250d\n");
//enter power-down mode first
_dev_gyro->write_register(GYRO_CTRL_REG1, 0);
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);
/* 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);
/* 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);
_dev_gyro->get_semaphore()->give();
return true;
fail_whoami:
_dev_gyro->get_semaphore()->give();
return false;
}
/*
* init accel
*/
bool AP_InertialSensor_RST::_init_accel(void)
{
uint8_t whoami;
if (!_dev_accel->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return false;
}
_dev_accel->set_speed(AP_HAL::Device::SPEED_HIGH);
_dev_accel->set_read_flag(0x80);
_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);
/* 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;
_dev_accel->get_semaphore()->give();
return true;
fail_whoami:
_dev_accel->get_semaphore()->give();
return false;
}
bool AP_InertialSensor_RST::_init_sensor(void)
{
if (!_init_gyro() || !_init_accel()) {
return false;
}
return true;
}
/*
startup the sensor
*/
void AP_InertialSensor_RST::start(void)
{
_gyro_instance = _imu.register_gyro(800, _dev_gyro->get_bus_id_devtype(DEVTYPE_GYR_I3G4250D));
_accel_instance = _imu.register_accel(1000, _dev_accel->get_bus_id_devtype(DEVTYPE_ACC_IIS328DQ));
set_gyro_orientation(_gyro_instance, _rotation_g);
set_accel_orientation(_accel_instance, _rotation_a);
// start the timer process to read samples
_dev_gyro->register_periodic_callback(1150, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_RST::gyro_measure, void));
_dev_accel->register_periodic_callback(800, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_RST::accel_measure, void));
}
/*
copy filtered data to the frontend
*/
bool AP_InertialSensor_RST::update(void)
{
update_gyro(_gyro_instance);
update_accel(_accel_instance);
return true;
}
// Accumulate values from gyros
void AP_InertialSensor_RST::gyro_measure(void)
{
Vector3f gyro;
uint8_t status = 0;
int16_t raw_data[3];
_dev_gyro->read_registers(GYRO_STATUS_REG, &status, sizeof(status));
if ((status & GYRO_STATUS_ZYXDA) == 0) {
return;
}
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
void AP_InertialSensor_RST::accel_measure(void)
{
Vector3f accel;
uint8_t status = 0;
int16_t raw_data[3];
_dev_accel->read_registers(ACCEL_STATUS_REG, &status, sizeof(status));
if ((status & ACCEL_STATUS_ZYXDA) == 0) {
return;
}
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);
_notify_new_accel_raw_sample(_accel_instance, accel);
}
}
#endif
Reference in New Issue View Git Blame Copy Permalink