/****************************************************************
*
* ** This example is based on InvenSense's _confidential_ Application Note "Programming Sequence for DMP Hardware Functions".
* ** We are grateful to InvenSense for sharing this with us.
*
* ** Important note: by default the DMP functionality is disabled in the library
* ** as the DMP firmware takes up 14301 Bytes of program memory.
* ** To use the DMP, you will need to:
* ** Edit ICM_20948_C.h
* ** Uncomment line 29: #define ICM_20948_USE_DMP
* ** Save changes
* ** If you are using Windows, you can find ICM_20948_C.h in:
* ** Documents\Arduino\libraries\SparkFun_ICM-20948_ArduinoLibrary\src\util
*
* Please see License.md for the license information.
*
* Distributed as-is; no warranty is given.
***************************************************************/
/***************************************************************
* V1.1 :
*/
#include <Arduino.h>
#include <ICM_20948.h> // Click here to get the library: http://librarymanager/All#SparkFun_ICM_20948_IMU
#include <PWMServo.h>
//#include <filters.h>
#define DEBUG
#define Serial Serial
#define AD0_VAL 1 // The value of the last bit of the I2C address. \
// On the SparkFun 9DoF IMU breakout the default is 1, and when \
// the ADR jumper is closed the value becomes 0
// PINOUT
#define ICM_SDA 18
#define ICM_SCL 19
#define OLED_SDA 17
#define OLED_SCL 16
#define SERVO_PWM 2
#define CYCLE_TIME 3
//#define CORRECTION_ACTIVE 4
#define ANGLE_90 1500
#define ANGLE_MIN 500
#define ANGLE_MAX 2500
#define STRAIGHT_DRIVE_TIME 4000
#define STRAIGHT_ANGLE_DETECTION 2 // Derivative value for stable angle detection
// OLED
//#define USE_DISPLAY
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
//Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire1);
ICM_20948_I2C myICM; // Otherwise create an ICM_20948_I2C object
PWMServo myServo;
int8_t blinkState;
// Logging timer interrupt
float accXOffset = -51.662448883;
float accYOffset = -7.584472656;
float accZOffset = 1007.918518066;
float gyrXOffset = -001.755664229;
float gyrYOffset = -000.003526719;
float gyrZOffset = 000.282961845;
float roll,lastRoll, derivRoll, leanAngle, fGyrY;
int sampleTime, straightDriveTime, servoValue;
volatile int8_t gotSampleInterrupt;
float correctionFactor = 1.0;
float baseCorrectionFactor = 1.000;
float correctionFactorIncrement = 0.0001;
int8_t straight_phase, record, dump;
int recordIdx;
bool wifiOK,SDOK, cycle;
// Filters
/*
const float cutoff_freq = 5.0; //Cutoff frequency in Hz
const float sampling_time = 0.001; //Sampling time in seconds.
IIR::ORDER order = IIR::ORDER::OD3; // Order (OD1 to OD4)
*/
float getInitialAccelerometerAngle();
int SetServoValue(float roll);
void Calibrate();
void printFormattedFloat(float val, uint8_t leading, uint8_t decimals);
bool Init_ICM20948() {
Wire.setClock(1000000);
Wire.begin();
//myICM.enableDebugging(); // Uncomment this line to enable helpful debug messages on Serial
bool initialized = false;
while (!initialized)
{
// Initialize the ICM-20948
// If the DMP is enabled, .begin performs a minimal startup. We need to configure the sample mode etc. manually.
myICM.begin(Wire, AD0_VAL);
//Serial.print(F("Initialization of the sensor returned: "));
//Serial.println(myICM.statusString());
if (myICM.status != ICM_20948_Stat_Ok)
{
Serial.println(F("Trying again..."));
delay(500);
}
else
{
initialized = true;
}
}
// Here we are doing a SW reset to make sure the device starts in a known state
myICM.swReset();
if (myICM.status != ICM_20948_Stat_Ok)
{
Serial.print(F("Software Reset returned: "));
Serial.println(myICM.statusString());
}
delay(250);
// Now wake the sensor up
myICM.sleep(false);
myICM.lowPower(false);
// The next few configuration functions accept a bit-mask of sensors for which the settings should be applied.
// Set Gyro and Accelerometer to a particular sample mode
// options: ICM_20948_Sample_Mode_Continuous
// ICM_20948_Sample_Mode_Cycled
myICM.setSampleMode((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Continuous);
if (myICM.status != ICM_20948_Stat_Ok)
{
Serial.print(F("setSampleMode returned: "));
Serial.println(myICM.statusString());
}
// Set full scale ranges for both acc and gyr
ICM_20948_fss_t myFSS; // This uses a "Full Scale Settings" structure that can contain values for all configurable sensors
myFSS.a = gpm2; // (ICM_20948_ACCEL_CONFIG_FS_SEL_e)
// gpm2
// gpm4
// gpm8
// gpm16
myFSS.g = dps250; // (ICM_20948_GYRO_CONFIG_1_FS_SEL_e)
// dps250
// dps500
// dps1000
// dps2000
myICM.setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myFSS);
if (myICM.status != ICM_20948_Stat_Ok)
{
Serial.print(F("setFullScale returned: "));
Serial.println(myICM.statusString());
}
// Set up Digital Low-Pass Filter configuration
ICM_20948_dlpcfg_t myDLPcfg; // Similar to FSS, this uses a configuration structure for the desired sensors
myDLPcfg.a = acc_d5bw7_n8bw3; // (ICM_20948_ACCEL_CONFIG_DLPCFG_e)
// acc_d246bw_n265bw - means 3db bandwidth is 246 hz and nyquist bandwidth is 265 hz
// acc_d111bw4_n136bw
// acc_d50bw4_n68bw8
// acc_d23bw9_n34bw4
// acc_d11bw5_n17bw
// acc_d5bw7_n8bw3 - means 3 db bandwidth is 5.7 hz and nyquist bandwidth is 8.3 hz
// acc_d473bw_n499bw
myDLPcfg.g = gyr_d5bw7_n8bw9; // (ICM_20948_GYRO_CONFIG_1_DLPCFG_e)
// gyr_d196bw6_n229bw8
// gyr_d151bw8_n187bw6
// gyr_d119bw5_n154bw3
// gyr_d51bw2_n73bw3
// gyr_d23bw9_n35bw9
// gyr_d11bw6_n17bw8
// gyr_d5bw7_n8bw9
// gyr_d361bw4_n376bw5
myICM.setDLPFcfg((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myDLPcfg);
if (myICM.status != ICM_20948_Stat_Ok)
{
Serial.print(F("setDLPcfg returned: "));
Serial.println(myICM.statusString());
}
// Choose whether or not to use DLPF
// Here we're also showing another way to access the status values, and that it is OK to supply individual sensor masks to these functions
ICM_20948_Status_e accDLPEnableStat = myICM.enableDLPF(ICM_20948_Internal_Acc, true);
ICM_20948_Status_e gyrDLPEnableStat = myICM.enableDLPF(ICM_20948_Internal_Gyr, true);
Serial.print(F("Enable DLPF for Accelerometer returned: "));
Serial.println(myICM.statusString(accDLPEnableStat));
Serial.print(F("Enable DLPF for Gyroscope returned: "));
Serial.println(myICM.statusString(gyrDLPEnableStat));
return true;
}
void taskICM(void *param) {
int servoValue = 0;
//for (;;) {
cycle = !cycle;
digitalWriteFast(CYCLE_TIME, cycle);
if (myICM.dataReady())
{
myICM.getAGMT(); // The values are only updated when you call 'getAGMT'
int deltaSample = micros() - sampleTime;
sampleTime = micros();
lastRoll = roll;
roll += -((myICM.gyrY() - gyrYOffset) * ((float)deltaSample / 1000000.0));
derivRoll = abs((roll - lastRoll) / (float)deltaSample * 100000.0);
// Vertical angle correction
// If not angle variation, maybe straight line, increase time
// Else angle changes so in turns, reset straight line detection.
if (abs(derivRoll) < STRAIGHT_ANGLE_DETECTION) {
straightDriveTime++;
} else {
//digitalWrite(CORRECTION_ACTIVE, LOW);
straightDriveTime = 0;
correctionFactor = baseCorrectionFactor;
}
// If staight drive detected, active 0 correction
if(straightDriveTime > STRAIGHT_DRIVE_TIME) {
//digitalWrite(CORRECTION_ACTIVE, HIGH);
straight_phase = 10;
roll /= correctionFactor;
correctionFactor = (correctionFactor < 2.0 ? correctionFactor + correctionFactorIncrement : correctionFactor);
} else {
straight_phase = 0;
}
//servoValue = SetServoValue(roll);
//Serial.println(roll);
}
//} // For ever
}
void setup()
{
pinMode(CYCLE_TIME,OUTPUT); //cycle time measure
pinMode(SERVO_PWM,OUTPUT); //Servo PWM
// pinMode(CORRECTION_ACTIVE,OUTPUT); //Correction MODE ACTIVE
Serial.begin(115200); // Start the serial console
while (Serial.available()) // Make sure the serial RX buffer is empty
Serial.read();
delay(100);
#ifdef USE_DISPLAY
Init_OLED();
#endif
Init_ICM20948();
// Initialize Servo
myServo.attach(SERVO_PWM);
//myServo.writeMicroseconds(ANGLE_90);
myServo.write(90);
Serial.println();
Serial.println(F("Configuration complete!"));
sampleTime = micros();
// Initial angle measurement
roll = getInitialAccelerometerAngle();
SetServoValue(roll);
}
void loop()
{
/*
if (Serial.available() > 0) {
int income;
income = Serial.read();
switch (income) {
case 'c':
case 'C':
Calibrate();
break;
}
}
*/
taskICM(NULL);
}
int SetServoValue(float roll) {
int servoValue;
//servoValue = ANGLE_90 + int ((roll * 1000.0) / 90.0);
//servoValue = servoValue < ANGLE_MIN ? ANGLE_MIN : servoValue;
//servoValue = servoValue > ANGLE_MAX ? ANGLE_MAX : servoValue;
servoValue = (int) 90 + (roll+0.5);
//myServo.writeMicroseconds(servoValue);
myServo.write(servoValue);
return servoValue;
}
void Calibrate() {
float meanAccX = 0, meanAccY = 0,meanAccZ = 0,meanGyrX = 0,meanGyrY = 0,meanGyrZ = 0;
int nbSamples = 500;
Serial.println("Computing offsets...");
delay(2000);
for (int i = 0; i < nbSamples; i++) {
if (myICM.dataReady())
{
myICM.getAGMT(); // The values are only updated when you call 'getAGMT'
meanAccX += myICM.accX();
meanAccY += myICM.accY();
meanAccZ += myICM.accZ();
meanGyrX += myICM.gyrX();
meanGyrY += myICM.gyrY();
meanGyrZ += myICM.gyrZ();
delay(5);
}
}
meanAccX /= nbSamples;
meanAccY /= nbSamples;
meanAccZ /= nbSamples;
meanGyrX /= nbSamples;
meanGyrY /= nbSamples;
meanGyrZ /= nbSamples;
accXOffset = meanAccX;
accYOffset = meanAccY;
accZOffset = meanAccZ;
gyrXOffset = meanGyrX;
gyrYOffset = meanGyrY;
gyrZOffset = meanGyrZ;
Serial.print(" AccX offset : ");
printFormattedFloat(accXOffset, 3, 9);
Serial.print(" AccY offset : ");
printFormattedFloat(accYOffset, 3, 9);
Serial.print(" AccZ offset : ");
printFormattedFloat(accZOffset, 3, 9);
Serial.print(" GyrX offset : ");
printFormattedFloat(gyrXOffset, 3, 9);
Serial.print(" GyrY offset : ");
printFormattedFloat(gyrYOffset, 3, 9);
Serial.print(" GyrZ offset : ");
printFormattedFloat(gyrZOffset, 3, 9);
Serial.println();
delay(5000);
roll = 0.0;
}
/**
* Compute Angle from Acceleration vectors
* That only works if the motorbike is in straight line !!!
*/
float getInitialAccelerometerAngle()
{
float acc_total_vector = 0.0;
float angleY = 0.0;
while (!acc_total_vector) {
while (!myICM.dataReady()) {
delay(5);
}
myICM.getAGMT();
// Calculate total 3D acceleration vector : √(X² + Y² + Z²)
acc_total_vector = sqrt(pow(myICM.accX(), 2) + pow(myICM.accY(), 2) + pow(myICM.accZ(), 2));
}
// To prevent asin to produce a NaN, make sure the input value is within [-1;+1]
if (abs(myICM.accY()) < acc_total_vector) {
angleY = asin((float)myICM.accX() / acc_total_vector) * (180 / PI);
}
return -angleY;
}
void printFormattedFloat(float val, uint8_t leading, uint8_t decimals)
{
float aval = abs(val);
if (val < 0)
{
Serial.print("-");
}
else
{
Serial.print(" ");
}
for (uint8_t indi = 0; indi < leading; indi++)
{
uint32_t tenpow = 0;
if (indi < (leading - 1))
{
tenpow = 1;
}
for (uint8_t c = 0; c < (leading - 1 - indi); c++)
{
tenpow *= 10;
}
if (aval < tenpow)
{
Serial.print("0");
}
else
{
break;
}
}
if (val < 0)
{
Serial.print(-val, decimals);
}
else
{
Serial.print(val, decimals);
}
}
