From 4d95a9d1c36075f535b9b1cb9599cd971b222230 Mon Sep 17 00:00:00 2001
From: Scott Parlane <scott@parlanenz.com>
Date: Wed, 11 Aug 2021 22:18:46 +1200
Subject: [PATCH] AP_InertialSensor: BMI160: Allow configuring the rotation

---
 .../AP_InertialSensor_BMI160.cpp              | 20 ++++++++++---------
 .../AP_InertialSensor_BMI160.h                | 16 ++++++++++++---
 2 files changed, 24 insertions(+), 12 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp
index 76a8b9f348..a4fb2e7e80 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp
@@ -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;
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.h b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.h
index 2264a9d2b8..77aac9eb08 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.h
@@ -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;