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_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) : AP_InertialSensor_Backend(imu)
, _dev(std::move(dev)) , _dev(std::move(dev))
, _rotation(rotation)
{ {
} }
AP_InertialSensor_Backend * AP_InertialSensor_Backend *
AP_InertialSensor_BMI160::probe(AP_InertialSensor &imu, 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) { if (!dev) {
return nullptr; 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) { if (!sensor) {
return nullptr; return nullptr;
@ -148,12 +151,13 @@ AP_InertialSensor_BMI160::probe(AP_InertialSensor &imu,
AP_InertialSensor_Backend * AP_InertialSensor_Backend *
AP_InertialSensor_BMI160::probe(AP_InertialSensor &imu, 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) { if (!dev) {
return nullptr; 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) { if (!sensor) {
return nullptr; 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.y),
(float)(int16_t)le16toh(raw_data[i].gyro.z)}; (float)(int16_t)le16toh(raw_data[i].gyro.z)};
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_AERO accel.rotate(_rotation);
accel.rotate(ROTATION_ROLL_180); gyro.rotate(_rotation);
gyro.rotate(ROTATION_ROLL_180);
#endif
accel *= _accel_scale; accel *= _accel_scale;
gyro *= _gyro_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.h"
#include "AP_InertialSensor_Backend.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 { class AP_InertialSensor_BMI160 : public AP_InertialSensor_Backend {
public: public:
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu, 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, 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. * Configure the sensors and start reading routine.
@ -40,7 +48,8 @@ public:
private: private:
AP_InertialSensor_BMI160(AP_InertialSensor &imu, 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 * If the macro BMI160_DEBUG is defined, check if there are errors reported
@ -109,6 +118,7 @@ private:
void _read_fifo(); void _read_fifo();
AP_HAL::OwnPtr<AP_HAL::Device> _dev; AP_HAL::OwnPtr<AP_HAL::Device> _dev;
enum Rotation _rotation;
uint8_t _accel_instance; uint8_t _accel_instance;
float _accel_scale; float _accel_scale;