aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: BMI160: Allow configuring the rotation

Browse Source
This commit is contained in:
Scott Parlane 2021-08-11 22:18:46 +12:00 committed by Andrew Tridgell
parent 700edd241f
commit 4d95a9d1c3
2 changed files with 24 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp
View File

@ -119,20 +119,23 @@ struct PACKED RawData {
};
AP_InertialSensor_BMI160::AP_InertialSensor_BMI160(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::Device> dev)
AP_HAL::OwnPtr<AP_HAL::Device> dev,
enum Rotation rotation)
: AP_InertialSensor_Backend(imu)
, _dev(std::move(dev))
, _rotation(rotation)
{
}
AP_InertialSensor_Backend *
AP_InertialSensor_BMI160::probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev)
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev,
enum Rotation rotation)
{
if (!dev) {
return nullptr;
}
auto sensor = new AP_InertialSensor_BMI160(imu, std::move(dev));
auto sensor = new AP_InertialSensor_BMI160(imu, std::move(dev), rotation);
if (!sensor) {
return nullptr;
@ -148,12 +151,13 @@ AP_InertialSensor_BMI160::probe(AP_InertialSensor &imu,
AP_InertialSensor_Backend *
AP_InertialSensor_BMI160::probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
enum Rotation rotation)
{
if (!dev) {
return nullptr;
}
auto sensor = new AP_InertialSensor_BMI160(imu, std::move(dev));
auto sensor = new AP_InertialSensor_BMI160(imu, std::move(dev), rotation);
if (!sensor) {
return nullptr;
@ -410,10 +414,8 @@ read_fifo_read_data:
(float)(int16_t)le16toh(raw_data[i].gyro.y),
(float)(int16_t)le16toh(raw_data[i].gyro.z)};
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_AERO
accel.rotate(ROTATION_ROLL_180);
gyro.rotate(ROTATION_ROLL_180);
#endif
accel.rotate(_rotation);
gyro.rotate(_rotation);
accel *= _accel_scale;
gyro *= _gyro_scale;

16
libraries/AP_InertialSensor/AP_InertialSensor_BMI160.h
View File

@ -23,13 +23,21 @@
#include "AP_InertialSensor.h"
#include "AP_InertialSensor_Backend.h"
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_AERO
#define BMI160_DEFAULT_ROTATION ROTATION_ROLL_180
#else
#define BMI160_DEFAULT_ROTATION ROTATION_NONE
#endif
class AP_InertialSensor_BMI160 : public AP_InertialSensor_Backend {
public:
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev);
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev,
enum Rotation rotation=BMI160_DEFAULT_ROTATION);
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
enum Rotation rotation=BMI160_DEFAULT_ROTATION);
/**
* Configure the sensors and start reading routine.
@ -40,7 +48,8 @@ public:
private:
AP_InertialSensor_BMI160(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::Device> dev);
AP_HAL::OwnPtr<AP_HAL::Device> dev,
enum Rotation rotation);
/**
* If the macro BMI160_DEBUG is defined, check if there are errors reported
@ -109,6 +118,7 @@ private:
void _read_fifo();
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
enum Rotation _rotation;
uint8_t _accel_instance;
float _accel_scale;