From bde8ed37c3776e7250952eb1e25e4401c4c3c6f6 Mon Sep 17 00:00:00 2001
From: duccan <mail@duccan.nl>
Date: Sat, 6 Jun 2020 12:30:32 +0200
Subject: [PATCH] AP_HAL_ChibiOS: Added support for Bitcraze Crazyflie 2.1

Rework after review:
- Kept old IMU and barometer definition of crazyflie 2.0 in hwdef
- Added comment regarding soft-reset command
- Added defaults.parm for crazyflie
---
 .../hwdef/crazyflie2/defaults.parm            | 37 +++++++++++++++++++
 .../AP_HAL_ChibiOS/hwdef/crazyflie2/hwdef.dat | 10 ++++-
 .../AP_InertialSensor_BMI088.cpp              |  5 ++-
 3 files changed, 49 insertions(+), 3 deletions(-)
 create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/crazyflie2/defaults.parm

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/crazyflie2/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/crazyflie2/defaults.parm
new file mode 100644
index 0000000000..c7c2053177
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/crazyflie2/defaults.parm
@@ -0,0 +1,37 @@
+# Basic Crazyflie 2.x settings
+
+# Tuning settings
+ACRO_YAW_P,4
+ATC_ACCEL_P_MAX,180000
+ATC_ACCEL_R_MAX,180000
+ATC_ACCEL_Y_MAX,54000
+ATC_ANG_PIT_P,6
+ATC_ANG_RLL_P,6
+ATC_ANG_YAW_P,6
+ATC_RAT_PIT_D,0.004
+ATC_RAT_PIT_FLTD,81
+ATC_RAT_PIT_FLTT,81
+ATC_RAT_PIT_I,0.3
+ATC_RAT_PIT_P,0.3
+ATC_RAT_RLL_D,0.004
+ATC_RAT_RLL_FLTD,81
+ATC_RAT_RLL_FLTT,81
+ATC_RAT_RLL_I,0.3
+ATC_RAT_RLL_P,0.3
+ATC_RAT_YAW_FLTE,2
+ATC_RAT_YAW_FLTT,81
+ATC_RAT_YAW_I,0.04
+ATC_RAT_YAW_P,0.4
+
+INS_GYRO_FILTER,162
+
+# X-frame
+FRAME_CLASS,1
+
+# Brushed PWM Motors - 160 kHz
+MOT_PWM_TYPE,3
+RC_SPEED,16000
+
+# 1s lipo
+MOT_BAT_VOLT_MAX,4.2
+MOT_BAT_VOLT_MIN,3.3
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/crazyflie2/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/crazyflie2/hwdef.dat
index 86718488a6..c99abfe675 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/crazyflie2/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/crazyflie2/hwdef.dat
@@ -98,12 +98,18 @@ define STORAGE_FLASH_PAGE 1
 # reserve 32k for bootloader and 32k for flash storage
 FLASH_RESERVE_START_KB 64
 
-# one IMU
+# One IMU for Crazyflie 2.1 - I2C
 IMU BMI088 I2C:0:0x18 I2C:0:0x69 ROTATION_ROLL_180
 
-# one baro, attached via I2C on IMU
+# One IMU for Crazyflie 2.0 - I2C
+IMU Invensense I2C:0:0x69 ROTATION_YAW_180
+
+# One baro for Crazyflie 2.1
 BARO BMP388 I2C:0:0x77
 
+# One bare for Crazyflie 2.0
+BARO LPS2XH:probe_InvensenseIMU I2C:0:0x5D 0x69
+
 # no built-in compass, but probe the i2c bus for all possible
 # external compass types
 define ALLOW_ARM_NO_COMPASS
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp
index 387140030a..5af1e41263 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp
@@ -223,7 +223,10 @@ bool AP_InertialSensor_BMI088::gyro_init()
         return false;
     }
 
-    // Soft-reset gyro
+    /* Soft-reset gyro
+        Return value of 'write_register()' is not checked.
+        This commands has the tendency to fail upon soft-reset.
+    */
     dev_gyro->write_register(REGG_BGW_SOFTRESET, 0xB6);
     hal.scheduler->delay(30);