update repo

config/config.h

@@ -0,0 +1,34 @@
+// Copyright (c) 2021 Juan Miguel Jimeno
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifdef USE_BEEBO_CONFIG
+    #include "custom/beebo_config.h"
+#endif
+
+#ifdef USE_BEEBO_M_CONFIG
+    #include "custom/beebo_m_config.h"
+#endif
+
+#ifdef USE_SQUARE_CONFIG
+    #include "custom/square_config.h"
+#endif
+
+#ifdef USE_DEV_CONFIG
+    #include "custom/dev_config.h"
+#endif
+
+#if !defined (USE_BEEBO_CONFIG) && !defined (USE_DEV_CONFIG) && !defined (USE_SQUARE_CONFIG) && !defined (USE_BEEBO_M_CONFIG)
+    #include "lino_base_config.h"
+#endif
+

config/custom/beebo_config.h

@@ -0,0 +1,84 @@
+// Copyright (c) 2021 Juan Miguel Jimeno
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEEBO_CONFIG_H
+#define BEEBO_CONFIG_H
+
+#define LED_PIN 13 
+
+#define LINO_BASE SKID_STEER 
+
+#define USE_GENERIC_1_IN_MOTOR_DRIVER 
+#define USE_GY85_IMU
+
+#define K_P 0.6                             
+#define K_I 0.8                             
+#define K_D 0.5                             
+
+#define MOTOR_MAX_RPM 140
+#define MAX_RPM_RATIO 0.85
+#define MOTOR_OPERATING_VOLTAGE 24
+#define MOTOR_POWER_MAX_VOLTAGE 14.6
+#define MOTOR_POWER_MEASURED_VOLTAGE 14.6                         
+#define COUNTS_PER_REV1 144384
+#define COUNTS_PER_REV2 140777
+#define COUNTS_PER_REV3 148326
+#define COUNTS_PER_REV4 144495
+#define WHEEL_DIAMETER 0.152               
+#define LR_WHEELS_DISTANCE 0.271            
+#define PWM_BITS 10                        
+#define PWM_FREQUENCY 20000
+
+/// ENCODER PINS
+#define MOTOR1_ENCODER_A 14
+#define MOTOR1_ENCODER_B 15 
+#define MOTOR1_ENCODER_INV false 
+
+#define MOTOR2_ENCODER_A 11
+#define MOTOR2_ENCODER_B 12 
+#define MOTOR2_ENCODER_INV false 
+
+#define MOTOR3_ENCODER_A 17
+#define MOTOR3_ENCODER_B 16 
+#define MOTOR3_ENCODER_INV true 
+
+#define MOTOR4_ENCODER_A 9
+#define MOTOR4_ENCODER_B 10
+#define MOTOR4_ENCODER_INV false 
+
+// Motor Pins
+#define MOTOR1_PWM 21
+#define MOTOR1_IN_A 20
+#define MOTOR1_IN_B -1 
+#define MOTOR1_INV false
+
+#define MOTOR2_PWM 5
+#define MOTOR2_IN_A 6
+#define MOTOR2_IN_B -1 
+#define MOTOR2_INV true
+
+#define MOTOR3_PWM 22
+#define MOTOR3_IN_A 23
+#define MOTOR3_IN_B -1
+#define MOTOR3_INV false
+
+#define MOTOR4_PWM 4
+#define MOTOR4_IN_A 3
+#define MOTOR4_IN_B -1
+#define MOTOR4_INV false
+
+#define PWM_MAX pow(2, PWM_BITS) - 1
+#define PWM_MIN -PWM_MAX
+
+#endif

config/custom/beebo_m_config.h

@@ -0,0 +1,84 @@
+// Copyright (c) 2021 Juan Miguel Jimeno
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEEBO_M_CONFIG_H
+#define BEEBO_M_CONFIG_H
+
+#define LED_PIN 13 
+
+#define LINO_BASE MECANUM 
+
+#define USE_GENERIC_1_IN_MOTOR_DRIVER 
+#define USE_GY85_IMU
+
+#define K_P 0.6                             
+#define K_I 0.8                             
+#define K_D 0.5                             
+
+#define MOTOR_MAX_RPM 140
+#define MAX_RPM_RATIO 0.85
+#define MOTOR_OPERATING_VOLTAGE 24
+#define MOTOR_POWER_MAX_VOLTAGE 14.6
+#define MOTOR_POWER_MEASURED_VOLTAGE 14.6                         
+#define COUNTS_PER_REV1 144384
+#define COUNTS_PER_REV2 140777
+#define COUNTS_PER_REV3 148326
+#define COUNTS_PER_REV4 144495
+#define WHEEL_DIAMETER 0.1               
+#define LR_WHEELS_DISTANCE 0.31            
+#define PWM_BITS 10                        
+#define PWM_FREQUENCY 20000
+
+/// ENCODER PINS
+#define MOTOR1_ENCODER_A 14
+#define MOTOR1_ENCODER_B 15 
+#define MOTOR1_ENCODER_INV false 
+
+#define MOTOR2_ENCODER_A 11
+#define MOTOR2_ENCODER_B 12 
+#define MOTOR2_ENCODER_INV false 
+
+#define MOTOR3_ENCODER_A 17
+#define MOTOR3_ENCODER_B 16 
+#define MOTOR3_ENCODER_INV true 
+
+#define MOTOR4_ENCODER_A 9
+#define MOTOR4_ENCODER_B 10
+#define MOTOR4_ENCODER_INV false 
+
+// Motor Pins
+#define MOTOR1_PWM 21
+#define MOTOR1_IN_A 20
+#define MOTOR1_IN_B -1 
+#define MOTOR1_INV false
+
+#define MOTOR2_PWM 5
+#define MOTOR2_IN_A 6
+#define MOTOR2_IN_B -1 
+#define MOTOR2_INV true
+
+#define MOTOR3_PWM 22
+#define MOTOR3_IN_A 23
+#define MOTOR3_IN_B -1
+#define MOTOR3_INV false
+
+#define MOTOR4_PWM 4
+#define MOTOR4_IN_A 3
+#define MOTOR4_IN_B -1
+#define MOTOR4_INV false
+
+#define PWM_MAX pow(2, PWM_BITS) - 1
+#define PWM_MIN -PWM_MAX
+
+#endif

config/custom/dev_config.h

@@ -0,0 +1,84 @@
+// Copyright (c) 2021 Juan Miguel Jimeno
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef DEV_CONFIG_H
+#define DEV_CONFIG_H
+
+#define LED_PIN 13 
+
+#define LINO_BASE SKID_STEER 
+
+#define USE_GENERIC_1_IN_MOTOR_DRIVER 
+#define USE_FAKE_IMU
+
+#define K_P 0.6                             
+#define K_I 0.8                             
+#define K_D 0.5                             
+
+#define MOTOR_MAX_RPM 140       
+#define MAX_RPM_RATIO 0.85
+#define MOTOR_OPERATING_VOLTAGE 24
+#define MOTOR_POWER_MAX_VOLTAGE 12
+#define MOTOR_POWER_MEASURED_VOLTAGE 12                  
+#define COUNTS_PER_REV1 144000
+#define COUNTS_PER_REV2 144000
+#define COUNTS_PER_REV3 144000
+#define COUNTS_PER_REV4 144000
+#define WHEEL_DIAMETER 0.152               
+#define LR_WHEELS_DISTANCE 0.271            
+#define PWM_BITS 8                         
+#define PWM_FREQUENCY 20000
+
+/// ENCODER PINS
+#define MOTOR1_ENCODER_A 14
+#define MOTOR1_ENCODER_B 15 
+#define MOTOR1_ENCODER_INV false 
+
+#define MOTOR2_ENCODER_A 11
+#define MOTOR2_ENCODER_B 12 
+#define MOTOR2_ENCODER_INV false 
+
+#define MOTOR3_ENCODER_A 17
+#define MOTOR3_ENCODER_B 16 
+#define MOTOR3_ENCODER_INV true 
+
+#define MOTOR4_ENCODER_A 9
+#define MOTOR4_ENCODER_B 10
+#define MOTOR4_ENCODER_INV false 
+
+// Motor Pins
+#define MOTOR1_PWM 21
+#define MOTOR1_IN_A 20
+#define MOTOR1_IN_B -1 
+#define MOTOR1_INV false
+
+#define MOTOR2_PWM 5
+#define MOTOR2_IN_A 6
+#define MOTOR2_IN_B -1 
+#define MOTOR2_INV true
+
+#define MOTOR3_PWM 22
+#define MOTOR3_IN_A 23
+#define MOTOR3_IN_B -1
+#define MOTOR3_INV false
+
+#define MOTOR4_PWM 4
+#define MOTOR4_IN_A 3
+#define MOTOR4_IN_B -1
+#define MOTOR4_INV false
+
+#define PWM_MAX pow(2, PWM_BITS) - 1
+#define PWM_MIN -PWM_MAX
+
+#endif

config/custom/square_config.h

@@ -0,0 +1,84 @@
+// Copyright (c) 2021 Juan Miguel Jimeno
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SQUARE_CONFIG_H
+#define SQUARE_CONFIG_H
+
+#define LED_PIN 13 
+
+#define LINO_BASE DIFFERENTIAL_DRIVE 
+
+#define USE_GENERIC_2_IN_MOTOR_DRIVER 
+#define USE_GY85_IMU
+
+#define K_P 20.                           
+#define K_I 0.8                             
+#define K_D 5.                             
+
+#define MOTOR_MAX_RPM 100
+#define MAX_RPM_RATIO 0.8
+#define MOTOR_OPERATING_VOLTAGE 12
+#define MOTOR_POWER_MAX_VOLTAGE 12
+#define MOTOR_POWER_MEASURED_VOLTAGE 11.67                         
+#define COUNTS_PER_REV1 2513
+#define COUNTS_PER_REV2 2580
+#define COUNTS_PER_REV3 2421
+#define COUNTS_PER_REV4 2501
+#define WHEEL_DIAMETER 0.09               
+#define LR_WHEELS_DISTANCE 0.24            
+#define PWM_BITS 10                        
+#define PWM_FREQUENCY 20000
+
+/// ENCODER PINS
+#define MOTOR1_ENCODER_A 14
+#define MOTOR1_ENCODER_B 15 
+#define MOTOR1_ENCODER_INV false 
+
+#define MOTOR2_ENCODER_A 11
+#define MOTOR2_ENCODER_B 12 
+#define MOTOR2_ENCODER_INV false 
+
+#define MOTOR3_ENCODER_A 17
+#define MOTOR3_ENCODER_B 16 
+#define MOTOR3_ENCODER_INV true 
+
+#define MOTOR4_ENCODER_A 9
+#define MOTOR4_ENCODER_B 10
+#define MOTOR4_ENCODER_INV false 
+
+// Motor Pins
+#define MOTOR1_PWM 1
+#define MOTOR1_IN_A 20
+#define MOTOR1_IN_B 21
+#define MOTOR1_INV false
+
+#define MOTOR2_PWM 5
+#define MOTOR2_IN_A 6
+#define MOTOR2_IN_B 8
+#define MOTOR2_INV true
+
+#define MOTOR3_PWM 22
+#define MOTOR3_IN_A 23
+#define MOTOR3_IN_B 0
+#define MOTOR3_INV false
+
+#define MOTOR4_PWM 4
+#define MOTOR4_IN_A 3
+#define MOTOR4_IN_B 2
+#define MOTOR4_INV false
+
+#define PWM_MAX pow(2, PWM_BITS) - 1
+#define PWM_MIN -PWM_MAX
+
+#endif

config/lino_base_config.h

@@ -0,0 +1,176 @@
+// Copyright (c) 2021 Juan Miguel Jimeno
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef LINO_BASE_CONFIG_H
+#define LINO_BASE_CONFIG_H
+
+#define LED_PIN 13 //LED_PIN LED_BUILTIN //13 //used for debugging status
+
+//uncomment the base you're building
+#define LINO_BASE DIFFERENTIAL_DRIVE       // 2WD and Tracked robot w/ 2 motors
+// #define LINO_BASE SKID_STEER            // 4WD robot
+// #define LINO_BASE MECANUM               // Mecanum drive robot
+
+//uncomment the motor driver you're using
+#define USE_GENERIC_2_IN_MOTOR_DRIVER      // Motor drivers with 2 Direction Pins(INA, INB) and 1 PWM(ENABLE) pin ie. L298, L293, VNH5019
+// #define USE_GENERIC_1_IN_MOTOR_DRIVER   // Motor drivers with 1 Direction Pin(INA) and 1 PWM(ENABLE) pin.
+// #define USE_BTS7960_MOTOR_DRIVER        // BTS7970 Motor Driver
+// #define USE_ESC_MOTOR_DRIVER            // Motor ESC for brushless motors
+
+//uncomment the IMU you're using
+// #define USE_GY85_IMU
+// #define USE_MPU6050_IMU
+// #define USE_MPU9150_IMU
+// #define USE_MPU9250_IMU
+#define USE_MPU9250_WE_IMU
+//#define USE_FAKE_IMU
+
+#define K_P 0.6                             // P constant
+#define K_I 0.8                             // I constant
+#define K_D 0.5                             // D constant
+
+/*
+ROBOT ORIENTATION
+         FRONT
+    MOTOR1  MOTOR2  (2WD/ACKERMANN)
+    MOTOR3  MOTOR4  (4WD/MECANUM)  
+         BACK
+*/
+
+//define your robot' specs here
+#define MOTOR_MAX_RPM 140                   // motor's max RPM          
+#define MAX_RPM_RATIO 0.85                  // max RPM allowed for each MAX_RPM_ALLOWED = MOTOR_MAX_RPM * MAX_RPM_RATIO          
+#define MOTOR_OPERATING_VOLTAGE 24          // motor's operating voltage (used to calculate max RPM)
+#define MOTOR_POWER_MAX_VOLTAGE 12          // max voltage of the motor's power source (used to calculate max RPM)
+#define MOTOR_POWER_MEASURED_VOLTAGE 12     // current voltage reading of the power connected to the motor (used for calibration)
+#define COUNTS_PER_REV1 144000              // wheel1 encoder's no of ticks per rev
+#define COUNTS_PER_REV2 144000              // wheel2 encoder's no of ticks per rev
+#define COUNTS_PER_REV3 144000              // wheel3 encoder's no of ticks per rev
+#define COUNTS_PER_REV4 144000              // wheel4 encoder's no of ticks per rev
+#define WHEEL_DIAMETER 0.152                // wheel's diameter in meters
+#define LR_WHEELS_DISTANCE 0.271            // distance between left and right wheels
+#define PWM_BITS 10                          // PWM Resolution of the microcontroller
+#define PWM_FREQUENCY 20000                 // PWM Frequency
+
+// INVERT ENCODER COUNTS
+#define MOTOR1_ENCODER_INV false 
+#define MOTOR2_ENCODER_INV false 
+#define MOTOR3_ENCODER_INV false 
+#define MOTOR4_ENCODER_INV false 
+
+// INVERT MOTOR DIRECTIONS
+#define MOTOR1_INV false
+#define MOTOR2_INV false
+#define MOTOR3_INV false
+#define MOTOR4_INV false
+
+// ENCODER PINS
+#define MOTOR1_ENCODER_A 14
+#define MOTOR1_ENCODER_B 15 
+
+#define MOTOR2_ENCODER_A 11
+#define MOTOR2_ENCODER_B 12 
+
+#define MOTOR3_ENCODER_A 17
+#define MOTOR3_ENCODER_B 16 
+
+#define MOTOR4_ENCODER_A 9
+#define MOTOR4_ENCODER_B 10
+
+// MOTOR PINS
+#ifdef USE_GENERIC_2_IN_MOTOR_DRIVER
+  #define MOTOR1_PWM 21 //Pin no 21 is not a PWM pin on Teensy 4.x, you can swap it with pin no 1 instead.
+  #define MOTOR1_IN_A 20
+  #define MOTOR1_IN_B 1 
+
+  #define MOTOR2_PWM 5
+  #define MOTOR2_IN_A 6
+  #define MOTOR2_IN_B 8
+
+  #define MOTOR3_PWM 22
+  #define MOTOR3_IN_A 23
+  #define MOTOR3_IN_B 0
+
+  #define MOTOR4_PWM 4
+  #define MOTOR4_IN_A 3
+  #define MOTOR4_IN_B 2
+
+  #define PWM_MAX pow(2, PWM_BITS) - 1
+  #define PWM_MIN -PWM_MAX
+#endif 
+
+#ifdef USE_GENERIC_1_IN_MOTOR_DRIVER
+  #define MOTOR1_PWM 21 //Pin no 21 is not a PWM pin on Teensy 4.x, you can use pin no 1 instead.
+  #define MOTOR1_IN_A 20
+  #define MOTOR1_IN_B -1 //DON'T TOUCH THIS! This is just a placeholder
+
+  #define MOTOR2_PWM 5
+  #define MOTOR2_IN_A 6
+  #define MOTOR2_IN_B -1 //DON'T TOUCH THIS! This is just a placeholder
+
+  #define MOTOR3_PWM 22
+  #define MOTOR3_IN_A 23
+  #define MOTOR3_IN_B -1 //DON'T TOUCH THIS! This is just a placeholder
+
+  #define MOTOR4_PWM 4
+  #define MOTOR4_IN_A 3
+  #define MOTOR4_IN_B -1 //DON'T TOUCH THIS! This is just a placeholder
+
+  #define PWM_MAX pow(2, PWM_BITS) - 1
+  #define PWM_MIN -PWM_MAX
+#endif 
+
+#ifdef USE_BTS7960_MOTOR_DRIVER
+  #define MOTOR1_PWM -1 //DON'T TOUCH THIS! This is just a placeholder
+  #define MOTOR1_IN_A 21 // Pin no 21 is not a PWM pin on Teensy 4.x, you can use pin no 1 instead.
+  #define MOTOR1_IN_B 20 // Pin no 20 is not a PWM pin on Teensy 4.x, you can use pin no 0 instead.
+
+  #define MOTOR2_PWM -1 //DON'T TOUCH THIS! This is just a placeholder
+  #define MOTOR2_IN_A 5
+  #define MOTOR2_IN_B 6
+
+  #define MOTOR3_PWM -1 //DON'T TOUCH THIS! This is just a placeholder
+  #define MOTOR3_IN_A 22
+  #define MOTOR3_IN_B 23
+
+  #define MOTOR4_PWM -1 //DON'T TOUCH THIS! This is just a placeholder
+  #define MOTOR4_IN_A 4
+  #define MOTOR4_IN_B 3
+
+  #define PWM_MAX pow(2, PWM_BITS) - 1
+  #define PWM_MIN -PWM_MAX
+#endif
+
+#ifdef USE_ESC_MOTOR_DRIVER
+  #define MOTOR1_PWM 21 //Pin no 21 is not a PWM pin on Teensy 4.x. You can use pin no 1 instead.
+  #define MOTOR1_IN_A -1 //DON'T TOUCH THIS! This is just a placeholder
+  #define MOTOR1_IN_B -1 //DON'T TOUCH THIS! This is just a placeholder
+
+  #define MOTOR2_PWM 5
+  #define MOTOR2_IN_A -1 //DON'T TOUCH THIS! This is just a placeholder
+  #define MOTOR2_IN_B -1 //DON'T TOUCH THIS! This is just a placeholder
+
+  #define MOTOR3_PWM 22 
+  #define MOTOR3_IN_A -1 //DON'T TOUCH THIS! This is just a placeholder
+  #define MOTOR3_IN_B -1 //DON'T TOUCH THIS! This is just a placeholder
+
+  #define MOTOR4_PWM 4
+  #define MOTOR4_IN_A -1 //DON'T TOUCH THIS! This is just a placeholder
+  #define MOTOR4_IN_B -1 //DON'T TOUCH THIS! This is just a placeholder
+
+  #define PWM_MAX 400
+  #define PWM_MIN -PWM_MAX
+#endif
+
+#endif

firmware/.gitignore

@@ -0,0 +1,5 @@
+.pio
+.vscode/.browse.c_cpp.db*
+.vscode/c_cpp_properties.json
+.vscode/launch.json
+.vscode/ipch

firmware/.vscode/extensions.json

@@ -0,0 +1,10 @@
+{
+    // See http://go.microsoft.com/fwlink/?LinkId=827846
+    // for the documentation about the extensions.json format
+    "recommendations": [
+        "platformio.platformio-ide"
+    ],
+    "unwantedRecommendations": [
+        "ms-vscode.cpptools-extension-pack"
+    ]
+}

firmware/.vscode/settings.json

@@ -0,0 +1,10 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/opt/ros/humble/lib/python3.10/site-packages",
+        "/opt/ros/humble/local/lib/python3.10/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/opt/ros/humble/lib/python3.10/site-packages",
+        "/opt/ros/humble/local/lib/python3.10/dist-packages"
+    ]
+}

firmware/include/README

@@ -0,0 +1,39 @@
+
+This directory is intended for project header files.
+
+A header file is a file containing C declarations and macro definitions
+to be shared between several project source files. You request the use of a
+header file in your project source file (C, C++, etc) located in `src` folder
+by including it, with the C preprocessing directive `#include'.
+
+```src/main.c
+
+#include "header.h"
+
+int main (void)
+{
+ ...
+}
+```
+
+Including a header file produces the same results as copying the header file
+into each source file that needs it. Such copying would be time-consuming
+and error-prone. With a header file, the related declarations appear
+in only one place. If they need to be changed, they can be changed in one
+place, and programs that include the header file will automatically use the
+new version when next recompiled. The header file eliminates the labor of
+finding and changing all the copies as well as the risk that a failure to
+find one copy will result in inconsistencies within a program.
+
+In C, the usual convention is to give header files names that end with `.h'.
+It is most portable to use only letters, digits, dashes, and underscores in
+header file names, and at most one dot.
+
+Read more about using header files in official GCC documentation:
+
+* Include Syntax
+* Include Operation
+* Once-Only Headers
+* Computed Includes
+
+https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html

firmware/lib/encoder/encoder.cpp

@@ -0,0 +1,10 @@
+
+// #include "encoder.h"
+
+// // Yes, all the code is in the header file, to provide the user
+// // configure options with #define (before they include it), and
+// // to facilitate some crafty optimizations!
+
+// Encoder_internal_state_t * Encoder::interruptArgs[];
+
+

firmware/lib/encoder/examples/Basic/Basic.pde

@@ -0,0 +1,29 @@
+/* Encoder Library - Basic Example
+ * http://www.pjrc.com/teensy/td_libs_Encoder.html
+ *
+ * This example code is in the public domain.
+ */
+
+#include <Encoder.h>
+
+// Change these two numbers to the pins connected to your encoder.
+//   Best Performance: both pins have interrupt capability
+//   Good Performance: only the first pin has interrupt capability
+//   Low Performance:  neither pin has interrupt capability
+Encoder myEnc(5, 6);
+//   avoid using pins with LEDs attached
+
+void setup() {
+  Serial.begin(9600);
+  Serial.println("Basic Encoder Test:");
+}
+
+long oldPosition  = -999;
+
+void loop() {
+  long newPosition = myEnc.read();
+  if (newPosition != oldPosition) {
+    oldPosition = newPosition;
+    Serial.println(newPosition);
+  }
+}

firmware/lib/encoder/examples/NoInterrupts/NoInterrupts.pde

@@ -0,0 +1,46 @@
+/* Encoder Library - NoInterrupts Example
+ * http://www.pjrc.com/teensy/td_libs_Encoder.html
+ *
+ * This example code is in the public domain.
+ */
+
+// If you define ENCODER_DO_NOT_USE_INTERRUPTS *before* including
+// Encoder, the library will never use interrupts.  This is mainly
+// useful to reduce the size of the library when you are using it
+// with pins that do not support interrupts.  Without interrupts,
+// your program must call the read() function rapidly, or risk
+// missing changes in position.
+#define ENCODER_DO_NOT_USE_INTERRUPTS
+#include <Encoder.h>
+
+// Beware of Serial.print() speed.  Without interrupts, if you
+// transmit too much data with Serial.print() it can slow your
+// reading from Encoder.  Arduino 1.0 has improved transmit code.
+// Using the fastest baud rate also helps.  Teensy has USB packet
+// buffering.  But all boards can experience problems if you print
+// too much and fill up buffers.
+
+// Change these two numbers to the pins connected to your encoder.
+//   With ENCODER_DO_NOT_USE_INTERRUPTS, no interrupts are ever
+//   used, even if the pin has interrupt capability
+Encoder myEnc(5, 6);
+//   avoid using pins with LEDs attached
+
+void setup() {
+  Serial.begin(9600);
+  Serial.println("Basic NoInterrupts Test:");
+}
+
+long position  = -999;
+
+void loop() {
+  long newPos = myEnc.read();
+  if (newPos != position) {
+    position = newPos;
+    Serial.println(position);
+  }
+  // With any substantial delay added, Encoder can only track
+  // very slow motion.  You may uncomment this line to see
+  // how badly a delay affects your encoder.
+  //delay(50);
+}

firmware/lib/encoder/examples/SpeedTest/SpeedTest.pde

@@ -0,0 +1,113 @@
+/* Encoder Library - SpeedTest - for measuring maximum Encoder speed
+ * http://www.pjrc.com/teensy/td_libs_Encoder.html
+ *
+ * This example code is in the public domain.
+ */
+
+
+// This SpeedTest example provides a simple way to verify how much
+// CPU time Encoder is consuming.  Connect a DC voltmeter to the
+// output pin and measure the voltage while the encoder is stopped
+// or running at a very slow speed.  Even though the pin is rapidly
+// pulsing, a DC voltmeter will show the average voltage.  Due to
+// software timing, it will read a number much less than a steady
+// logic high, but this number will give you a baseline reading
+// for output with minimal interrupt overhead.  Then increase the
+// encoder speed.  The voltage will decrease as the processor spends
+// more time in Encoder's interrupt routines counting the pulses
+// and less time pulsing the output pin.  When the voltage is
+// close to zero and will not decrease any farther, you have reached
+// the absolute speed limit.  Or, if using a mechanical system where
+// you reach a speed limit imposed by your motors or other hardware,
+// the amount this voltage has decreased, compared to the baseline,
+// should give you a good approximation of the portion of available
+// CPU time Encoder is consuming at your maximum speed.
+
+// Encoder requires low latency interrupt response.  Available CPU
+// time does NOT necessarily prove or guarantee correct performance.
+// If another library, like NewSoftSerial, is disabling interrupts
+// for lengthy periods of time, Encoder can be prevented from
+// properly counting the intput signals while interrupt are disabled.
+
+
+// This optional setting causes Encoder to use more optimized code,
+// but the downside is a conflict if any other part of your sketch
+// or any other library you're using requires attachInterrupt().
+// It must be defined before Encoder.h is included.
+//#define ENCODER_OPTIMIZE_INTERRUPTS
+
+#include <Encoder.h>
+#include "pins_arduino.h"
+
+// Change these two numbers to the pins connected to your encoder
+// or shift register circuit which emulates a quadrature encoder
+//  case 1: both pins are interrupts
+//  case 2: only first pin used as interrupt
+Encoder myEnc(5, 6);
+
+// Connect a DC voltmeter to this pin.
+const int outputPin = 12;
+
+/* This simple circuit, using a Dual Flip-Flop chip, can emulate
+   quadrature encoder signals.  The clock can come from a fancy
+   function generator or a cheap 555 timer chip.  The clock
+   frequency can be measured with another board running FreqCount
+   http://www.pjrc.com/teensy/td_libs_FreqCount.html
+
+                        +5V
+                         |        Quadrature Encoder Signal Emulator
+ Clock                   |
+ Input o----*--------------------------      ---------------------------o Output1
+            |            |14           |    |
+            |     _______|_______      |    |     _______________ 
+            |    |    CD4013     |     |    |    |    CD4013     |
+            |  5 |               | 1   |    |  9 |               | 13
+        ---------|  D         Q  |-----|----*----|  D         Q  |------o Output2
+       |    |    |               |     |         |               |
+       |    |  3 |               |     |      11 |               |
+       |     ----|> Clk          |      ---------|> Clk          |
+       |         |               |               |               |
+       |       6 |               |             8 |               |
+       |     ----|  S            |           ----|  S            |
+       |    |    |               |          |    |               |
+       |    |  4 |            _  | 2        | 10 |            _  | 12
+       |    *----|  R         Q  |---       *----|  R         Q  |----
+       |    |    |               |          |    |               |    |
+       |    |    |_______________|          |    |_______________|    |
+       |    |            |                  |                         |
+       |    |            | 7                |                         |
+       |    |            |                  |                         |
+        --------------------------------------------------------------
+            |            |                  |
+            |            |                  |
+          -----        -----              -----
+           ---          ---                ---
+            -            -                  -
+*/
+
+
+void setup() {
+  pinMode(outputPin, OUTPUT);
+}
+
+#if defined(__AVR__)
+#define REGTYPE unsigned char
+#elif defined(__PIC32MX__)
+#define REGTYPE unsigned long
+#endif
+
+void loop() {
+  volatile int count = 0;
+  volatile REGTYPE *reg = portOutputRegister(digitalPinToPort(outputPin));
+  REGTYPE mask = digitalPinToBitMask(outputPin);
+
+  while (1) {
+    myEnc.read();	// Read the encoder while interrupts are enabled.
+    noInterrupts();
+    *reg |= mask;	// Pulse the pin high, while interrupts are disabled.
+    count = count + 1;
+    *reg &= ~mask;
+    interrupts();
+  }
+}
+

firmware/lib/encoder/examples/TwoKnobs/TwoKnobs.pde

@@ -0,0 +1,46 @@
+/* Encoder Library - TwoKnobs Example
+ * http://www.pjrc.com/teensy/td_libs_Encoder.html
+ *
+ * This example code is in the public domain.
+ */
+
+#include <Encoder.h>
+
+// Change these pin numbers to the pins connected to your encoder.
+//   Best Performance: both pins have interrupt capability
+//   Good Performance: only the first pin has interrupt capability
+//   Low Performance:  neither pin has interrupt capability
+Encoder knobLeft(5, 6);
+Encoder knobRight(7, 8);
+//   avoid using pins with LEDs attached
+
+void setup() {
+  Serial.begin(9600);
+  Serial.println("TwoKnobs Encoder Test:");
+}
+
+long positionLeft  = -999;
+long positionRight = -999;
+
+void loop() {
+  long newLeft, newRight;
+  newLeft = knobLeft.read();
+  newRight = knobRight.read();
+  if (newLeft != positionLeft || newRight != positionRight) {
+    Serial.print("Left = ");
+    Serial.print(newLeft);
+    Serial.print(", Right = ");
+    Serial.print(newRight);
+    Serial.println();
+    positionLeft = newLeft;
+    positionRight = newRight;
+  }
+  // if a character is sent from the serial monitor,
+  // reset both back to zero.
+  if (Serial.available()) {
+    Serial.read();
+    Serial.println("Reset both knobs to zero");
+    knobLeft.write(0);
+    knobRight.write(0);
+  }
+}

firmware/lib/encoder/keywords.txt

@@ -0,0 +1,4 @@
+ENCODER_USE_INTERRUPTS	LITERAL1
+ENCODER_OPTIMIZE_INTERRUPTS	LITERAL1
+ENCODER_DO_NOT_USE_INTERRUPTS	LITERAL1
+Encoder	KEYWORD1

firmware/lib/encoder/utility/direct_pin_read.h

@@ -0,0 +1,110 @@
+#ifndef direct_pin_read_h_
+#define direct_pin_read_h_
+
+#if defined(__AVR__)
+
+#define IO_REG_TYPE			uint8_t
+#define PIN_TO_BASEREG(pin)             (portInputRegister(digitalPinToPort(pin)))
+#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
+#define DIRECT_PIN_READ(base, mask)     (((*(base)) & (mask)) ? 1 : 0)
+
+#elif defined(TEENSYDUINO) && (defined(KINETISK) || defined(KINETISL))
+
+#define IO_REG_TYPE			uint8_t
+#define PIN_TO_BASEREG(pin)             (portInputRegister(digitalPinToPort(pin)))
+#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
+#define DIRECT_PIN_READ(base, mask)     (((*(base)) & (mask)) ? 1 : 0)
+
+#elif defined(__IMXRT1052__) || defined(__IMXRT1062__)
+
+#define IO_REG_TYPE			uint32_t
+#define PIN_TO_BASEREG(pin)             (portOutputRegister(pin))
+#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
+#define DIRECT_PIN_READ(base, mask)     (((*(base)) & (mask)) ? 1 : 0)
+
+#elif defined(__SAM3X8E__)  // || defined(ESP8266)
+
+#define IO_REG_TYPE			uint32_t
+#define PIN_TO_BASEREG(pin)             (portInputRegister(digitalPinToPort(pin)))
+#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
+#define DIRECT_PIN_READ(base, mask)     (((*(base)) & (mask)) ? 1 : 0)
+
+#elif defined(__PIC32MX__)
+
+#define IO_REG_TYPE			uint32_t
+#define PIN_TO_BASEREG(pin)             (portModeRegister(digitalPinToPort(pin)))
+#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
+#define DIRECT_PIN_READ(base, mask)	(((*(base+4)) & (mask)) ? 1 : 0)
+
+/* ESP8266 v2.0.0 Arduino workaround for bug https://github.com/esp8266/Arduino/issues/1110 */
+#elif defined(ESP8266)
+
+#define IO_REG_TYPE			uint32_t
+#define PIN_TO_BASEREG(pin)             ((volatile uint32_t *)(0x60000000+(0x318)))
+#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
+#define DIRECT_PIN_READ(base, mask)     (((*(base)) & (mask)) ? 1 : 0)
+
+/* ESP32  Arduino (https://github.com/espressif/arduino-esp32) */
+#elif defined(ESP32)
+
+#define IO_REG_TYPE			uint32_t
+#define PIN_TO_BASEREG(pin)             (portInputRegister(digitalPinToPort(pin)))
+#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
+#define DIRECT_PIN_READ(base, mask)     (((*(base)) & (mask)) ? 1 : 0)
+
+#elif defined(__SAMD21G18A__) || defined(__SAMD21E18A__)
+
+#define IO_REG_TYPE                     uint32_t
+#define PIN_TO_BASEREG(pin)             portModeRegister(digitalPinToPort(pin))
+#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
+#define DIRECT_PIN_READ(base, mask)     (((*((base)+8)) & (mask)) ? 1 : 0)
+
+#elif defined(__SAMD51__)
+
+#define IO_REG_TYPE                     uint32_t
+#define PIN_TO_BASEREG(pin)             portInputRegister(digitalPinToPort(pin))
+#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
+#define DIRECT_PIN_READ(base, mask)     (((*(base)) & (mask)) ? 1 : 0)
+
+#elif defined(RBL_NRF51822)
+
+#define IO_REG_TYPE                     uint32_t
+#define PIN_TO_BASEREG(pin)             (0)
+#define PIN_TO_BITMASK(pin)             (pin)
+#define DIRECT_PIN_READ(base, pin)      nrf_gpio_pin_read(pin)
+
+#elif defined(ARDUINO_ARCH_NRF52840)
+#define IO_REG_TYPE                     uint32_t
+#define PIN_TO_BASEREG(pin)             (0)
+#define PIN_TO_BITMASK(pin)             digitalPinToPinName(pin)
+#define DIRECT_PIN_READ(base, pin)      nrf_gpio_pin_read(pin)
+
+#elif defined(__arc__) /* Arduino101/Genuino101 specifics */
+
+#include "scss_registers.h"
+#include "portable.h"
+#include "avr/pgmspace.h"
+#define GPIO_ID(pin)                    (g_APinDescription[pin].ulGPIOId)
+#define GPIO_TYPE(pin)                  (g_APinDescription[pin].ulGPIOType)
+#define GPIO_BASE(pin)                  (g_APinDescription[pin].ulGPIOBase)
+#define EXT_PORT_OFFSET_SS              0x0A
+#define EXT_PORT_OFFSET_SOC             0x50
+#define PIN_TO_BASEREG(pin)             ((volatile uint32_t *)g_APinDescription[pin].ulGPIOBase)
+#define PIN_TO_BITMASK(pin)             pin
+#define IO_REG_TYPE                     uint32_t
+static inline __attribute__((always_inline))
+IO_REG_TYPE directRead(volatile IO_REG_TYPE *base, IO_REG_TYPE pin)
+{
+    IO_REG_TYPE ret;
+    if (SS_GPIO == GPIO_TYPE(pin)) {
+        ret = READ_ARC_REG(((IO_REG_TYPE)base + EXT_PORT_OFFSET_SS));
+    } else {
+        ret = MMIO_REG_VAL_FROM_BASE((IO_REG_TYPE)base, EXT_PORT_OFFSET_SOC);
+    }
+    return ((ret >> GPIO_ID(pin)) & 0x01);
+}
+#define DIRECT_PIN_READ(base, pin)      directRead(base, pin)
+
+#endif
+
+#endif

firmware/lib/encoder/utility/interrupt_config.h

@@ -0,0 +1,87 @@
+#if defined(__AVR__)
+
+#include <avr/io.h>
+#include <avr/interrupt.h>
+
+#define attachInterrupt(num, func, mode) enableInterrupt(num)
+#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+#define SCRAMBLE_INT_ORDER(num) ((num < 4) ? num + 2 : ((num < 6) ? num - 4 : num))
+#define DESCRAMBLE_INT_ORDER(num) ((num < 2) ? num + 4 : ((num < 6) ? num - 2 : num))
+#else
+#define SCRAMBLE_INT_ORDER(num) (num)
+#define DESCRAMBLE_INT_ORDER(num) (num)
+#endif
+
+static void enableInterrupt(uint8_t num)
+{
+	switch (DESCRAMBLE_INT_ORDER(num)) {
+		#if defined(EICRA) && defined(EIMSK)
+		case 0:
+			EICRA = (EICRA & 0xFC) | 0x01;
+			EIMSK |= 0x01;
+			return;
+		case 1:
+			EICRA = (EICRA & 0xF3) | 0x04;
+			EIMSK |= 0x02;
+			return;
+		case 2:
+			EICRA = (EICRA & 0xCF) | 0x10;
+			EIMSK |= 0x04;
+			return;
+		case 3:
+			EICRA = (EICRA & 0x3F) | 0x40;
+			EIMSK |= 0x08;
+			return;
+		#elif defined(MCUCR) && defined(GICR)
+		case 0:
+			MCUCR = (MCUCR & ~((1 << ISC00) | (1 << ISC01))) | (mode << ISC00);
+			GICR |= (1 << INT0);
+			return;
+		case 1:
+			MCUCR = (MCUCR & ~((1 << ISC10) | (1 << ISC11))) | (mode << ISC10);
+			GICR |= (1 << INT1);
+			return;
+		#elif defined(MCUCR) && defined(GIMSK)
+		case 0:
+			MCUCR = (MCUCR & ~((1 << ISC00) | (1 << ISC01))) | (mode << ISC00);
+			GIMSK |= (1 << INT0);
+			return;
+		case 1:
+			MCUCR = (MCUCR & ~((1 << ISC10) | (1 << ISC11))) | (mode << ISC10);
+			GIMSK |= (1 << INT1);
+			return;
+		#endif
+		#if defined(EICRB) && defined(EIMSK)
+		case 4:
+			EICRB = (EICRB & 0xFC) | 0x01;
+			EIMSK |= 0x10;
+			return;
+		case 5:
+			EICRB = (EICRB & 0xF3) | 0x04;
+			EIMSK |= 0x20;
+			return;
+		case 6:
+			EICRB = (EICRB & 0xCF) | 0x10;
+			EIMSK |= 0x40;
+			return;
+		case 7:
+			EICRB = (EICRB & 0x3F) | 0x40;
+			EIMSK |= 0x80;
+			return;
+		#endif
+	}
+}
+
+#elif defined(__PIC32MX__)
+
+#ifdef ENCODER_OPTIMIZE_INTERRUPTS
+#undef ENCODER_OPTIMIZE_INTERRUPTS
+#endif
+
+#else
+
+#ifdef ENCODER_OPTIMIZE_INTERRUPTS
+#undef ENCODER_OPTIMIZE_INTERRUPTS
+#endif
+
+#endif

firmware/lib/encoder/utility/interrupt_pins.h

@@ -0,0 +1,369 @@
+// interrupt pins for known boards
+
+// Teensy (and maybe others) define these automatically
+#if !defined(CORE_NUM_INTERRUPT)
+
+// Wiring boards
+#if defined(WIRING)
+  #define CORE_NUM_INTERRUPT	NUM_EXTERNAL_INTERRUPTS
+  #if NUM_EXTERNAL_INTERRUPTS > 0
+  #define CORE_INT0_PIN		EI0
+  #endif
+  #if NUM_EXTERNAL_INTERRUPTS > 1
+  #define CORE_INT1_PIN		EI1
+  #endif
+  #if NUM_EXTERNAL_INTERRUPTS > 2
+  #define CORE_INT2_PIN		EI2
+  #endif
+  #if NUM_EXTERNAL_INTERRUPTS > 3
+  #define CORE_INT3_PIN		EI3
+  #endif
+  #if NUM_EXTERNAL_INTERRUPTS > 4
+  #define CORE_INT4_PIN		EI4
+  #endif
+  #if NUM_EXTERNAL_INTERRUPTS > 5
+  #define CORE_INT5_PIN		EI5
+  #endif
+  #if NUM_EXTERNAL_INTERRUPTS > 6
+  #define CORE_INT6_PIN		EI6
+  #endif
+  #if NUM_EXTERNAL_INTERRUPTS > 7
+  #define CORE_INT7_PIN		EI7
+  #endif
+
+// Arduino Uno, Duemilanove, Diecimila, LilyPad, Mini, Fio, etc...
+#elif defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328PB__) ||defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__)
+  #define CORE_NUM_INTERRUPT	2
+  #define CORE_INT0_PIN		2
+  #define CORE_INT1_PIN		3
+
+// Arduino Mega
+#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+  #define CORE_NUM_INTERRUPT	6
+  #define CORE_INT0_PIN		2
+  #define CORE_INT1_PIN		3
+  #define CORE_INT2_PIN		21
+  #define CORE_INT3_PIN		20
+  #define CORE_INT4_PIN		19
+  #define CORE_INT5_PIN		18
+
+// Arduino Nano Every, Uno R2 Wifi
+#elif defined(__AVR_ATmega4809__)
+  #define CORE_NUM_INTERRUPT  22
+  #define CORE_INT0_PIN   0
+  #define CORE_INT1_PIN   1
+  #define CORE_INT2_PIN   2
+  #define CORE_INT3_PIN   3
+  #define CORE_INT4_PIN   4
+  #define CORE_INT5_PIN   5
+  #define CORE_INT6_PIN   6
+  #define CORE_INT7_PIN   7
+  #define CORE_INT8_PIN   8
+  #define CORE_INT9_PIN   9
+  #define CORE_INT10_PIN  10
+  #define CORE_INT11_PIN  11
+  #define CORE_INT12_PIN  12
+  #define CORE_INT13_PIN  13
+  #define CORE_INT14_PIN  14
+  #define CORE_INT15_PIN  15
+  #define CORE_INT16_PIN  16
+  #define CORE_INT17_PIN  17
+  #define CORE_INT18_PIN  18
+  #define CORE_INT19_PIN  19
+  #define CORE_INT20_PIN  20
+  #define CORE_INT21_PIN  21
+
+// Arduino Leonardo (untested)
+#elif defined(__AVR_ATmega32U4__) && !defined(CORE_TEENSY)
+  #define CORE_NUM_INTERRUPT	5
+  #define CORE_INT0_PIN		3
+  #define CORE_INT1_PIN		2
+  #define CORE_INT2_PIN		0
+  #define CORE_INT3_PIN		1
+  #define CORE_INT4_PIN		7
+
+// Sanguino (untested) and ATmega1284P
+#elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) || defined(__AVR_ATmega1284P__)
+  #define CORE_NUM_INTERRUPT	3
+  #define CORE_INT0_PIN		10
+  #define CORE_INT1_PIN		11
+  #define CORE_INT2_PIN		2
+
+// ATmega32u2 and ATmega32u16 based boards with HoodLoader2
+#elif defined(__AVR_ATmega32U2__) || defined(__AVR_ATmega16U2__)
+  #define CORE_NUM_INTERRUPT 8
+  #define CORE_INT0_PIN 8
+  #define CORE_INT1_PIN 17
+  #define CORE_INT2_PIN 13
+  #define CORE_INT3_PIN 14
+  #define CORE_INT4_PIN 15
+  #define CORE_INT5_PIN 16
+  #define CORE_INT6_PIN 19
+  #define CORE_INT7_PIN 20
+
+// Chipkit Uno32 - attachInterrupt may not support CHANGE option
+#elif defined(__PIC32MX__) && defined(_BOARD_UNO_)
+  #define CORE_NUM_INTERRUPT	5
+  #define CORE_INT0_PIN		38
+  #define CORE_INT1_PIN		2
+  #define CORE_INT2_PIN		7
+  #define CORE_INT3_PIN		8
+  #define CORE_INT4_PIN		35
+
+// Chipkit Uno32 - attachInterrupt may not support CHANGE option
+#elif defined(__PIC32MX__) && defined(_BOARD_MEGA_)
+  #define CORE_NUM_INTERRUPT	5
+  #define CORE_INT0_PIN		3
+  #define CORE_INT1_PIN		2
+  #define CORE_INT2_PIN		7
+  #define CORE_INT3_PIN		21
+  #define CORE_INT4_PIN		20
+
+// http://hlt.media.mit.edu/?p=1229
+#elif defined(__AVR_ATtiny45__) || defined(__AVR_ATtiny85__)
+  #define CORE_NUM_INTERRUPT    1
+  #define CORE_INT0_PIN		2
+
+  // ATtiny44 ATtiny84
+#elif defined(__AVR_ATtiny44__) || defined(__AVR_ATtiny84__)
+  #define CORE_NUM_INTERRUPT	1
+  #define CORE_INT0_PIN		8
+  
+// ATtiny441 ATtiny841
+#elif defined(__AVR_ATtiny441__) || defined(__AVR_ATtiny841__)
+  #define CORE_NUM_INTERRUPT	1
+  #define CORE_INT0_PIN		9
+
+//https://github.com/SpenceKonde/ATTinyCore/blob/master/avr/extras/ATtiny_x313.md
+#elif defined(__AVR_ATtinyX313__)
+  #define CORE_NUM_INTERRUPT    2
+  #define CORE_INT0_PIN		4
+  #define CORE_INT1_PIN		5
+ 
+// Attiny167 same core as abobe
+#elif defined(__AVR_ATtiny167__)
+  #define CORE_NUM_INTERRUPT	2
+  #define CORE_INT0_PIN		14
+  #define CORE_INT1_PIN		3
+
+  
+// Arduino Due
+#elif defined(__SAM3X8E__) 
+  #define CORE_NUM_INTERRUPT	54
+  #define CORE_INT0_PIN		0
+  #define CORE_INT1_PIN		1
+  #define CORE_INT2_PIN		2
+  #define CORE_INT3_PIN		3
+  #define CORE_INT4_PIN		4
+  #define CORE_INT5_PIN		5
+  #define CORE_INT6_PIN		6
+  #define CORE_INT7_PIN		7
+  #define CORE_INT8_PIN		8
+  #define CORE_INT9_PIN		9
+  #define CORE_INT10_PIN	10
+  #define CORE_INT11_PIN	11
+  #define CORE_INT12_PIN	12
+  #define CORE_INT13_PIN	13
+  #define CORE_INT14_PIN	14
+  #define CORE_INT15_PIN	15
+  #define CORE_INT16_PIN	16
+  #define CORE_INT17_PIN	17
+  #define CORE_INT18_PIN	18
+  #define CORE_INT19_PIN	19
+  #define CORE_INT20_PIN	20
+  #define CORE_INT21_PIN	21
+  #define CORE_INT22_PIN	22
+  #define CORE_INT23_PIN	23
+  #define CORE_INT24_PIN	24
+  #define CORE_INT25_PIN	25
+  #define CORE_INT26_PIN	26
+  #define CORE_INT27_PIN	27
+  #define CORE_INT28_PIN	28
+  #define CORE_INT29_PIN	29
+  #define CORE_INT30_PIN	30
+  #define CORE_INT31_PIN	31
+  #define CORE_INT32_PIN	32
+  #define CORE_INT33_PIN	33
+  #define CORE_INT34_PIN	34
+  #define CORE_INT35_PIN	35
+  #define CORE_INT36_PIN	36
+  #define CORE_INT37_PIN	37
+  #define CORE_INT38_PIN	38
+  #define CORE_INT39_PIN	39
+  #define CORE_INT40_PIN	40
+  #define CORE_INT41_PIN	41
+  #define CORE_INT42_PIN	42
+  #define CORE_INT43_PIN	43
+  #define CORE_INT44_PIN	44
+  #define CORE_INT45_PIN	45
+  #define CORE_INT46_PIN	46
+  #define CORE_INT47_PIN	47
+  #define CORE_INT48_PIN	48
+  #define CORE_INT49_PIN	49
+  #define CORE_INT50_PIN	50
+  #define CORE_INT51_PIN	51
+  #define CORE_INT52_PIN	52
+  #define CORE_INT53_PIN	53
+
+// ESP8266 (https://github.com/esp8266/Arduino/)
+#elif defined(ESP8266)
+  #define CORE_NUM_INTERRUPT EXTERNAL_NUM_INTERRUPTS
+  #define CORE_INT0_PIN		0
+  #define CORE_INT1_PIN		1
+  #define CORE_INT2_PIN		2
+  #define CORE_INT3_PIN		3
+  #define CORE_INT4_PIN		4
+  #define CORE_INT5_PIN		5
+  // GPIO6-GPIO11 are typically used to interface with the flash memory IC on 
+  // most esp8266 modules, so we should avoid adding interrupts to these pins.
+  #define CORE_INT12_PIN	12
+  #define CORE_INT13_PIN	13
+  #define CORE_INT14_PIN	14
+  #define CORE_INT15_PIN	15
+
+// ESP32 (https://github.com/espressif/arduino-esp32)
+#elif defined(ESP32)
+
+  #define CORE_NUM_INTERRUPT  40 
+  #define CORE_INT0_PIN		0
+  #define CORE_INT1_PIN		1
+  #define CORE_INT2_PIN		2
+  #define CORE_INT3_PIN		3
+  #define CORE_INT4_PIN		4
+  #define CORE_INT5_PIN		5
+  // GPIO6-GPIO11 are typically used to interface with the flash memory IC on 
+  // esp32, so we should avoid adding interrupts to these pins.
+  #define CORE_INT12_PIN	12
+  #define CORE_INT13_PIN	13
+  #define CORE_INT14_PIN	14
+  #define CORE_INT15_PIN	15
+  #define CORE_INT16_PIN        16
+  #define CORE_INT17_PIN        17
+  #define CORE_INT18_PIN        18
+  #define CORE_INT19_PIN        19
+  #define CORE_INT21_PIN        21
+  #define CORE_INT22_PIN        22
+  #define CORE_INT23_PIN        23
+  #define CORE_INT25_PIN        25
+  #define CORE_INT26_PIN        26
+  #define CORE_INT27_PIN        27
+  #define CORE_INT32_PIN        32
+  #define CORE_INT33_PIN        33
+  #define CORE_INT34_PIN        34
+  #define CORE_INT35_PIN        35
+  #define CORE_INT36_PIN        36
+  #define CORE_INT39_PIN        39
+
+
+// Arduino Zero - TODO: interrupts do not seem to work
+//                      please help, contribute a fix!
+#elif defined(__SAMD21G18A__) || defined(__SAMD21E18A__)
+  #define CORE_NUM_INTERRUPT	31
+  #define CORE_INT0_PIN		0
+  #define CORE_INT1_PIN		1
+  #define CORE_INT2_PIN		2
+  #define CORE_INT3_PIN		3
+  #define CORE_INT4_PIN		4
+  #define CORE_INT5_PIN		5
+  #define CORE_INT6_PIN		6
+  #define CORE_INT7_PIN		7
+  #define CORE_INT8_PIN		8
+  #define CORE_INT9_PIN		9
+  #define CORE_INT10_PIN	10
+  #define CORE_INT11_PIN	11
+  #define CORE_INT12_PIN	12
+  #define CORE_INT13_PIN	13
+  #define CORE_INT14_PIN	14
+  #define CORE_INT15_PIN	15
+  #define CORE_INT16_PIN	16
+  #define CORE_INT17_PIN	17
+  #define CORE_INT18_PIN	18
+  #define CORE_INT19_PIN	19
+  #define CORE_INT20_PIN	20
+  #define CORE_INT21_PIN	21
+  #define CORE_INT22_PIN	22
+  #define CORE_INT23_PIN	23
+  #define CORE_INT24_PIN	24
+  #define CORE_INT25_PIN	25
+  #define CORE_INT26_PIN	26
+  #define CORE_INT27_PIN	27
+  #define CORE_INT28_PIN	28
+  #define CORE_INT29_PIN	29
+  #define CORE_INT30_PIN	30
+
+#elif defined(__SAMD51__)
+  #define CORE_NUM_INTERRUPT	26
+  #define CORE_INT0_PIN		0
+  #define CORE_INT1_PIN		1
+  #define CORE_INT2_PIN		2
+  #define CORE_INT3_PIN		3
+  #define CORE_INT4_PIN		4
+  #define CORE_INT5_PIN		5
+  #define CORE_INT6_PIN		6
+  #define CORE_INT7_PIN		7
+  #define CORE_INT8_PIN		8
+  #define CORE_INT9_PIN		9
+  #define CORE_INT10_PIN	10
+  #define CORE_INT11_PIN	11
+  #define CORE_INT12_PIN	12
+  #define CORE_INT13_PIN	13
+  #define CORE_INT14_PIN	14
+  #define CORE_INT15_PIN	15
+  #define CORE_INT16_PIN	16
+  #define CORE_INT17_PIN	17
+  #define CORE_INT18_PIN	18
+  #define CORE_INT19_PIN	19
+  #define CORE_INT20_PIN	20
+  #define CORE_INT21_PIN	21
+  #define CORE_INT22_PIN	22
+  #define CORE_INT23_PIN	23
+  #define CORE_INT24_PIN	24
+  #define CORE_INT25_PIN	25
+
+// Arduino 101
+#elif defined(__arc__)
+  #define CORE_NUM_INTERRUPT	14
+  #define CORE_INT2_PIN		2
+  #define CORE_INT5_PIN		5
+  #define CORE_INT7_PIN		7
+  #define CORE_INT8_PIN		8
+  #define CORE_INT10_PIN	10
+  #define CORE_INT11_PIN	11
+  #define CORE_INT12_PIN	12
+  #define CORE_INT13_PIN	13
+
+// Arduino Nano 33 BLE
+#elif defined(ARDUINO_ARCH_NRF52840)
+  #define CORE_NUM_INTERRUPT	22
+  #define CORE_INT0_PIN		0
+  #define CORE_INT1_PIN		1
+  #define CORE_INT2_PIN		2
+  #define CORE_INT3_PIN		3
+  #define CORE_INT4_PIN		4
+  #define CORE_INT5_PIN		5
+  #define CORE_INT6_PIN		6
+  #define CORE_INT7_PIN		7
+  #define CORE_INT8_PIN		8
+  #define CORE_INT9_PIN		9
+  #define CORE_INT10_PIN	10
+  #define CORE_INT11_PIN	11
+  #define CORE_INT12_PIN	12
+  #define CORE_INT13_PIN	13
+  #define CORE_INT14_PIN	A0
+  #define CORE_INT15_PIN	A1
+  #define CORE_INT16_PIN	A2
+  #define CORE_INT17_PIN	A3
+  #define CORE_INT18_PIN	A4
+  #define CORE_INT19_PIN	A5
+  #define CORE_INT20_PIN	A6
+  #define CORE_INT21_PIN	A7
+#endif
+#endif
+
+#if !defined(CORE_NUM_INTERRUPT)
+#error "Interrupts are unknown for this board, please add to this code"
+#endif
+#if CORE_NUM_INTERRUPT <= 0
+#error "Encoder requires interrupt pins, but this board does not have any :("
+#error "You could try defining ENCODER_DO_NOT_USE_INTERRUPTS as a kludge."
+#endif

firmware/lib/imu/ADXL345.h

@@ -0,0 +1,361 @@
+// I2Cdev library collection - ADXL345 I2C device class header file
+// Based on Analog Devices ADXL345 datasheet rev. C, 5/2011
+// 7/31/2011 by Jeff Rowberg <jeff@rowberg.net>
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//     2011-07-31 - initial release
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2011 Jeff Rowberg
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+===============================================
+*/
+
+#ifndef _ADXL345_H_
+#define _ADXL345_H_
+
+#include "I2Cdev.h"
+
+#define ADXL345_ADDRESS_ALT_LOW     0x53 // alt address pin low (GND)
+#define ADXL345_ADDRESS_ALT_HIGH    0x1D // alt address pin high (VCC)
+#define ADXL345_DEFAULT_ADDRESS     ADXL345_ADDRESS_ALT_LOW
+
+#define ADXL345_RA_DEVID            0x00
+#define ADXL345_RA_RESERVED1        0x01
+#define ADXL345_RA_THRESH_TAP       0x1D
+#define ADXL345_RA_OFSX             0x1E
+#define ADXL345_RA_OFSY             0x1F
+#define ADXL345_RA_OFSZ             0x20
+#define ADXL345_RA_DUR              0x21
+#define ADXL345_RA_LATENT           0x22
+#define ADXL345_RA_WINDOW           0x23
+#define ADXL345_RA_THRESH_ACT       0x24
+#define ADXL345_RA_THRESH_INACT     0x25
+#define ADXL345_RA_TIME_INACT       0x26
+#define ADXL345_RA_ACT_INACT_CTL    0x27
+#define ADXL345_RA_THRESH_FF        0x28
+#define ADXL345_RA_TIME_FF          0x29
+#define ADXL345_RA_TAP_AXES         0x2A
+#define ADXL345_RA_ACT_TAP_STATUS   0x2B
+#define ADXL345_RA_BW_RATE          0x2C
+#define ADXL345_RA_POWER_CTL        0x2D
+#define ADXL345_RA_INT_ENABLE       0x2E
+#define ADXL345_RA_INT_MAP          0x2F
+#define ADXL345_RA_INT_SOURCE       0x30
+#define ADXL345_RA_DATA_FORMAT      0x31
+#define ADXL345_RA_DATAX0           0x32
+#define ADXL345_RA_DATAX1           0x33
+#define ADXL345_RA_DATAY0           0x34
+#define ADXL345_RA_DATAY1           0x35
+#define ADXL345_RA_DATAZ0           0x36
+#define ADXL345_RA_DATAZ1           0x37
+#define ADXL345_RA_FIFO_CTL         0x38
+#define ADXL345_RA_FIFO_STATUS      0x39
+
+#define ADXL345_AIC_ACT_AC_BIT      7
+#define ADXL345_AIC_ACT_X_BIT       6
+#define ADXL345_AIC_ACT_Y_BIT       5
+#define ADXL345_AIC_ACT_Z_BIT       4
+#define ADXL345_AIC_INACT_AC_BIT    3
+#define ADXL345_AIC_INACT_X_BIT     2
+#define ADXL345_AIC_INACT_Y_BIT     1
+#define ADXL345_AIC_INACT_Z_BIT     0
+
+#define ADXL345_TAPAXIS_SUP_BIT     3
+#define ADXL345_TAPAXIS_X_BIT       2
+#define ADXL345_TAPAXIS_Y_BIT       1
+#define ADXL345_TAPAXIS_Z_BIT       0
+
+#define ADXL345_TAPSTAT_ACTX_BIT    6
+#define ADXL345_TAPSTAT_ACTY_BIT    5
+#define ADXL345_TAPSTAT_ACTZ_BIT    4
+#define ADXL345_TAPSTAT_ASLEEP_BIT  3
+#define ADXL345_TAPSTAT_TAPX_BIT    2
+#define ADXL345_TAPSTAT_TAPY_BIT    1
+#define ADXL345_TAPSTAT_TAPZ_BIT    0
+
+#define ADXL345_BW_LOWPOWER_BIT     4
+#define ADXL345_BW_RATE_BIT         3
+#define ADXL345_BW_RATE_LENGTH      4
+
+#define ADXL345_RATE_3200           0b1111
+#define ADXL345_RATE_1600           0b1110
+#define ADXL345_RATE_800            0b1101
+#define ADXL345_RATE_400            0b1100
+#define ADXL345_RATE_200            0b1011
+#define ADXL345_RATE_100            0b1010
+#define ADXL345_RATE_50             0b1001
+#define ADXL345_RATE_25             0b1000
+#define ADXL345_RATE_12P5           0b0111
+#define ADXL345_RATE_6P25           0b0110
+#define ADXL345_RATE_3P13           0b0101
+#define ADXL345_RATE_1P56           0b0100
+#define ADXL345_RATE_0P78           0b0011
+#define ADXL345_RATE_0P39           0b0010
+#define ADXL345_RATE_0P20           0b0001
+#define ADXL345_RATE_0P10           0b0000
+
+#define ADXL345_PCTL_LINK_BIT       5
+#define ADXL345_PCTL_AUTOSLEEP_BIT  4
+#define ADXL345_PCTL_MEASURE_BIT    3
+#define ADXL345_PCTL_SLEEP_BIT      2
+#define ADXL345_PCTL_WAKEUP_BIT     1
+#define ADXL345_PCTL_WAKEUP_LENGTH  2
+
+#define ADXL345_WAKEUP_8HZ          0b00
+#define ADXL345_WAKEUP_4HZ          0b01
+#define ADXL345_WAKEUP_2HZ          0b10
+#define ADXL345_WAKEUP_1HZ          0b11
+
+#define ADXL345_INT_DATA_READY_BIT  7
+#define ADXL345_INT_SINGLE_TAP_BIT  6
+#define ADXL345_INT_DOUBLE_TAP_BIT  5
+#define ADXL345_INT_ACTIVITY_BIT    4
+#define ADXL345_INT_INACTIVITY_BIT  3
+#define ADXL345_INT_FREE_FALL_BIT   2
+#define ADXL345_INT_WATERMARK_BIT   1
+#define ADXL345_INT_OVERRUN_BIT     0
+
+#define ADXL345_FORMAT_SELFTEST_BIT 7
+#define ADXL345_FORMAT_SPIMODE_BIT  6
+#define ADXL345_FORMAT_INTMODE_BIT  5
+#define ADXL345_FORMAT_FULL_RES_BIT 3
+#define ADXL345_FORMAT_JUSTIFY_BIT  2
+#define ADXL345_FORMAT_RANGE_BIT    1
+#define ADXL345_FORMAT_RANGE_LENGTH 2
+
+#define ADXL345_RANGE_2G            0b00
+#define ADXL345_RANGE_4G            0b01
+#define ADXL345_RANGE_8G            0b10
+#define ADXL345_RANGE_16G           0b11
+
+#define ADXL345_FIFO_MODE_BIT       7
+#define ADXL345_FIFO_MODE_LENGTH    2
+#define ADXL345_FIFO_TRIGGER_BIT    5
+#define ADXL345_FIFO_SAMPLES_BIT    4
+#define ADXL345_FIFO_SAMPLES_LENGTH 5
+
+#define ADXL345_FIFO_MODE_BYPASS    0b00
+#define ADXL345_FIFO_MODE_FIFO      0b01
+#define ADXL345_FIFO_MODE_STREAM    0b10
+#define ADXL345_FIFO_MODE_TRIGGER   0b11
+
+#define ADXL345_FIFOSTAT_TRIGGER_BIT        7
+#define ADXL345_FIFOSTAT_LENGTH_BIT         5
+#define ADXL345_FIFOSTAT_LENGTH_LENGTH      6
+
+class ADXL345 {
+    public:
+        ADXL345();
+        ADXL345(uint8_t address);
+
+        void initialize();
+        bool testConnection();
+
+        // DEVID register
+        uint8_t getDeviceID();
+        
+        // THRESH_TAP register
+        uint8_t getTapThreshold();
+        void setTapThreshold(uint8_t threshold);
+
+        // OFS* registers
+        void getOffset(int8_t* x, int8_t* y, int8_t* z);
+        void setOffset(int8_t x, int8_t y, int8_t z);
+        int8_t getOffsetX();
+        void setOffsetX(int8_t x);
+        int8_t getOffsetY();
+        void setOffsetY(int8_t y);
+        int8_t getOffsetZ();
+        void setOffsetZ(int8_t z);
+        
+        // DUR register
+        uint8_t getTapDuration();
+        void setTapDuration(uint8_t duration);
+        
+        // LATENT register
+        uint8_t getDoubleTapLatency();
+        void setDoubleTapLatency(uint8_t latency);
+        
+        // WINDOW register
+        uint8_t getDoubleTapWindow();
+        void setDoubleTapWindow(uint8_t window);
+        
+        // THRESH_ACT register
+        uint8_t getActivityThreshold();
+        void setActivityThreshold(uint8_t threshold);
+        
+        // THRESH_INACT register
+        uint8_t getInactivityThreshold();
+        void setInactivityThreshold(uint8_t threshold);
+
+        // TIME_INACT register
+        uint8_t getInactivityTime();
+        void setInactivityTime(uint8_t time);
+        
+        // ACT_INACT_CTL register
+        bool getActivityAC();
+        void setActivityAC(bool enabled);
+        bool getActivityXEnabled();
+        void setActivityXEnabled(bool enabled);
+        bool getActivityYEnabled();
+        void setActivityYEnabled(bool enabled);
+        bool getActivityZEnabled();
+        void setActivityZEnabled(bool enabled);
+        bool getInactivityAC();
+        void setInactivityAC(bool enabled);
+        bool getInactivityXEnabled();
+        void setInactivityXEnabled(bool enabled);
+        bool getInactivityYEnabled();
+        void setInactivityYEnabled(bool enabled);
+        bool getInactivityZEnabled();
+        void setInactivityZEnabled(bool enabled);
+        
+        // THRESH_FF register
+        uint8_t getFreefallThreshold();
+        void setFreefallThreshold(uint8_t threshold);
+        
+        // TIME_FF register
+        uint8_t getFreefallTime();
+        void setFreefallTime(uint8_t time);
+        
+        // TAP_AXES register
+        bool getTapAxisSuppress();
+        void setTapAxisSuppress(bool enabled);
+        bool getTapAxisXEnabled();
+        void setTapAxisXEnabled(bool enabled);
+        bool getTapAxisYEnabled();
+        void setTapAxisYEnabled(bool enabled);
+        bool getTapAxisZEnabled();
+        void setTapAxisZEnabled(bool enabled);
+        
+        // ACT_TAP_STATUS register
+        bool getActivitySourceX();
+        bool getActivitySourceY();
+        bool getActivitySourceZ();
+        bool getAsleep();
+        bool getTapSourceX();
+        bool getTapSourceY();
+        bool getTapSourceZ();
+        
+        // BW_RATE register
+        bool getLowPowerEnabled();
+        void setLowPowerEnabled(bool enabled);
+        uint8_t getRate();
+        void setRate(uint8_t rate);
+
+        // POWER_CTL register
+        bool getLinkEnabled();
+        void setLinkEnabled(bool enabled);
+        bool getAutoSleepEnabled();
+        void setAutoSleepEnabled(bool enabled);
+        bool getMeasureEnabled();
+        void setMeasureEnabled(bool enabled);
+        bool getSleepEnabled();
+        void setSleepEnabled(bool enabled);
+        uint8_t getWakeupFrequency();
+        void setWakeupFrequency(uint8_t frequency);
+        
+        // INT_ENABLE register
+        bool getIntDataReadyEnabled();
+        void setIntDataReadyEnabled(bool enabled);
+        bool getIntSingleTapEnabled();
+        void setIntSingleTapEnabled(bool enabled);
+        bool getIntDoubleTapEnabled();
+        void setIntDoubleTapEnabled(bool enabled);
+        bool getIntActivityEnabled();
+        void setIntActivityEnabled(bool enabled);
+        bool getIntInactivityEnabled();
+        void setIntInactivityEnabled(bool enabled);
+        bool getIntFreefallEnabled();
+        void setIntFreefallEnabled(bool enabled);
+        bool getIntWatermarkEnabled();
+        void setIntWatermarkEnabled(bool enabled);
+        bool getIntOverrunEnabled();
+        void setIntOverrunEnabled(bool enabled);
+        
+        // INT_MAP register
+        uint8_t getIntDataReadyPin();
+        void setIntDataReadyPin(uint8_t pin);
+        uint8_t getIntSingleTapPin();
+        void setIntSingleTapPin(uint8_t pin);
+        uint8_t getIntDoubleTapPin();
+        void setIntDoubleTapPin(uint8_t pin);
+        uint8_t getIntActivityPin();
+        void setIntActivityPin(uint8_t pin);
+        uint8_t getIntInactivityPin();
+        void setIntInactivityPin(uint8_t pin);
+        uint8_t getIntFreefallPin();
+        void setIntFreefallPin(uint8_t pin);
+        uint8_t getIntWatermarkPin();
+        void setIntWatermarkPin(uint8_t pin);
+        uint8_t getIntOverrunPin();
+        void setIntOverrunPin(uint8_t pin);
+
+        // INT_SOURCE register
+        uint8_t getIntDataReadySource();
+        uint8_t getIntSingleTapSource();
+        uint8_t getIntDoubleTapSource();
+        uint8_t getIntActivitySource();
+        uint8_t getIntInactivitySource();
+        uint8_t getIntFreefallSource();
+        uint8_t getIntWatermarkSource();
+        uint8_t getIntOverrunSource();
+        
+        // DATA_FORMAT register
+        uint8_t getSelfTestEnabled();
+        void setSelfTestEnabled(uint8_t enabled);
+        uint8_t getSPIMode();
+        void setSPIMode(uint8_t mode);
+        uint8_t getInterruptMode();
+        void setInterruptMode(uint8_t mode);
+        uint8_t getFullResolution();
+        void setFullResolution(uint8_t resolution);
+        uint8_t getDataJustification();
+        void setDataJustification(uint8_t justification);
+        uint8_t getRange();
+        void setRange(uint8_t range);
+
+        // DATA* registers
+        void getAcceleration(int16_t* x, int16_t* y, int16_t* z);
+        int16_t getAccelerationX();
+        int16_t getAccelerationY();
+        int16_t getAccelerationZ();
+
+        // FIFO_CTL register
+        uint8_t getFIFOMode();
+        void setFIFOMode(uint8_t mode);
+        uint8_t getFIFOTriggerInterruptPin();
+        void setFIFOTriggerInterruptPin(uint8_t interrupt);
+        uint8_t getFIFOSamples();
+        void setFIFOSamples(uint8_t size);
+        
+        // FIFO_STATUS register
+        bool getFIFOTriggerOccurred();
+        uint8_t getFIFOLength();
+
+    private:
+        uint8_t devAddr;
+        uint8_t buffer[6];
+};
+
+#endif /* _ADXL345_H_ */

firmware/lib/imu/HMC5883L.cpp

@@ -0,0 +1,365 @@
+// I2Cdev library collection - HMC5883L I2C device class
+// Based on Honeywell HMC5883L datasheet, 10/2010 (Form #900405 Rev B)
+// 6/12/2012 by Jeff Rowberg <jeff@rowberg.net>
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//     2012-06-12 - fixed swapped Y/Z axes
+//     2011-08-22 - small Doxygen comment fixes
+//     2011-07-31 - initial release
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2012 Jeff Rowberg
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+===============================================
+*/
+
+#include "HMC5883L.h"
+
+/** Default constructor, uses default I2C address.
+ * @see HMC5883L_DEFAULT_ADDRESS
+ */
+HMC5883L::HMC5883L() {
+    devAddr = HMC5883L_DEFAULT_ADDRESS;
+}
+
+/** Specific address constructor.
+ * @param address I2C address
+ * @see HMC5883L_DEFAULT_ADDRESS
+ * @see HMC5883L_ADDRESS
+ */
+HMC5883L::HMC5883L(uint8_t address) {
+    devAddr = address;
+}
+
+/** Power on and prepare for general usage.
+ * This will prepare the magnetometer with default settings, ready for single-
+ * use mode (very low power requirements). Default settings include 8-sample
+ * averaging, 15 Hz data output rate, normal measurement bias, a,d 1090 gain (in
+ * terms of LSB/Gauss). Be sure to adjust any settings you need specifically
+ * after initialization, especially the gain settings if you happen to be seeing
+ * a lot of -4096 values (see the datasheet for mor information).
+ */
+void HMC5883L::initialize() {
+    // write CONFIG_A register
+    I2Cdev::writeByte(devAddr, HMC5883L_RA_CONFIG_A,
+        (HMC5883L_AVERAGING_8 << (HMC5883L_CRA_AVERAGE_BIT - HMC5883L_CRA_AVERAGE_LENGTH + 1)) |
+        (HMC5883L_RATE_15     << (HMC5883L_CRA_RATE_BIT - HMC5883L_CRA_RATE_LENGTH + 1)) |
+        (HMC5883L_BIAS_NORMAL << (HMC5883L_CRA_BIAS_BIT - HMC5883L_CRA_BIAS_LENGTH + 1)));
+
+    // write CONFIG_B register
+    setGain(HMC5883L_GAIN_1090);
+    
+    // write MODE register
+    setMode(HMC5883L_MODE_SINGLE);
+}
+
+/** Verify the I2C connection.
+ * Make sure the device is connected and responds as expected.
+ * @return True if connection is valid, false otherwise
+ */
+bool HMC5883L::testConnection() {
+    if (I2Cdev::readBytes(devAddr, HMC5883L_RA_ID_A, 3, buffer) == 3) {
+        return (buffer[0] == 'H' && buffer[1] == '4' && buffer[2] == '3');
+    }
+    return false;
+}
+
+// CONFIG_A register
+
+/** Get number of samples averaged per measurement.
+ * @return Current samples averaged per measurement (0-3 for 1/2/4/8 respectively)
+ * @see HMC5883L_AVERAGING_8
+ * @see HMC5883L_RA_CONFIG_A
+ * @see HMC5883L_CRA_AVERAGE_BIT
+ * @see HMC5883L_CRA_AVERAGE_LENGTH
+ */
+uint8_t HMC5883L::getSampleAveraging() {
+    I2Cdev::readBits(devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_AVERAGE_BIT, HMC5883L_CRA_AVERAGE_LENGTH, buffer);
+    return buffer[0];
+}
+/** Set number of samples averaged per measurement.
+ * @param averaging New samples averaged per measurement setting(0-3 for 1/2/4/8 respectively)
+ * @see HMC5883L_RA_CONFIG_A
+ * @see HMC5883L_CRA_AVERAGE_BIT
+ * @see HMC5883L_CRA_AVERAGE_LENGTH
+ */
+void HMC5883L::setSampleAveraging(uint8_t averaging) {
+    I2Cdev::writeBits(devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_AVERAGE_BIT, HMC5883L_CRA_AVERAGE_LENGTH, averaging);
+}
+/** Get data output rate value.
+ * The Table below shows all selectable output rates in continuous measurement
+ * mode. All three channels shall be measured within a given output rate. Other
+ * output rates with maximum rate of 160 Hz can be achieved by monitoring DRDY
+ * interrupt pin in single measurement mode.
+ *
+ * Value | Typical Data Output Rate (Hz)
+ * ------+------------------------------
+ * 0     | 0.75
+ * 1     | 1.5
+ * 2     | 3
+ * 3     | 7.5
+ * 4     | 15 (Default)
+ * 5     | 30
+ * 6     | 75
+ * 7     | Not used
+ *
+ * @return Current rate of data output to registers
+ * @see HMC5883L_RATE_15
+ * @see HMC5883L_RA_CONFIG_A
+ * @see HMC5883L_CRA_RATE_BIT
+ * @see HMC5883L_CRA_RATE_LENGTH
+ */
+uint8_t HMC5883L::getDataRate() {
+    I2Cdev::readBits(devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_RATE_BIT, HMC5883L_CRA_RATE_LENGTH, buffer);
+    return buffer[0];
+}
+/** Set data output rate value.
+ * @param rate Rate of data output to registers
+ * @see getDataRate()
+ * @see HMC5883L_RATE_15
+ * @see HMC5883L_RA_CONFIG_A
+ * @see HMC5883L_CRA_RATE_BIT
+ * @see HMC5883L_CRA_RATE_LENGTH
+ */
+void HMC5883L::setDataRate(uint8_t rate) {
+    I2Cdev::writeBits(devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_RATE_BIT, HMC5883L_CRA_RATE_LENGTH, rate);
+}
+/** Get measurement bias value.
+ * @return Current bias value (0-2 for normal/positive/negative respectively)
+ * @see HMC5883L_BIAS_NORMAL
+ * @see HMC5883L_RA_CONFIG_A
+ * @see HMC5883L_CRA_BIAS_BIT
+ * @see HMC5883L_CRA_BIAS_LENGTH
+ */
+uint8_t HMC5883L::getMeasurementBias() {
+    I2Cdev::readBits(devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_BIAS_BIT, HMC5883L_CRA_BIAS_LENGTH, buffer);
+    return buffer[0];
+}
+/** Set measurement bias value.
+ * @param bias New bias value (0-2 for normal/positive/negative respectively)
+ * @see HMC5883L_BIAS_NORMAL
+ * @see HMC5883L_RA_CONFIG_A
+ * @see HMC5883L_CRA_BIAS_BIT
+ * @see HMC5883L_CRA_BIAS_LENGTH
+ */
+void HMC5883L::setMeasurementBias(uint8_t bias) {
+    I2Cdev::writeBits(devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_BIAS_BIT, HMC5883L_CRA_BIAS_LENGTH, bias);
+}
+
+// CONFIG_B register
+
+/** Get magnetic field gain value.
+ * The table below shows nominal gain settings. Use the "Gain" column to convert
+ * counts to Gauss. Choose a lower gain value (higher GN#) when total field
+ * strength causes overflow in one of the data output registers (saturation).
+ * The data output range for all settings is 0xF800-0x07FF (-2048 - 2047).
+ *
+ * Value | Field Range | Gain (LSB/Gauss)
+ * ------+-------------+-----------------
+ * 0     | +/- 0.88 Ga | 1370
+ * 1     | +/- 1.3 Ga  | 1090 (Default)
+ * 2     | +/- 1.9 Ga  | 820
+ * 3     | +/- 2.5 Ga  | 660
+ * 4     | +/- 4.0 Ga  | 440
+ * 5     | +/- 4.7 Ga  | 390
+ * 6     | +/- 5.6 Ga  | 330
+ * 7     | +/- 8.1 Ga  | 230
+ *
+ * @return Current magnetic field gain value
+ * @see HMC5883L_GAIN_1090
+ * @see HMC5883L_RA_CONFIG_B
+ * @see HMC5883L_CRB_GAIN_BIT
+ * @see HMC5883L_CRB_GAIN_LENGTH
+ */
+uint8_t HMC5883L::getGain() {
+    I2Cdev::readBits(devAddr, HMC5883L_RA_CONFIG_B, HMC5883L_CRB_GAIN_BIT, HMC5883L_CRB_GAIN_LENGTH, buffer);
+    return buffer[0];
+}
+/** Set magnetic field gain value.
+ * @param gain New magnetic field gain value
+ * @see getGain()
+ * @see HMC5883L_RA_CONFIG_B
+ * @see HMC5883L_CRB_GAIN_BIT
+ * @see HMC5883L_CRB_GAIN_LENGTH
+ */
+void HMC5883L::setGain(uint8_t gain) {
+    // use this method to guarantee that bits 4-0 are set to zero, which is a
+    // requirement specified in the datasheet; it's actually more efficient than
+    // using the I2Cdev.writeBits method
+    I2Cdev::writeByte(devAddr, HMC5883L_RA_CONFIG_B, gain << (HMC5883L_CRB_GAIN_BIT - HMC5883L_CRB_GAIN_LENGTH + 1));
+}
+
+// MODE register
+
+/** Get measurement mode.
+ * In continuous-measurement mode, the device continuously performs measurements
+ * and places the result in the data register. RDY goes high when new data is
+ * placed in all three registers. After a power-on or a write to the mode or
+ * configuration register, the first measurement set is available from all three
+ * data output registers after a period of 2/fDO and subsequent measurements are
+ * available at a frequency of fDO, where fDO is the frequency of data output.
+ *
+ * When single-measurement mode (default) is selected, device performs a single
+ * measurement, sets RDY high and returned to idle mode. Mode register returns
+ * to idle mode bit values. The measurement remains in the data output register
+ * and RDY remains high until the data output register is read or another
+ * measurement is performed.
+ *
+ * @return Current measurement mode
+ * @see HMC5883L_MODE_CONTINUOUS
+ * @see HMC5883L_MODE_SINGLE
+ * @see HMC5883L_MODE_IDLE
+ * @see HMC5883L_RA_MODE
+ * @see HMC5883L_MODEREG_BIT
+ * @see HMC5883L_MODEREG_LENGTH
+ */
+uint8_t HMC5883L::getMode() {
+    I2Cdev::readBits(devAddr, HMC5883L_RA_MODE, HMC5883L_MODEREG_BIT, HMC5883L_MODEREG_LENGTH, buffer);
+    return buffer[0];
+}
+/** Set measurement mode.
+ * @param newMode New measurement mode
+ * @see getMode()
+ * @see HMC5883L_MODE_CONTINUOUS
+ * @see HMC5883L_MODE_SINGLE
+ * @see HMC5883L_MODE_IDLE
+ * @see HMC5883L_RA_MODE
+ * @see HMC5883L_MODEREG_BIT
+ * @see HMC5883L_MODEREG_LENGTH
+ */
+void HMC5883L::setMode(uint8_t newMode) {
+    // use this method to guarantee that bits 7-2 are set to zero, which is a
+    // requirement specified in the datasheet; it's actually more efficient than
+    // using the I2Cdev.writeBits method
+    I2Cdev::writeByte(devAddr, HMC5883L_RA_MODE, newMode << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
+    mode = newMode; // track to tell if we have to clear bit 7 after a read
+}
+
+// DATA* registers
+
+/** Get 3-axis heading measurements.
+ * In the event the ADC reading overflows or underflows for the given channel,
+ * or if there is a math overflow during the bias measurement, this data
+ * register will contain the value -4096. This register value will clear when
+ * after the next valid measurement is made. Note that this method automatically
+ * clears the appropriate bit in the MODE register if Single mode is active.
+ * @param x 16-bit signed integer container for X-axis heading
+ * @param y 16-bit signed integer container for Y-axis heading
+ * @param z 16-bit signed integer container for Z-axis heading
+ * @see HMC5883L_RA_DATAX_H
+ */
+void HMC5883L::getHeading(int16_t *x, int16_t *y, int16_t *z) {
+    I2Cdev::readBytes(devAddr, HMC5883L_RA_DATAX_H, 6, buffer);
+    if (mode == HMC5883L_MODE_SINGLE) I2Cdev::writeByte(devAddr, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
+    *x = (((int16_t)buffer[0]) << 8) | buffer[1];
+    *y = (((int16_t)buffer[4]) << 8) | buffer[5];
+    *z = (((int16_t)buffer[2]) << 8) | buffer[3];
+}
+/** Get X-axis heading measurement.
+ * @return 16-bit signed integer with X-axis heading
+ * @see HMC5883L_RA_DATAX_H
+ */
+int16_t HMC5883L::getHeadingX() {
+    // each axis read requires that ALL axis registers be read, even if only
+    // one is used; this was not done ineffiently in the code by accident
+    I2Cdev::readBytes(devAddr, HMC5883L_RA_DATAX_H, 6, buffer);
+    if (mode == HMC5883L_MODE_SINGLE) I2Cdev::writeByte(devAddr, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
+    return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+/** Get Y-axis heading measurement.
+ * @return 16-bit signed integer with Y-axis heading
+ * @see HMC5883L_RA_DATAY_H
+ */
+int16_t HMC5883L::getHeadingY() {
+    // each axis read requires that ALL axis registers be read, even if only
+    // one is used; this was not done ineffiently in the code by accident
+    I2Cdev::readBytes(devAddr, HMC5883L_RA_DATAX_H, 6, buffer);
+    if (mode == HMC5883L_MODE_SINGLE) I2Cdev::writeByte(devAddr, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
+    return (((int16_t)buffer[4]) << 8) | buffer[5];
+}
+/** Get Z-axis heading measurement.
+ * @return 16-bit signed integer with Z-axis heading
+ * @see HMC5883L_RA_DATAZ_H
+ */
+int16_t HMC5883L::getHeadingZ() {
+    // each axis read requires that ALL axis registers be read, even if only
+    // one is used; this was not done ineffiently in the code by accident
+    I2Cdev::readBytes(devAddr, HMC5883L_RA_DATAX_H, 6, buffer);
+    if (mode == HMC5883L_MODE_SINGLE) I2Cdev::writeByte(devAddr, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
+    return (((int16_t)buffer[2]) << 8) | buffer[3];
+}
+
+// STATUS register
+
+/** Get data output register lock status.
+ * This bit is set when this some but not all for of the six data output
+ * registers have been read. When this bit is set, the six data output registers
+ * are locked and any new data will not be placed in these register until one of
+ * three conditions are met: one, all six bytes have been read or the mode
+ * changed, two, the mode is changed, or three, the measurement configuration is
+ * changed.
+ * @return Data output register lock status
+ * @see HMC5883L_RA_STATUS
+ * @see HMC5883L_STATUS_LOCK_BIT
+ */
+bool HMC5883L::getLockStatus() {
+    I2Cdev::readBit(devAddr, HMC5883L_RA_STATUS, HMC5883L_STATUS_LOCK_BIT, buffer);
+    return buffer[0];
+}
+/** Get data ready status.
+ * This bit is set when data is written to all six data registers, and cleared
+ * when the device initiates a write to the data output registers and after one
+ * or more of the data output registers are written to. When RDY bit is clear it
+ * shall remain cleared for 250 us. DRDY pin can be used as an alternative to
+ * the status register for monitoring the device for measurement data.
+ * @return Data ready status
+ * @see HMC5883L_RA_STATUS
+ * @see HMC5883L_STATUS_READY_BIT
+ */
+bool HMC5883L::getReadyStatus() {
+    I2Cdev::readBit(devAddr, HMC5883L_RA_STATUS, HMC5883L_STATUS_READY_BIT, buffer);
+    return buffer[0];
+}
+
+// ID_* registers
+
+/** Get identification byte A
+ * @return ID_A byte (should be 01001000, ASCII value 'H')
+ */
+uint8_t HMC5883L::getIDA() {
+    I2Cdev::readByte(devAddr, HMC5883L_RA_ID_A, buffer);
+    return buffer[0];
+}
+/** Get identification byte B
+ * @return ID_A byte (should be 00110100, ASCII value '4')
+ */
+uint8_t HMC5883L::getIDB() {
+    I2Cdev::readByte(devAddr, HMC5883L_RA_ID_B, buffer);
+    return buffer[0];
+}
+/** Get identification byte C
+ * @return ID_A byte (should be 00110011, ASCII value '3')
+ */
+uint8_t HMC5883L::getIDC() {
+    I2Cdev::readByte(devAddr, HMC5883L_RA_ID_C, buffer);
+    return buffer[0];
+}

firmware/lib/imu/HMC5883L.h

@@ -0,0 +1,148 @@
+// I2Cdev library collection - HMC5883L I2C device class header file
+// Based on Honeywell HMC5883L datasheet, 10/2010 (Form #900405 Rev B)
+// 6/12/2012 by Jeff Rowberg <jeff@rowberg.net>
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//     2012-06-12 - fixed swapped Y/Z axes
+//     2011-08-22 - small Doxygen comment fixes
+//     2011-07-31 - initial release
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2011 Jeff Rowberg
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+===============================================
+*/
+
+#ifndef _HMC5883L_H_
+#define _HMC5883L_H_
+
+#include "I2Cdev.h"
+
+#define HMC5883L_ADDRESS            0x1E // this device only has one address
+#define HMC5883L_DEFAULT_ADDRESS    0x1E
+
+#define HMC5883L_RA_CONFIG_A        0x00
+#define HMC5883L_RA_CONFIG_B        0x01
+#define HMC5883L_RA_MODE            0x02
+#define HMC5883L_RA_DATAX_H         0x03
+#define HMC5883L_RA_DATAX_L         0x04
+#define HMC5883L_RA_DATAZ_H         0x05
+#define HMC5883L_RA_DATAZ_L         0x06
+#define HMC5883L_RA_DATAY_H         0x07
+#define HMC5883L_RA_DATAY_L         0x08
+#define HMC5883L_RA_STATUS          0x09
+#define HMC5883L_RA_ID_A            0x0A
+#define HMC5883L_RA_ID_B            0x0B
+#define HMC5883L_RA_ID_C            0x0C
+
+#define HMC5883L_CRA_AVERAGE_BIT    6
+#define HMC5883L_CRA_AVERAGE_LENGTH 2
+#define HMC5883L_CRA_RATE_BIT       4
+#define HMC5883L_CRA_RATE_LENGTH    3
+#define HMC5883L_CRA_BIAS_BIT       1
+#define HMC5883L_CRA_BIAS_LENGTH    2
+
+#define HMC5883L_AVERAGING_1        0x00
+#define HMC5883L_AVERAGING_2        0x01
+#define HMC5883L_AVERAGING_4        0x02
+#define HMC5883L_AVERAGING_8        0x03
+
+#define HMC5883L_RATE_0P75          0x00
+#define HMC5883L_RATE_1P5           0x01
+#define HMC5883L_RATE_3             0x02
+#define HMC5883L_RATE_7P5           0x03
+#define HMC5883L_RATE_15            0x04
+#define HMC5883L_RATE_30            0x05
+#define HMC5883L_RATE_75            0x06
+
+#define HMC5883L_BIAS_NORMAL        0x00
+#define HMC5883L_BIAS_POSITIVE      0x01
+#define HMC5883L_BIAS_NEGATIVE      0x02
+
+#define HMC5883L_CRB_GAIN_BIT       7
+#define HMC5883L_CRB_GAIN_LENGTH    3
+
+#define HMC5883L_GAIN_1370          0x00
+#define HMC5883L_GAIN_1090          0x01
+#define HMC5883L_GAIN_820           0x02
+#define HMC5883L_GAIN_660           0x03
+#define HMC5883L_GAIN_440           0x04
+#define HMC5883L_GAIN_390           0x05
+#define HMC5883L_GAIN_330           0x06
+#define HMC5883L_GAIN_220           0x07
+
+#define HMC5883L_MODEREG_BIT        1
+#define HMC5883L_MODEREG_LENGTH     2
+
+#define HMC5883L_MODE_CONTINUOUS    0x00
+#define HMC5883L_MODE_SINGLE        0x01
+#define HMC5883L_MODE_IDLE          0x02
+
+#define HMC5883L_STATUS_LOCK_BIT    1
+#define HMC5883L_STATUS_READY_BIT   0
+
+class HMC5883L {
+    public:
+        HMC5883L();
+        HMC5883L(uint8_t address);
+        
+        void initialize();
+        bool testConnection();
+
+        // CONFIG_A register
+        uint8_t getSampleAveraging();
+        void setSampleAveraging(uint8_t averaging);
+        uint8_t getDataRate();
+        void setDataRate(uint8_t rate);
+        uint8_t getMeasurementBias();
+        void setMeasurementBias(uint8_t bias);
+
+        // CONFIG_B register
+        uint8_t getGain();
+        void setGain(uint8_t gain);
+
+        // MODE register
+        uint8_t getMode();
+        void setMode(uint8_t mode);
+
+        // DATA* registers
+        void getHeading(int16_t *x, int16_t *y, int16_t *z);
+        int16_t getHeadingX();
+        int16_t getHeadingY();
+        int16_t getHeadingZ();
+
+        // STATUS register
+        bool getLockStatus();
+        bool getReadyStatus();
+
+        // ID_* registers
+        uint8_t getIDA();
+        uint8_t getIDB();
+        uint8_t getIDC();
+
+    private:
+        uint8_t devAddr;
+        uint8_t buffer[6];
+        uint8_t mode;
+};
+
+#endif /* _HMC5883L_H_ */

firmware/lib/imu/I2Cdev.h

@@ -0,0 +1,280 @@
+// I2Cdev library collection - Main I2C device class header file
+// Abstracts bit and byte I2C R/W functions into a convenient class
+// 2013-06-05 by Jeff Rowberg <jeff@rowberg.net>
+//
+// Changelog:
+//      2015-10-30 - simondlevy : support i2c_t3 for Teensy3.1
+//      2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications
+//      2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan)
+//      2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire
+//                 - add compiler warnings when using outdated or IDE or limited I2Cdev implementation
+//      2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums)
+//      2011-10-03 - added automatic Arduino version detection for ease of use
+//      2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications
+//      2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x)
+//      2011-08-03 - added optional timeout parameter to read* methods to easily change from default
+//      2011-08-02 - added support for 16-bit registers
+//                 - fixed incorrect Doxygen comments on some methods
+//                 - added timeout value for read operations (thanks mem @ Arduino forums)
+//      2011-07-30 - changed read/write function structures to return success or byte counts
+//                 - made all methods static for multi-device memory savings
+//      2011-07-28 - initial release
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2013 Jeff Rowberg
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+===============================================
+*/
+
+#ifndef _I2CDEV_H_
+#define _I2CDEV_H_
+
+// -----------------------------------------------------------------------------
+// I2C interface implementation setting
+// -----------------------------------------------------------------------------
+#ifndef I2CDEV_IMPLEMENTATION
+#define I2CDEV_IMPLEMENTATION       I2CDEV_ARDUINO_WIRE
+//#define I2CDEV_IMPLEMENTATION       I2CDEV_BUILTIN_FASTWIRE
+#endif // I2CDEV_IMPLEMENTATION
+
+// comment this out if you are using a non-optimal IDE/implementation setting
+// but want the compiler to shut up about it
+#define I2CDEV_IMPLEMENTATION_WARNINGS
+
+// -----------------------------------------------------------------------------
+// I2C interface implementation options
+// -----------------------------------------------------------------------------
+#define I2CDEV_ARDUINO_WIRE         1 // Wire object from Arduino
+#define I2CDEV_BUILTIN_NBWIRE       2 // Tweaked Wire object from Gene Knight's NBWire project
+                                      // ^^^ NBWire implementation is still buggy w/some interrupts!
+#define I2CDEV_BUILTIN_FASTWIRE     3 // FastWire object from Francesco Ferrara's project
+#define I2CDEV_I2CMASTER_LIBRARY    4 // I2C object from DSSCircuits I2C-Master Library at https://github.com/DSSCircuits/I2C-Master-Library
+
+// -----------------------------------------------------------------------------
+// Arduino-style "Serial.print" debug constant (uncomment to enable)
+// -----------------------------------------------------------------------------
+//#define I2CDEV_SERIAL_DEBUG
+
+#ifdef ARDUINO
+    #if ARDUINO < 100
+        #include "WProgram.h"
+    #else
+        #include "Arduino.h"
+    #endif
+    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
+        #include <Wire.h>
+    #endif
+    #if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY
+        #include <I2C.h>
+    #endif
+#endif
+
+#ifdef SPARK
+    #include <spark_wiring_i2c.h>
+    #define ARDUINO 101
+#endif
+
+#define BUFFER_LENGTH   32 // added 07.07.2023
+
+
+// 1000ms default read timeout (modify with "I2Cdev::readTimeout = [ms];")
+#define I2CDEV_DEFAULT_READ_TIMEOUT     1000
+
+class I2Cdev {
+    public:
+        I2Cdev();
+
+        static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
+
+        static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data);
+        static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data);
+        static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data);
+        static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data);
+        static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);
+        static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data);
+        static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
+        static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
+
+        static uint16_t readTimeout;
+};
+
+#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
+    //////////////////////
+    // FastWire 0.24
+    // This is a library to help faster programs to read I2C devices.
+    // Copyright(C) 2012
+    // Francesco Ferrara
+    //////////////////////
+
+    /* Master */
+    #define TW_START                0x08
+    #define TW_REP_START            0x10
+
+    /* Master Transmitter */
+    #define TW_MT_SLA_ACK           0x18
+    #define TW_MT_SLA_NACK          0x20
+    #define TW_MT_DATA_ACK          0x28
+    #define TW_MT_DATA_NACK         0x30
+    #define TW_MT_ARB_LOST          0x38
+
+    /* Master Receiver */
+    #define TW_MR_ARB_LOST          0x38
+    #define TW_MR_SLA_ACK           0x40
+    #define TW_MR_SLA_NACK          0x48
+    #define TW_MR_DATA_ACK          0x50
+    #define TW_MR_DATA_NACK         0x58
+
+    #define TW_OK                   0
+    #define TW_ERROR                1
+
+    class Fastwire {
+        private:
+            static boolean waitInt();
+
+        public:
+            static void setup(int khz, boolean pullup);
+            static byte beginTransmission(byte device);
+            static byte write(byte value);
+            static byte writeBuf(byte device, byte address, byte *data, byte num);
+            static byte readBuf(byte device, byte address, byte *data, byte num);
+            static void reset();
+            static byte stop();
+    };
+#endif
+
+#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE
+    // NBWire implementation based heavily on code by Gene Knight <Gene@Telobot.com>
+    // Originally posted on the Arduino forum at http://arduino.cc/forum/index.php/topic,70705.0.html
+    // Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html
+
+    #define NBWIRE_BUFFER_LENGTH 32
+
+    class TwoWire {
+        private:
+            static uint8_t rxBuffer[];
+            static uint8_t rxBufferIndex;
+            static uint8_t rxBufferLength;
+
+            static uint8_t txAddress;
+            static uint8_t txBuffer[];
+            static uint8_t txBufferIndex;
+            static uint8_t txBufferLength;
+
+            // static uint8_t transmitting;
+            static void (*user_onRequest)(void);
+            static void (*user_onReceive)(int);
+            static void onRequestService(void);
+            static void onReceiveService(uint8_t*, int);
+
+        public:
+            TwoWire();
+            void begin();
+            void begin(uint8_t);
+            void begin(int);
+            void beginTransmission(uint8_t);
+            //void beginTransmission(int);
+            uint8_t endTransmission(uint16_t timeout=0);
+            void nbendTransmission(void (*function)(int)) ;
+            uint8_t requestFrom(uint8_t, int, uint16_t timeout=0);
+            //uint8_t requestFrom(int, int);
+            void nbrequestFrom(uint8_t, int, void (*function)(int));
+            void send(uint8_t);
+            void send(uint8_t*, uint8_t);
+            //void send(int);
+            void send(char*);
+            uint8_t available(void);
+            uint8_t receive(void);
+            void onReceive(void (*)(int));
+            void onRequest(void (*)(void));
+    };
+
+    #define TWI_READY   0
+    #define TWI_MRX     1
+    #define TWI_MTX     2
+    #define TWI_SRX     3
+    #define TWI_STX     4
+
+    #define TW_WRITE    0
+    #define TW_READ     1
+
+    #define TW_MT_SLA_NACK      0x20
+    #define TW_MT_DATA_NACK     0x30
+
+    #define CPU_FREQ            16000000L
+    #define TWI_FREQ            100000L
+    #define TWI_BUFFER_LENGTH   32
+
+    /* TWI Status is in TWSR, in the top 5 bits: TWS7 - TWS3 */
+
+    #define TW_STATUS_MASK              (_BV(TWS7)|_BV(TWS6)|_BV(TWS5)|_BV(TWS4)|_BV(TWS3))
+    #define TW_STATUS                   (TWSR & TW_STATUS_MASK)
+    #define TW_START                    0x08
+    #define TW_REP_START                0x10
+    #define TW_MT_SLA_ACK               0x18
+    #define TW_MT_SLA_NACK              0x20
+    #define TW_MT_DATA_ACK              0x28
+    #define TW_MT_DATA_NACK             0x30
+    #define TW_MT_ARB_LOST              0x38
+    #define TW_MR_ARB_LOST              0x38
+    #define TW_MR_SLA_ACK               0x40
+    #define TW_MR_SLA_NACK              0x48
+    #define TW_MR_DATA_ACK              0x50
+    #define TW_MR_DATA_NACK             0x58
+    #define TW_ST_SLA_ACK               0xA8
+    #define TW_ST_ARB_LOST_SLA_ACK      0xB0
+    #define TW_ST_DATA_ACK              0xB8
+    #define TW_ST_DATA_NACK             0xC0
+    #define TW_ST_LAST_DATA             0xC8
+    #define TW_SR_SLA_ACK               0x60
+    #define TW_SR_ARB_LOST_SLA_ACK      0x68
+    #define TW_SR_GCALL_ACK             0x70
+    #define TW_SR_ARB_LOST_GCALL_ACK    0x78
+    #define TW_SR_DATA_ACK              0x80
+    #define TW_SR_DATA_NACK             0x88
+    #define TW_SR_GCALL_DATA_ACK        0x90
+    #define TW_SR_GCALL_DATA_NACK       0x98
+    #define TW_SR_STOP                  0xA0
+    #define TW_NO_INFO                  0xF8
+    #define TW_BUS_ERROR                0x00
+
+    //#define _MMIO_BYTE(mem_addr) (*(volatile uint8_t *)(mem_addr))
+    //#define _SFR_BYTE(sfr) _MMIO_BYTE(_SFR_ADDR(sfr))
+
+    #ifndef sbi // set bit
+        #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
+    #endif // sbi
+
+    #ifndef cbi // clear bit
+        #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
+    #endif // cbi
+
+    extern TwoWire Wire;
+
+#endif // I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE
+
+#endif /* _I2CDEV_H_ */

firmware/lib/imu/ITG3200.cpp

@@ -0,0 +1,521 @@
+// I2Cdev library collection - ITG3200 I2C device class
+// Based on InvenSense ITG-3200 datasheet rev. 1.4, 3/30/2010 (PS-ITG-3200A-00-01.4)
+// 7/31/2011 by Jeff Rowberg <jeff@rowberg.net>
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//     2011-07-31 - initial release
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2011 Jeff Rowberg
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+===============================================
+*/
+
+#include "ITG3200.h"
+
+/** Default constructor, uses default I2C address.
+ * @see ITG3200_DEFAULT_ADDRESS
+ */
+ITG3200::ITG3200() {
+    devAddr = ITG3200_DEFAULT_ADDRESS;
+}
+
+/** Specific address constructor.
+ * @param address I2C address
+ * @see ITG3200_DEFAULT_ADDRESS
+ * @see ITG3200_ADDRESS_AD0_LOW
+ * @see ITG3200_ADDRESS_AD0_HIGH
+ */
+ITG3200::ITG3200(uint8_t address) {
+    devAddr = address;
+}
+
+/** Power on and prepare for general usage.
+ * This will activate the gyroscope, so be sure to adjust the power settings
+ * after you call this method if you want it to enter standby mode, or another
+ * less demanding mode of operation. This also sets the gyroscope to use the
+ * X-axis gyro for a clock source. Note that it doesn't have any delays in the
+ * routine, which means you might want to add ~50ms to be safe if you happen
+ * to need to read gyro data immediately after initialization. The data will
+ * flow in either case, but the first reports may have higher error offsets.
+ */
+void ITG3200::initialize() {
+    setFullScaleRange(ITG3200_FULLSCALE_2000);
+    setClockSource(ITG3200_CLOCK_PLL_XGYRO);
+}
+
+/** Verify the I2C connection.
+ * Make sure the device is connected and responds as expected.
+ * @return True if connection is valid, false otherwise
+ */
+bool ITG3200::testConnection() {
+    return getDeviceID() == 0b110100;
+}
+
+// WHO_AM_I register
+
+/** Get Device ID.
+ * This register is used to verify the identity of the device (0b110100).
+ * @return Device ID (should be 0x34, 52 dec, 64 oct)
+ * @see ITG3200_RA_WHO_AM_I
+ * @see ITG3200_RA_DEVID_BIT
+ * @see ITG3200_RA_DEVID_LENGTH
+ */
+uint8_t ITG3200::getDeviceID() {
+    I2Cdev::readBits(devAddr, ITG3200_RA_WHO_AM_I, ITG3200_DEVID_BIT, ITG3200_DEVID_LENGTH, buffer);
+    return buffer[0];
+}
+/** Set Device ID.
+ * Write a new ID into the WHO_AM_I register (no idea why this should ever be
+ * necessary though).
+ * @param id New device ID to set.
+ * @see getDeviceID()
+ * @see ITG3200_RA_WHO_AM_I
+ * @see ITG3200_RA_DEVID_BIT
+ * @see ITG3200_RA_DEVID_LENGTH
+ */
+void ITG3200::setDeviceID(uint8_t id) {
+    I2Cdev::writeBits(devAddr, ITG3200_RA_WHO_AM_I, ITG3200_DEVID_BIT, ITG3200_DEVID_LENGTH, id);
+}
+
+// SMPLRT_DIV register
+
+/** Get sample rate.
+ * This register determines the sample rate of the ITG-3200 gyros. The gyros'
+ * outputs are sampled internally at either 1kHz or 8kHz, determined by the
+ * DLPF_CFG setting (see register 22). This sampling is then filtered digitally
+ * and delivered into the sensor registers after the number of cycles determined
+ * by this register. The sample rate is given by the following formula:
+ *
+ * F_sample = F_internal / (divider+1), where F_internal is either 1kHz or 8kHz
+ *
+ * As an example, if the internal sampling is at 1kHz, then setting this
+ * register to 7 would give the following:
+ *
+ * F_sample = 1kHz / (7 + 1) = 125Hz, or 8ms per sample
+ *
+ * @return Current sample rate
+ * @see setDLPFBandwidth()
+ * @see ITG3200_RA_SMPLRT_DIV
+ */
+uint8_t ITG3200::getRate() {
+    I2Cdev::readByte(devAddr, ITG3200_RA_SMPLRT_DIV, buffer);
+    return buffer[0];
+}
+/** Set sample rate.
+ * @param rate New sample rate
+ * @see getRate()
+ * @see setDLPFBandwidth()
+ * @see ITG3200_RA_SMPLRT_DIV
+ */
+void ITG3200::setRate(uint8_t rate) {
+    I2Cdev::writeByte(devAddr, ITG3200_RA_SMPLRT_DIV, rate);
+}
+
+// DLPF_FS register
+
+/** Set full-scale range.
+ * The FS_SEL parameter allows setting the full-scale range of the gyro sensors,
+ * as described in the table below. The power-on-reset value of FS_SEL is 00h.
+ * Set to 03h for proper operation.
+ *
+ * 0 = Reserved
+ * 1 = Reserved
+ * 2 = Reserved
+ * 3 = +/- 2000 degrees/sec
+ *
+ * @return Current full-scale range setting
+ * @see ITG3200_FULLSCALE_2000
+ * @see ITG3200_RA_DLPF_FS
+ * @see ITG3200_DF_FS_SEL_BIT
+ * @see ITG3200_DF_FS_SEL_LENGTH
+ */
+uint8_t ITG3200::getFullScaleRange() {
+    I2Cdev::readBits(devAddr, ITG3200_RA_DLPF_FS, ITG3200_DF_FS_SEL_BIT, ITG3200_DF_FS_SEL_LENGTH, buffer);
+    return buffer[0];
+}
+/** Set full-scale range setting.
+ * @param range New full-scale range value
+ * @see getFullScaleRange()
+ * @see ITG3200_FULLSCALE_2000
+ * @see ITG3200_RA_DLPF_FS
+ * @see ITG3200_DF_FS_SEL_BIT
+ * @see ITG3200_DF_FS_SEL_LENGTH
+ */
+void ITG3200::setFullScaleRange(uint8_t range) {
+    I2Cdev::writeBits(devAddr, ITG3200_RA_DLPF_FS, ITG3200_DF_FS_SEL_BIT, ITG3200_DF_FS_SEL_LENGTH, range);
+}
+/** Get digital low-pass filter bandwidth.
+ * The DLPF_CFG parameter sets the digital low pass filter configuration. It
+ * also determines the internal sampling rate used by the device as shown in
+ * the table below.
+ *
+ * DLPF_CFG | Low-Pass Filter Bandwidth | Internal Sample Rate
+ * ---------+---------------------------+---------------------
+ * 0        | 256Hz                     | 8kHz
+ * 1        | 188Hz                     | 1kHz
+ * 2        | 98Hz                      | 1kHz
+ * 3        | 42Hz                      | 1kHz
+ * 4        | 20Hz                      | 1kHz
+ * 5        | 10Hz                      | 1kHz
+ * 6        | 5Hz                       | 1kHz
+ * 7        | Reserved                  | Reserved
+ *
+ * @return DLFP bandwidth setting
+ * @see ITG3200_RA_DLPF_FS
+ * @see ITG3200_DF_DLPF_CFG_BIT
+ * @see ITG3200_DF_DLPF_CFG_LENGTH
+ */
+uint8_t ITG3200::getDLPFBandwidth() {
+    I2Cdev::readBits(devAddr, ITG3200_RA_DLPF_FS, ITG3200_DF_DLPF_CFG_BIT, ITG3200_DF_DLPF_CFG_LENGTH, buffer);
+    return buffer[0];
+}
+/** Set digital low-pass filter bandwidth.
+ * @param bandwidth New DLFP bandwidth setting
+ * @see getDLPFBandwidth()
+ * @see ITG3200_DLPF_BW_256
+ * @see ITG3200_RA_DLPF_FS
+ * @see ITG3200_DF_DLPF_CFG_BIT
+ * @see ITG3200_DF_DLPF_CFG_LENGTH
+ */
+void ITG3200::setDLPFBandwidth(uint8_t bandwidth) {
+    I2Cdev::writeBits(devAddr, ITG3200_RA_DLPF_FS, ITG3200_DF_DLPF_CFG_BIT, ITG3200_DF_DLPF_CFG_LENGTH, bandwidth);
+}
+
+// INT_CFG register
+
+/** Get interrupt logic level mode.
+ * Will be set 0 for active-high, 1 for active-low.
+ * @return Current interrupt mode (0=active-high, 1=active-low)
+ * @see ITG3200_RA_INT_CFG
+ * @see ITG3200_INTCFG_ACTL_BIT
+ */
+bool ITG3200::getInterruptMode() {
+    I2Cdev::readBit(devAddr, ITG3200_RA_INT_CFG, ITG3200_INTCFG_ACTL_BIT, buffer);
+    return buffer[0];
+}
+/** Set interrupt logic level mode.
+ * @param mode New interrupt mode (0=active-high, 1=active-low)
+ * @see getInterruptMode()
+ * @see ITG3200_RA_INT_CFG
+ * @see ITG3200_INTCFG_ACTL_BIT
+ */
+void ITG3200::setInterruptMode(bool mode) {
+    I2Cdev::writeBit(devAddr, ITG3200_RA_INT_CFG, ITG3200_INTCFG_ACTL_BIT, mode);
+}
+/** Get interrupt drive mode.
+ * Will be set 0 for push-pull, 1 for open-drain.
+ * @return Current interrupt drive mode (0=push-pull, 1=open-drain)
+ * @see ITG3200_RA_INT_CFG
+ * @see ITG3200_INTCFG_OPEN_BIT
+ */
+bool ITG3200::getInterruptDrive() {
+    I2Cdev::readBit(devAddr, ITG3200_RA_INT_CFG, ITG3200_INTCFG_OPEN_BIT, buffer);
+    return buffer[0];
+}
+/** Set interrupt drive mode.
+ * @param drive New interrupt drive mode (0=push-pull, 1=open-drain)
+ * @see getInterruptDrive()
+ * @see ITG3200_RA_INT_CFG
+ * @see ITG3200_INTCFG_OPEN_BIT
+ */
+void ITG3200::setInterruptDrive(bool drive) {
+    I2Cdev::writeBit(devAddr, ITG3200_RA_INT_CFG, ITG3200_INTCFG_OPEN_BIT, drive);
+}
+/** Get interrupt latch mode.
+ * Will be set 0 for 50us-pulse, 1 for latch-until-int-cleared.
+ * @return Current latch mode (0=50us-pulse, 1=latch-until-int-cleared)
+ * @see ITG3200_RA_INT_CFG
+ * @see ITG3200_INTCFG_LATCH_INT_EN_BIT
+ */
+bool ITG3200::getInterruptLatch() {
+    I2Cdev::readBit(devAddr, ITG3200_RA_INT_CFG, ITG3200_INTCFG_LATCH_INT_EN_BIT, buffer);
+    return buffer[0];
+}
+/** Set interrupt latch mode.
+ * @param latch New latch mode (0=50us-pulse, 1=latch-until-int-cleared)
+ * @see getInterruptLatch()
+ * @see ITG3200_RA_INT_CFG
+ * @see ITG3200_INTCFG_LATCH_INT_EN_BIT
+ */
+void ITG3200::setInterruptLatch(bool latch) {
+    I2Cdev::writeBit(devAddr, ITG3200_RA_INT_CFG, ITG3200_INTCFG_LATCH_INT_EN_BIT, latch);
+}
+/** Get interrupt latch clear mode.
+ * Will be set 0 for status-read-only, 1 for any-register-read.
+ * @return Current latch clear mode (0=status-read-only, 1=any-register-read)
+ * @see ITG3200_RA_INT_CFG
+ * @see ITG3200_INTCFG_INT_ANYRD_2CLEAR_BIT
+ */
+bool ITG3200::getInterruptLatchClear() {
+    I2Cdev::readBit(devAddr, ITG3200_RA_INT_CFG, ITG3200_INTCFG_INT_ANYRD_2CLEAR_BIT, buffer);
+    return buffer[0];
+}
+/** Set interrupt latch clear mode.
+ * @param clear New latch clear mode (0=status-read-only, 1=any-register-read)
+ * @see getInterruptLatchClear()
+ * @see ITG3200_RA_INT_CFG
+ * @see ITG3200_INTCFG_INT_ANYRD_2CLEAR_BIT
+ */
+void ITG3200::setInterruptLatchClear(bool clear) {
+    I2Cdev::writeBit(devAddr, ITG3200_RA_INT_CFG, ITG3200_INTCFG_INT_ANYRD_2CLEAR_BIT, clear);
+}
+/** Get "device ready" interrupt enabled setting.
+ * Will be set 0 for disabled, 1 for enabled.
+ * @return Current interrupt enabled setting
+ * @see ITG3200_RA_INT_CFG
+ * @see ITG3200_INTCFG_ITG_RDY_EN_BIT
+ */
+bool ITG3200::getIntDeviceReadyEnabled() {
+    I2Cdev::readBit(devAddr, ITG3200_RA_INT_CFG, ITG3200_INTCFG_ITG_RDY_EN_BIT, buffer);
+    return buffer[0];
+}
+/** Set "device ready" interrupt enabled setting.
+ * @param enabled New interrupt enabled setting
+ * @see getIntDeviceReadyEnabled()
+ * @see ITG3200_RA_INT_CFG
+ * @see ITG3200_INTCFG_ITG_RDY_EN_BIT
+ */
+void ITG3200::setIntDeviceReadyEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, ITG3200_RA_INT_CFG, ITG3200_INTCFG_ITG_RDY_EN_BIT, enabled);
+}
+/** Get "data ready" interrupt enabled setting.
+ * Will be set 0 for disabled, 1 for enabled.
+ * @return Current interrupt enabled setting
+ * @see ITG3200_RA_INT_CFG
+ * @see ITG3200_INTCFG_RAW_RDY_EN_BIT
+ */
+bool ITG3200::getIntDataReadyEnabled() {
+    I2Cdev::readBit(devAddr, ITG3200_RA_INT_CFG, ITG3200_INTCFG_RAW_RDY_EN_BIT, buffer);
+    return buffer[0];
+}
+/** Set "data ready" interrupt enabled setting.
+ * @param enabled New interrupt enabled setting
+ * @see getIntDataReadyEnabled()
+ * @see ITG3200_RA_INT_CFG
+ * @see ITG3200_INTCFG_RAW_RDY_EN_BIT
+ */
+void ITG3200::setIntDataReadyEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, ITG3200_RA_INT_CFG, ITG3200_INTCFG_RAW_RDY_EN_BIT, enabled);
+}
+
+// INT_STATUS register
+
+/** Get Device Ready interrupt status.
+ * The ITG_RDY interrupt indicates that the PLL is ready and gyroscopic data can
+ * be read.
+ * @return Device Ready interrupt status
+ * @see ITG3200_RA_INT_STATUS
+ * @see ITG3200_INTSTAT_RAW_DATA_READY_BIT
+ */
+bool ITG3200::getIntDeviceReadyStatus() {
+    I2Cdev::readBit(devAddr, ITG3200_RA_INT_STATUS, ITG3200_INTSTAT_ITG_RDY_BIT, buffer);
+    return buffer[0];
+}
+/** Get Data Ready interrupt status.
+ * In normal use, the RAW_DATA_RDY interrupt is used to determine when new
+ * sensor data is available in and of the sensor registers (27 to 32).
+ * @return Data Ready interrupt status
+ * @see ITG3200_RA_INT_STATUS
+ * @see ITG3200_INTSTAT_RAW_DATA_READY_BIT
+ */
+bool ITG3200::getIntDataReadyStatus() {
+    I2Cdev::readBit(devAddr, ITG3200_RA_INT_STATUS, ITG3200_INTSTAT_RAW_DATA_READY_BIT, buffer);
+    return buffer[0];
+}
+
+// TEMP_OUT_* registers
+
+/** Get current internal temperature.
+ * @return Temperature reading in 16-bit 2's complement format
+ * @see ITG3200_RA_TEMP_OUT_H
+ */
+int16_t ITG3200::getTemperature() {
+    I2Cdev::readBytes(devAddr, ITG3200_RA_TEMP_OUT_H, 2, buffer);
+    return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+
+// GYRO_*OUT_* registers
+
+/** Get 3-axis gyroscope readings.
+ * @param x 16-bit signed integer container for X-axis rotation
+ * @param y 16-bit signed integer container for Y-axis rotation
+ * @param z 16-bit signed integer container for Z-axis rotation
+ * @see ITG3200_RA_GYRO_XOUT_H
+ */
+void ITG3200::getRotation(int16_t* x, int16_t* y, int16_t* z) {
+    I2Cdev::readBytes(devAddr, ITG3200_RA_GYRO_XOUT_H, 6, buffer);
+    *x = (((int16_t)buffer[0]) << 8) | buffer[1];
+    *y = (((int16_t)buffer[2]) << 8) | buffer[3];
+    *z = (((int16_t)buffer[4]) << 8) | buffer[5];
+}
+/** Get X-axis gyroscope reading.
+ * @return X-axis rotation measurement in 16-bit 2's complement format
+ * @see ITG3200_RA_GYRO_XOUT_H
+ */
+int16_t ITG3200::getRotationX() {
+    I2Cdev::readBytes(devAddr, ITG3200_RA_GYRO_XOUT_H, 2, buffer);
+    return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+/** Get Y-axis gyroscope reading.
+ * @return Y-axis rotation measurement in 16-bit 2's complement format
+ * @see ITG3200_RA_GYRO_YOUT_H
+ */
+int16_t ITG3200::getRotationY() {
+    I2Cdev::readBytes(devAddr, ITG3200_RA_GYRO_YOUT_H, 2, buffer);
+    return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+/** Get Z-axis gyroscope reading.
+ * @return Z-axis rotation measurement in 16-bit 2's complement format
+ * @see ITG3200_RA_GYRO_ZOUT_H
+ */
+int16_t ITG3200::getRotationZ() {
+    I2Cdev::readBytes(devAddr, ITG3200_RA_GYRO_ZOUT_H, 2, buffer);
+    return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+
+// PWR_MGM register
+
+/** Trigger a full device reset.
+ * A small delay of ~50ms may be desirable after triggering a reset.
+ * @see ITG3200_RA_PWR_MGM
+ * @see ITG3200_PWR_H_RESET_BIT
+ */
+void ITG3200::reset() {
+    I2Cdev::writeBit(devAddr, ITG3200_RA_PWR_MGM, ITG3200_PWR_H_RESET_BIT, true);
+}
+/** Get sleep mode status.
+ * Setting the SLEEP bit in the register puts the device into very low power
+ * sleep mode. In this mode, only the serial interface and internal registers
+ * remain active, allowing for a very low standby current. Clearing this bit
+ * puts the device back into normal mode. To save power, the individual standby
+ * selections for each of the gyros should be used if any gyro axis is not used
+ * by the application.
+ * @return Current sleep mode enabled status
+ * @see ITG3200_RA_PWR_MGM
+ * @see ITG3200_PWR_SLEEP_BIT
+ */
+bool ITG3200::getSleepEnabled() {
+    I2Cdev::readBit(devAddr, ITG3200_RA_PWR_MGM, ITG3200_PWR_SLEEP_BIT, buffer);
+    return buffer[0];
+}
+/** Set sleep mode status.
+ * @param enabled New sleep mode enabled status
+ * @see getSleepEnabled()
+ * @see ITG3200_RA_PWR_MGM
+ * @see ITG3200_PWR_SLEEP_BIT
+ */
+void ITG3200::setSleepEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, ITG3200_RA_PWR_MGM, ITG3200_PWR_SLEEP_BIT, enabled);
+}
+/** Get X-axis standby enabled status.
+ * If enabled, the X-axis will not gather or report data (or use power).
+ * @return Current X-axis standby enabled status
+ * @see ITG3200_RA_PWR_MGM
+ * @see ITG3200_PWR_STBY_XG_BIT
+ */
+bool ITG3200::getStandbyXEnabled() {
+    I2Cdev::readBit(devAddr, ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_XG_BIT, buffer);
+    return buffer[0];
+}
+/** Set X-axis standby enabled status.
+ * @param New X-axis standby enabled status
+ * @see getStandbyXEnabled()
+ * @see ITG3200_RA_PWR_MGM
+ * @see ITG3200_PWR_STBY_XG_BIT
+ */
+void ITG3200::setStandbyXEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_XG_BIT, enabled);
+}
+/** Get Y-axis standby enabled status.
+ * If enabled, the Y-axis will not gather or report data (or use power).
+ * @return Current Y-axis standby enabled status
+ * @see ITG3200_RA_PWR_MGM
+ * @see ITG3200_PWR_STBY_YG_BIT
+ */
+bool ITG3200::getStandbyYEnabled() {
+    I2Cdev::readBit(devAddr, ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_YG_BIT, buffer);
+    return buffer[0];
+}
+/** Set Y-axis standby enabled status.
+ * @param New Y-axis standby enabled status
+ * @see getStandbyYEnabled()
+ * @see ITG3200_RA_PWR_MGM
+ * @see ITG3200_PWR_STBY_YG_BIT
+ */
+void ITG3200::setStandbyYEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_YG_BIT, enabled);
+}
+/** Get Z-axis standby enabled status.
+ * If enabled, the Z-axis will not gather or report data (or use power).
+ * @return Current Z-axis standby enabled status
+ * @see ITG3200_RA_PWR_MGM
+ * @see ITG3200_PWR_STBY_ZG_BIT
+ */
+bool ITG3200::getStandbyZEnabled() {
+    I2Cdev::readBit(devAddr, ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_ZG_BIT, buffer);
+    return buffer[0];
+}
+/** Set Z-axis standby enabled status.
+ * @param New Z-axis standby enabled status
+ * @see getStandbyZEnabled()
+ * @see ITG3200_RA_PWR_MGM
+ * @see ITG3200_PWR_STBY_ZG_BIT
+ */
+void ITG3200::setStandbyZEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_ZG_BIT, enabled);
+}
+/** Get clock source setting.
+ * @return Current clock source setting
+ * @see ITG3200_RA_PWR_MGM
+ * @see ITG3200_PWR_CLK_SEL_BIT
+ * @see ITG3200_PWR_CLK_SEL_LENGTH
+ */
+uint8_t ITG3200::getClockSource() {
+    I2Cdev::readBits(devAddr, ITG3200_RA_PWR_MGM, ITG3200_PWR_CLK_SEL_BIT, ITG3200_PWR_CLK_SEL_LENGTH, buffer);
+    return buffer[0];
+}
+/** Set clock source setting.
+ * On power up, the ITG-3200 defaults to the internal oscillator. It is highly recommended that the device is configured to use one of the gyros (or an external clock) as the clock reference, due to the improved stability.
+ *
+ * The CLK_SEL setting determines the device clock source as follows:
+ *
+ * CLK_SEL | Clock Source
+ * --------+--------------------------------------
+ * 0       | Internal oscillator
+ * 1       | PLL with X Gyro reference
+ * 2       | PLL with Y Gyro reference
+ * 3       | PLL with Z Gyro reference
+ * 4       | PLL with external 32.768kHz reference
+ * 5       | PLL with external 19.2MHz reference
+ * 6       | Reserved
+ * 7       | Reserved
+ *
+ * @param source New clock source setting
+ * @see getClockSource()
+ * @see ITG3200_RA_PWR_MGM
+ * @see ITG3200_PWR_CLK_SEL_BIT
+ * @see ITG3200_PWR_CLK_SEL_LENGTH
+ */
+void ITG3200::setClockSource(uint8_t source) {
+    I2Cdev::writeBits(devAddr, ITG3200_RA_PWR_MGM, ITG3200_PWR_CLK_SEL_BIT, ITG3200_PWR_CLK_SEL_LENGTH, source);
+}

firmware/lib/imu/ITG3200.h

@@ -0,0 +1,179 @@
+// I2Cdev library collection - ITG3200 I2C device class header file
+// Based on InvenSense ITG-3200 datasheet rev. 1.4, 3/30/2010 (PS-ITG-3200A-00-01.4)
+// 7/31/2011 by Jeff Rowberg <jeff@rowberg.net>
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//     2011-07-31 - initial release
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2011 Jeff Rowberg
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+===============================================
+*/
+
+#ifndef _ITG3200_H_
+#define _ITG3200_H_
+
+#include "I2Cdev.h"
+
+#define ITG3200_ADDRESS_AD0_LOW     0x68 // address pin low (GND), default for SparkFun IMU Digital Combo board
+#define ITG3200_ADDRESS_AD0_HIGH    0x69 // address pin high (VCC), default for SparkFun ITG-3200 Breakout board
+#define ITG3200_DEFAULT_ADDRESS     ITG3200_ADDRESS_AD0_LOW
+
+#define ITG3200_RA_WHO_AM_I         0x00
+#define ITG3200_RA_SMPLRT_DIV       0x15
+#define ITG3200_RA_DLPF_FS          0x16
+#define ITG3200_RA_INT_CFG          0x17
+#define ITG3200_RA_INT_STATUS       0x1A
+#define ITG3200_RA_TEMP_OUT_H       0x1B
+#define ITG3200_RA_TEMP_OUT_L       0x1C
+#define ITG3200_RA_GYRO_XOUT_H      0x1D
+#define ITG3200_RA_GYRO_XOUT_L      0x1E
+#define ITG3200_RA_GYRO_YOUT_H      0x1F
+#define ITG3200_RA_GYRO_YOUT_L      0x20
+#define ITG3200_RA_GYRO_ZOUT_H      0x21
+#define ITG3200_RA_GYRO_ZOUT_L      0x22
+#define ITG3200_RA_PWR_MGM          0x3E
+
+#define ITG3200_DEVID_BIT           6
+#define ITG3200_DEVID_LENGTH        6
+
+#define ITG3200_DF_FS_SEL_BIT       4
+#define ITG3200_DF_FS_SEL_LENGTH    2
+#define ITG3200_DF_DLPF_CFG_BIT     2
+#define ITG3200_DF_DLPF_CFG_LENGTH  3
+
+#define ITG3200_FULLSCALE_2000      0x03
+
+#define ITG3200_DLPF_BW_256         0x00
+#define ITG3200_DLPF_BW_188         0x01
+#define ITG3200_DLPF_BW_98          0x02
+#define ITG3200_DLPF_BW_42          0x03
+#define ITG3200_DLPF_BW_20          0x04
+#define ITG3200_DLPF_BW_10          0x05
+#define ITG3200_DLPF_BW_5           0x06
+
+#define ITG3200_INTCFG_ACTL_BIT             7
+#define ITG3200_INTCFG_OPEN_BIT             6
+#define ITG3200_INTCFG_LATCH_INT_EN_BIT     5
+#define ITG3200_INTCFG_INT_ANYRD_2CLEAR_BIT 4
+#define ITG3200_INTCFG_ITG_RDY_EN_BIT       2
+#define ITG3200_INTCFG_RAW_RDY_EN_BIT       0
+
+#define ITG3200_INTMODE_ACTIVEHIGH  0x00
+#define ITG3200_INTMODE_ACTIVELOW   0x01
+
+#define ITG3200_INTDRV_PUSHPULL     0x00
+#define ITG3200_INTDRV_OPENDRAIN    0x01
+
+#define ITG3200_INTLATCH_50USPULSE  0x00
+#define ITG3200_INTLATCH_WAITCLEAR  0x01
+
+#define ITG3200_INTCLEAR_STATUSREAD 0x00
+#define ITG3200_INTCLEAR_ANYREAD    0x01
+
+#define ITG3200_INTSTAT_ITG_RDY_BIT         2
+#define ITG3200_INTSTAT_RAW_DATA_READY_BIT  0
+
+#define ITG3200_PWR_H_RESET_BIT     7
+#define ITG3200_PWR_SLEEP_BIT       6
+#define ITG3200_PWR_STBY_XG_BIT     5
+#define ITG3200_PWR_STBY_YG_BIT     4
+#define ITG3200_PWR_STBY_ZG_BIT     3
+#define ITG3200_PWR_CLK_SEL_BIT     2
+#define ITG3200_PWR_CLK_SEL_LENGTH  3
+
+#define ITG3200_CLOCK_INTERNAL      0x00
+#define ITG3200_CLOCK_PLL_XGYRO     0x01
+#define ITG3200_CLOCK_PLL_YGYRO     0x02
+#define ITG3200_CLOCK_PLL_ZGYRO     0x03
+#define ITG3200_CLOCK_PLL_EXT32K    0x04
+#define ITG3200_CLOCK_PLL_EXT19M    0x05
+
+class ITG3200 {
+    public:
+        ITG3200();
+        ITG3200(uint8_t address);
+        
+        void initialize();
+        bool testConnection();
+        
+        // WHO_AM_I register
+        uint8_t getDeviceID();
+        void setDeviceID(uint8_t id);
+
+        // SMPLRT_DIV register
+        uint8_t getRate();
+        void setRate(uint8_t rate);
+
+        // DLPF_FS register
+        uint8_t getFullScaleRange();
+        void setFullScaleRange(uint8_t range);
+        uint8_t getDLPFBandwidth();
+        void setDLPFBandwidth(uint8_t bandwidth);
+        
+        // INT_CFG register
+        bool getInterruptMode();
+        void setInterruptMode(bool mode);
+        bool getInterruptDrive();
+        void setInterruptDrive(bool drive);
+        bool getInterruptLatch();
+        void setInterruptLatch(bool latch);
+        bool getInterruptLatchClear();
+        void setInterruptLatchClear(bool clear);
+        bool getIntDeviceReadyEnabled();
+        void setIntDeviceReadyEnabled(bool enabled);
+        bool getIntDataReadyEnabled();
+        void setIntDataReadyEnabled(bool enabled);
+
+        // INT_STATUS register
+        bool getIntDeviceReadyStatus();
+        bool getIntDataReadyStatus();
+        
+        // TEMP_OUT_* registers
+        int16_t getTemperature();
+        
+        // GYRO_*OUT_* registers
+        void getRotation(int16_t* x, int16_t* y, int16_t* z);
+        int16_t getRotationX();
+        int16_t getRotationY();
+        int16_t getRotationZ();
+
+        // PWR_MGM register
+        void reset();
+        bool getSleepEnabled();
+        void setSleepEnabled(bool enabled);
+        bool getStandbyXEnabled();
+        void setStandbyXEnabled(bool enabled);
+        bool getStandbyYEnabled();
+        void setStandbyYEnabled(bool enabled);
+        bool getStandbyZEnabled();
+        void setStandbyZEnabled(bool enabled);
+        uint8_t getClockSource();
+        void setClockSource(uint8_t source);
+
+    private:
+        uint8_t devAddr;
+        uint8_t buffer[6];
+};
+
+#endif /* _ITG3200_H_ */

firmware/lib/imu/default_imu.h

@@ -0,0 +1,400 @@
+// Copyright (c) 2021 Juan Miguel Jimeno
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef DEFAULT_IMU
+#define DEFAULT_IMU
+
+//include IMU base interface
+#include "imu_interface.h"
+
+//include sensor API headers
+#include "I2Cdev.h"
+#include "ADXL345.h"
+#include "ITG3200.h"
+#include "HMC5883L.h"
+#include "MPU6050.h"
+#include "MPU9150.h"
+#include "MPU9250.h"
+#include "MPU9250_WE.h"
+
+class GY85IMU: public IMUInterface 
+{
+    private:
+        //constants specific to the sensor
+        const float accel_scale_ = 1 / 256.0;
+        const float gyro_scale_ = 1 / 14.375;
+
+        // driver objects to be used
+        ADXL345 accelerometer_;
+        ITG3200 gyroscope_;
+
+        // returned vector for sensor reading
+        geometry_msgs__msg__Vector3 accel_;
+        geometry_msgs__msg__Vector3 gyro_;
+
+    public:
+        GY85IMU()
+        {
+            // accel_cov_ = 0.001; //you can overwrite the convariance values here
+            // gyro_cov_ = 0.001; //you can overwrite the convariance values here
+        }
+
+        bool startSensor() override
+        {
+            // here you can override startSensor() function and use the sensor's driver API
+            // to initialize and test the sensor's connection during boot time
+            Wire.begin();
+            bool ret;
+            accelerometer_.initialize();
+            ret = accelerometer_.testConnection();
+            if(!ret)
+                return false;
+
+            gyroscope_.initialize();
+            ret = gyroscope_.testConnection();
+            if(!ret)
+                return false;
+
+            return true;
+        }
+
+        geometry_msgs__msg__Vector3 readAccelerometer() override
+        {
+            // here you can override readAccelerometer function and use the sensor's driver API
+            // to grab the data from accelerometer and return as a Vector3 object
+            int16_t ax, ay, az;
+            
+            accelerometer_.getAcceleration(&ax, &ay, &az);
+
+            accel_.x = ax * (double) accel_scale_ * g_to_accel_;
+            accel_.y = ay * (double) accel_scale_ * g_to_accel_;
+            accel_.z = az * (double) accel_scale_ * g_to_accel_;
+
+            return accel_;
+        }
+
+        geometry_msgs__msg__Vector3 readGyroscope() override
+        {
+            // here you can override readAccelerometer function and use the sensor's driver API
+            // to grab the data from gyroscope and return as a Vector3 object
+            int16_t gx, gy, gz;
+
+            gyroscope_.getRotation(&gx, &gy, &gz);
+
+            gyro_.x = gx * (double) gyro_scale_ * DEG_TO_RAD;
+            gyro_.y = gy * (double) gyro_scale_ * DEG_TO_RAD;
+            gyro_.z = gz * (double) gyro_scale_ * DEG_TO_RAD;
+
+            return gyro_;
+        }
+};
+
+
+class MPU6050IMU: public IMUInterface 
+{
+    private:
+        const float accel_scale_ = 1 / 16384.0;
+        const float gyro_scale_ = 1 / 131.0;
+
+        MPU6050 accelerometer_;
+        MPU6050 gyroscope_;
+
+        geometry_msgs__msg__Vector3 accel_;
+        geometry_msgs__msg__Vector3 gyro_;
+
+    public:
+        MPU6050IMU()
+        {
+        }
+
+        bool startSensor() override
+        {
+            Wire.begin();
+            bool ret;
+            accelerometer_.initialize();
+            ret = accelerometer_.testConnection();
+            if(!ret)
+                return false;
+
+            gyroscope_.initialize();
+            ret = gyroscope_.testConnection();
+            if(!ret)
+                return false;
+
+            return true;
+        }
+
+        geometry_msgs__msg__Vector3 readAccelerometer() override
+        {
+            int16_t ax, ay, az;
+            
+            accelerometer_.getAcceleration(&ax, &ay, &az);
+
+            accel_.x = ax * (double) accel_scale_ * g_to_accel_;
+            accel_.y = ay * (double) accel_scale_ * g_to_accel_;
+            accel_.z = az * (double) accel_scale_ * g_to_accel_;
+
+            return accel_;
+        }
+
+        geometry_msgs__msg__Vector3 readGyroscope() override
+        {
+            int16_t gx, gy, gz;
+
+            gyroscope_.getRotation(&gx, &gy, &gz);
+
+            gyro_.x = gx * (double) gyro_scale_ * DEG_TO_RAD;
+            gyro_.y = gy * (double) gyro_scale_ * DEG_TO_RAD;
+            gyro_.z = gz * (double) gyro_scale_ * DEG_TO_RAD;
+
+            return gyro_;
+        }
+};
+
+class MPU9150IMU: public IMUInterface 
+{
+    private:
+        const float accel_scale_ = 1 / 16384.0;
+        const float gyro_scale_ = 1 / 131.0;
+
+        MPU9150 accelerometer_;
+        MPU9150 gyroscope_;
+
+        geometry_msgs__msg__Vector3 accel_;
+        geometry_msgs__msg__Vector3 gyro_;
+
+    public:
+        MPU9150IMU()
+        {
+        }
+
+        bool startSensor() override
+        {
+            Wire.begin();
+            bool ret;
+            accelerometer_.initialize();
+            ret = accelerometer_.testConnection();
+            if(!ret)
+                return false;
+
+            gyroscope_.initialize();
+            ret = gyroscope_.testConnection();
+            if(!ret)
+                return false;
+
+            return true;
+        }
+
+        geometry_msgs__msg__Vector3 readAccelerometer() override
+        {
+            int16_t ax, ay, az;
+            
+            accelerometer_.getAcceleration(&ax, &ay, &az);
+
+            accel_.x = ax * (double) accel_scale_ * g_to_accel_;
+            accel_.y = ay * (double) accel_scale_ * g_to_accel_;
+            accel_.z = az * (double) accel_scale_ * g_to_accel_;
+
+            return accel_;
+        }
+
+        geometry_msgs__msg__Vector3 readGyroscope() override
+        {
+            int16_t gx, gy, gz;
+
+            gyroscope_.getRotation(&gx, &gy, &gz);
+
+            gyro_.x = gx * (double) gyro_scale_ * DEG_TO_RAD;
+            gyro_.y = gy * (double) gyro_scale_ * DEG_TO_RAD;
+            gyro_.z = gz * (double) gyro_scale_ * DEG_TO_RAD;
+
+            return gyro_;
+        }
+};
+
+class MPU9250IMU: public IMUInterface 
+{
+    private:
+        const float accel_scale_ = 1 / 16384.0;
+        const float gyro_scale_ = 1 / 131.0;
+
+        MPU9250 accelerometer_;
+        MPU9250 gyroscope_;
+
+        geometry_msgs__msg__Vector3 accel_;
+        geometry_msgs__msg__Vector3 gyro_;
+
+    public:
+        MPU9250IMU()
+        {
+        }
+
+        bool startSensor() override
+        {
+            Wire.begin();
+            bool ret;
+            accelerometer_.initialize();
+            ret = accelerometer_.testConnection();
+            if(!ret)
+                return false;
+
+            gyroscope_.initialize();
+            ret = gyroscope_.testConnection();
+            if(!ret)
+                return false;
+
+            return true;
+        }
+
+        geometry_msgs__msg__Vector3 readAccelerometer() override
+        {
+            int16_t ax, ay, az;
+            
+            accelerometer_.getAcceleration(&ax, &ay, &az);
+
+            accel_.x = ax * (double) accel_scale_ * g_to_accel_;
+            accel_.y = ay * (double) accel_scale_ * g_to_accel_;
+            accel_.z = az * (double) accel_scale_ * g_to_accel_;
+
+            return accel_;
+        }
+
+        geometry_msgs__msg__Vector3 readGyroscope() override
+        {
+            int16_t gx, gy, gz;
+
+            gyroscope_.getRotation(&gx, &gy, &gz);
+
+            gyro_.x = gx * (double) gyro_scale_ * DEG_TO_RAD;
+            gyro_.y = gy * (double) gyro_scale_ * DEG_TO_RAD;
+            gyro_.z = gz * (double) gyro_scale_ * DEG_TO_RAD;
+
+            return gyro_;
+        }
+};
+
+class MPU9250IMU_WE: public IMUInterface 
+{
+    private:
+        const float accel_scale_ = 1 / 16384.0;
+        const float gyro_scale_ = 1 / 131.0;
+
+        MPU9250_WE accelerometer_;
+        MPU9250_WE gyroscope_;
+
+        geometry_msgs__msg__Vector3 accel_;
+        geometry_msgs__msg__Vector3 gyro_;
+
+    public:
+        MPU9250IMU_WE()
+        {
+        }
+
+        bool startSensor() override
+        {
+            Wire.begin();
+            bool ret;
+            accelerometer_.init();
+            ret = accelerometer_.init();
+            if(!ret)
+                return false;
+
+            gyroscope_.init();
+            ret = gyroscope_.init();
+            if(!ret)
+                return false;
+
+            return true;
+        }
+
+        geometry_msgs__msg__Vector3 readAccelerometer() override
+        {
+            //int16_t ax, ay, az;
+                
+            xyzFloat xyz_Gval = accelerometer_.getGValues();
+            // ax = static_cast<int>(xyz_Gval.x); // Convert x to an integer
+            // ay = static_cast<int>(xyz_Gval.y); // Convert y to an integer
+            // az = static_cast<int>(xyz_Gval.z); // Convert z to an integer
+
+            float ax = xyz_Gval.x; // Convert x to an integer
+            float ay = xyz_Gval.y; // Convert y to an integer
+            float az = xyz_Gval.z; // Convert z to an integer
+
+            accel_.x = ax * (double) accel_scale_ * g_to_accel_;
+            accel_.y = ay * (double) accel_scale_ * g_to_accel_;
+            accel_.z = az * (double) accel_scale_ * g_to_accel_;
+
+            return accel_;
+        }
+
+        geometry_msgs__msg__Vector3 readGyroscope() override
+        {
+            //int16_t gx, gy, gz;
+
+            xyzFloat xyz_gyr = gyroscope_.getGyrValues();
+            // gx = static_cast<int>(xyz_gyr.x); // Convert x to an integer
+            // gy = static_cast<int>(xyz_gyr.y); // Convert y to an integer
+            // gz = static_cast<int>(xyz_gyr.z); // Convert z to an integer
+
+            float gx = xyz_gyr.x; // Convert x to an integer
+            float gy = xyz_gyr.y; // Convert y to an integer
+            float gz = xyz_gyr.z; // Convert z to an integer
+
+            gyro_.x = gx * (double) gyro_scale_ * DEG_TO_RAD;
+            gyro_.y = gy * (double) gyro_scale_ * DEG_TO_RAD;
+            gyro_.z = gz * (double) gyro_scale_ * DEG_TO_RAD;
+
+            return gyro_;
+        }
+};
+
+class FakeIMU: public IMUInterface 
+{
+    private:
+        geometry_msgs__msg__Vector3 accel_;
+        geometry_msgs__msg__Vector3 gyro_;
+
+    public:
+        FakeIMU()
+        {
+        }
+
+        bool startSensor() override
+        {
+            return true;
+        }
+
+        geometry_msgs__msg__Vector3 readAccelerometer() override
+        {
+            return accel_;
+        }
+
+        geometry_msgs__msg__Vector3 readGyroscope() override
+        {
+            return gyro_;
+        }
+};
+
+#endif
+//ADXL345 https://www.sparkfun.com/datasheets/Sensors/Accelerometer/ADXL345.pdf
+//HMC8553L https://cdn-shop.adafruit.com/datasheets/HMC5883L_3-Axis_Digital_Compass_IC.pdf
+//ITG320 https://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf
+
+
+//MPU9150 https://www.invensense.com/wp-content/uploads/2015/02/PS-MPU-9250A-01-v1.1.pdf
+//MPU9250 https://www.invensense.com/wp-content/uploads/2015/02/MPU-9150-Datasheet.pdf
+//MPU6050 https://store.invensense.com/datasheets/invensense/MPU-6050_DataSheet_V3%204.pdf
+
+//http://www.sureshjoshi.com/embedded/invensense-imus-what-to-know/
+//https://stackoverflow.com/questions/19161872/meaning-of-lsb-unit-and-unit-lsb

firmware/lib/imu/fake_mag.h

@@ -0,0 +1,38 @@
+#ifndef _FAKE_MAG_H_
+#define _FAKE_MAG_H_
+
+
+//This is a pseudo Magnetometer Class as a placeholder for IMUs that do not have a Magnetometer
+
+class FakeMag
+{
+    public:
+        FakeMag();
+        bool initialize();
+        bool testConnection();
+        void getHeading(int16_t* mx, int16_t* my, int16_t* mz);
+};
+
+FakeMag::FakeMag()
+{
+
+}
+
+bool  FakeMag::initialize()
+{
+    return true;
+}
+
+bool  FakeMag::testConnection()
+{
+    return true;
+}
+
+void FakeMag::getHeading(int16_t* mx, int16_t* my, int16_t* mz)
+{
+    *mx = 0;
+    *my = 0;
+    *mz = 0;
+}
+
+#endif

firmware/lib/imu/imu.h

@@ -0,0 +1,48 @@
+// Copyright (c) 2021 Juan Miguel Jimeno
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef IMU_CONFIG_H
+#define IMU_CONFIG_H
+
+// include the header of your new driver here similar to default_imu.h
+#include "default_imu.h"
+
+// now you can create a config constant that you can use in lino_base_config.h
+#ifdef USE_GY85_IMU
+    // pass your built in class to IMU macro
+    #define IMU GY85IMU
+#endif
+
+#ifdef USE_MPU6050_IMU
+    #define IMU MPU6050IMU
+#endif
+
+#ifdef USE_MPU9150_IMU
+    #define IMU MPU9150IMU
+#endif
+
+#ifdef USE_MPU9250_IMU
+    #define IMU MPU9250IMU
+#endif
+
+#ifdef USE_MPU9250_WE_IMU
+    #define IMU MPU9250IMU_WE
+#endif
+
+#ifdef USE_FAKE_IMU
+    #define IMU FakeIMU
+#endif
+
+#endif
+

firmware/lib/imu/imu_interface.h

@@ -0,0 +1,101 @@
+// Copyright (c) 2021 Juan Miguel Jimeno
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef IMU_INTERFACE
+#define IMU_INTERFACE
+
+#include <sensor_msgs/msg/imu.h>
+
+class IMUInterface
+{
+    protected:
+        sensor_msgs__msg__Imu imu_msg_;
+        const float g_to_accel_ = 9.81;
+        const float mgauss_to_utesla_ = 0.1;
+        const float utesla_to_tesla_ = 0.000001;
+
+        float accel_cov_ = 0.00001;
+        float gyro_cov_ = 0.00001;
+        const int sample_size_ = 40;
+
+        geometry_msgs__msg__Vector3 gyro_cal_;
+
+        void calibrateGyro()
+        {
+            geometry_msgs__msg__Vector3 gyro;
+
+            for(int i=0; i<sample_size_; i++)
+            {
+                gyro = readGyroscope();
+                gyro_cal_.x += gyro.x;
+                gyro_cal_.y += gyro.y;
+                gyro_cal_.z += gyro.z;
+
+                delay(50);
+            }
+
+            gyro_cal_.x = gyro_cal_.x / (float)sample_size_;
+            gyro_cal_.y = gyro_cal_.y / (float)sample_size_;
+            gyro_cal_.z = gyro_cal_.z / (float)sample_size_;
+        }
+   
+    public:
+        IMUInterface()
+        {
+            imu_msg_.header.frame_id = micro_ros_string_utilities_set(imu_msg_.header.frame_id, "imu_link");
+        }
+
+        virtual geometry_msgs__msg__Vector3 readAccelerometer() = 0;
+        virtual geometry_msgs__msg__Vector3 readGyroscope() = 0;
+        virtual bool startSensor() = 0;
+
+        bool init()
+        {
+            bool sensor_ok = startSensor();
+            if(sensor_ok)
+                calibrateGyro();
+
+            return sensor_ok;
+        }
+
+        sensor_msgs__msg__Imu getData()
+        {
+            imu_msg_.angular_velocity = readGyroscope();
+            imu_msg_.angular_velocity.x -= gyro_cal_.x; 
+            imu_msg_.angular_velocity.y -= gyro_cal_.y; 
+            imu_msg_.angular_velocity.z -= gyro_cal_.z; 
+
+            if(imu_msg_.angular_velocity.x > -0.01 && imu_msg_.angular_velocity.x < 0.01 )
+                imu_msg_.angular_velocity.x = 0; 
+         
+            if(imu_msg_.angular_velocity.y > -0.01 && imu_msg_.angular_velocity.y < 0.01 )
+                imu_msg_.angular_velocity.y = 0;
+
+            if(imu_msg_.angular_velocity.z > -0.01 && imu_msg_.angular_velocity.z < 0.01 )
+                imu_msg_.angular_velocity.z = 0;
+       
+            imu_msg_.angular_velocity_covariance[0] = gyro_cov_;
+            imu_msg_.angular_velocity_covariance[4] = gyro_cov_;
+            imu_msg_.angular_velocity_covariance[8] = gyro_cov_;
+            
+            imu_msg_.linear_acceleration = readAccelerometer();
+            imu_msg_.linear_acceleration_covariance[0] = accel_cov_;
+            imu_msg_.linear_acceleration_covariance[4] = accel_cov_;
+            imu_msg_.linear_acceleration_covariance[8] = accel_cov_;
+
+            return imu_msg_;
+        }
+};
+
+#endif

firmware/lib/kinematics/kinematics.cpp

@@ -0,0 +1,149 @@
+// Copyright (c) 2021 Juan Miguel Jimeno
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "Arduino.h"
+#include "kinematics.h"
+
+Kinematics::Kinematics(base robot_base, int motor_max_rpm, float max_rpm_ratio,
+                       float motor_operating_voltage, float motor_power_max_voltage,
+                       float wheel_diameter, float wheels_y_distance):
+    base_platform_(robot_base),
+    wheels_y_distance_(wheels_y_distance),
+    wheel_circumference_(PI * wheel_diameter),
+    total_wheels_(getTotalWheels(robot_base))
+{    
+    motor_power_max_voltage = constrain(motor_power_max_voltage, 0, motor_operating_voltage);
+    max_rpm_ =  ((motor_power_max_voltage / motor_operating_voltage) * motor_max_rpm) * max_rpm_ratio;
+}
+
+Kinematics::rpm Kinematics::calculateRPM(float linear_x, float linear_y, float angular_z)
+{
+
+    float tangential_vel = angular_z * (wheels_y_distance_ / 2.0);
+
+    //convert m/s to m/min
+    float linear_vel_x_mins = linear_x * 60.0;
+    float linear_vel_y_mins = linear_y * 60.0;
+    //convert rad/s to rad/min
+    float tangential_vel_mins = tangential_vel * 60.0;
+
+    float x_rpm = linear_vel_x_mins / wheel_circumference_;
+    float y_rpm = linear_vel_y_mins / wheel_circumference_;
+    float tan_rpm = tangential_vel_mins / wheel_circumference_;
+
+    float a_x_rpm = fabs(x_rpm);
+    float a_y_rpm = fabs(y_rpm);
+    float a_tan_rpm = fabs(tan_rpm);
+
+    float xy_sum = a_x_rpm + a_y_rpm;
+    float xtan_sum = a_x_rpm + a_tan_rpm;
+
+    //calculate the scale value how much each target velocity
+    //must be scaled down in such cases where the total required RPM
+    //is more than the motor's max RPM
+    //this is to ensure that the required motion is achieved just with slower speed
+    if(xy_sum >= max_rpm_ && angular_z == 0)
+    {
+        float vel_scaler = max_rpm_ / xy_sum;
+
+        x_rpm *= vel_scaler;
+        y_rpm *= vel_scaler;
+    }
+    
+    else if(xtan_sum >= max_rpm_ && linear_y == 0)
+    {
+        float vel_scaler = max_rpm_ / xtan_sum;
+
+        x_rpm *= vel_scaler;
+        tan_rpm *= vel_scaler;
+    }
+
+    Kinematics::rpm rpm;
+
+    //calculate for the target motor RPM and direction
+    //front-left motor
+    rpm.motor1 = x_rpm - y_rpm - tan_rpm;
+    rpm.motor1 = constrain(rpm.motor1, -max_rpm_, max_rpm_);
+
+    //front-right motor
+    rpm.motor2 = x_rpm + y_rpm + tan_rpm;
+    rpm.motor2 = constrain(rpm.motor2, -max_rpm_, max_rpm_);
+
+    //rear-left motor
+    rpm.motor3 = x_rpm + y_rpm - tan_rpm;
+    rpm.motor3 = constrain(rpm.motor3, -max_rpm_, max_rpm_);
+
+    //rear-right motor
+    rpm.motor4 = x_rpm - y_rpm + tan_rpm;
+    rpm.motor4 = constrain(rpm.motor4, -max_rpm_, max_rpm_);
+
+    return rpm;
+}
+
+Kinematics::rpm Kinematics::getRPM(float linear_x, float linear_y, float angular_z)
+{
+    if(base_platform_ == DIFFERENTIAL_DRIVE || base_platform_ == SKID_STEER)
+    {
+        linear_y = 0;
+    }
+
+    return calculateRPM(linear_x, linear_y, angular_z);;
+}
+
+Kinematics::velocities Kinematics::getVelocities(float rpm1, float rpm2, float rpm3, float rpm4)
+{
+    Kinematics::velocities vel;
+    float average_rps_x;
+    float average_rps_y;
+    float average_rps_a;
+
+    if(base_platform_ == DIFFERENTIAL_DRIVE)
+    {
+        rpm3 = 0.0;
+        rpm4 = 0.0;
+    }
+ 
+    //convert average revolutions per minute to revolutions per second
+    average_rps_x = ((float)(rpm1 + rpm2 + rpm3 + rpm4) / total_wheels_) / 60.0; // RPM
+    vel.linear_x = average_rps_x * wheel_circumference_; // m/s
+
+    //convert average revolutions per minute in y axis to revolutions per second
+    average_rps_y = ((float)(-rpm1 + rpm2 + rpm3 - rpm4) / total_wheels_) / 60.0; // RPM
+    if(base_platform_ == MECANUM)
+        vel.linear_y = average_rps_y * wheel_circumference_; // m/s
+    else
+        vel.linear_y = 0;
+
+    //convert average revolutions per minute to revolutions per second
+    average_rps_a = ((float)(-rpm1 + rpm2 - rpm3 + rpm4) / total_wheels_) / 60.0;
+    vel.angular_z =  (average_rps_a * wheel_circumference_) / (wheels_y_distance_ / 2.0); //  rad/s
+
+    return vel;
+}
+
+int Kinematics::getTotalWheels(base robot_base)
+{
+    switch(robot_base)
+    {
+        case DIFFERENTIAL_DRIVE:    return 2;
+        case SKID_STEER:            return 4;
+        case MECANUM:               return 4;
+        default:                    return 2;
+    }
+}
+
+float Kinematics::getMaxRPM()
+{
+    return max_rpm_;
+}

firmware/lib/kinematics/kinematics.h

@@ -0,0 +1,69 @@
+// Copyright (c) 2021 Juan Miguel Jimeno
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef KINEMATICS_H
+#define KINEMATICS_H
+
+#include "Arduino.h"
+
+#define RPM_TO_RPS 1/60
+
+class Kinematics
+{
+    public:
+        enum base {DIFFERENTIAL_DRIVE, SKID_STEER, MECANUM};
+
+        base base_platform_;
+
+        struct rpm
+        {
+            float motor1;
+            float motor2;
+            float motor3;
+            float motor4;
+        };
+        
+        struct velocities
+        {
+            float linear_x;
+            float linear_y;
+            float angular_z;
+        };
+
+        struct pwm
+        {
+            int motor1;
+            int motor2;
+            int motor3;
+            int motor4;
+        };
+        Kinematics(base robot_base, int motor_max_rpm, float max_rpm_ratio,
+                   float motor_operating_voltage, float motor_power_max_voltage,
+                   float wheel_diameter, float wheels_y_distance);
+        velocities getVelocities(float rpm1, float rpm2, float rpm3, float rpm4);
+        rpm getRPM(float linear_x, float linear_y, float angular_z);
+        float getMaxRPM();
+
+    private:
+        rpm calculateRPM(float linear_x, float linear_y, float angular_z);
+        int getTotalWheels(base robot_base);
+
+        float max_rpm_;
+        float wheels_y_distance_;
+        float pwm_res_;
+        float wheel_circumference_;
+        int total_wheels_;
+};
+
+#endif

firmware/lib/motor/default_motor.h

@@ -0,0 +1,220 @@
+// // Copyright (c) 2021 Juan Miguel Jimeno
+// //
+// // Licensed under the Apache License, Version 2.0 (the "License");
+// // you may not use this file except in compliance with the License.
+// // You may obtain a copy of the License at
+// //
+// //     http://www.apache.org/licenses/LICENSE-2.0
+// //
+// // Unless required by applicable law or agreed to in writing, software
+// // distributed under the License is distributed on an "AS IS" BASIS,
+// // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// // See the License for the specific language governing permissions and
+// // limitations under the License.
+
+// #ifndef DEFAULT_MOTOR
+// #define DEFAULT_MOTOR
+
+// #include <Arduino.h>
+// #include <Servo.h> 
+
+// #include "motor_interface.h"
+
+// class Generic2: public MotorInterface
+// {
+//     private:
+//         int in_a_pin_;
+//         int in_b_pin_;
+//         int pwm_pin_;
+
+//     protected:
+//         void forward(int pwm) override
+//         {
+//             digitalWrite(in_a_pin_, HIGH);
+//             digitalWrite(in_b_pin_, LOW);
+//             analogWrite(pwm_pin_, abs(pwm));
+//         }
+
+//         void reverse(int pwm) override
+//         {
+//             digitalWrite(in_a_pin_, LOW);
+//             digitalWrite(in_b_pin_, HIGH);
+//             analogWrite(pwm_pin_, abs(pwm));
+//         }
+
+//     public:
+//         Generic2(float pwm_frequency, int pwm_bits, bool invert, int pwm_pin, int in_a_pin, int in_b_pin): 
+//             MotorInterface(invert),
+//             in_a_pin_(in_a_pin),
+//             in_b_pin_(in_b_pin),
+//             pwm_pin_(pwm_pin)
+//         {
+//             pinMode(in_a_pin_, OUTPUT);
+//             pinMode(in_b_pin_, OUTPUT);
+//             pinMode(pwm_pin_, OUTPUT);
+
+//             if(pwm_frequency > 0)
+//             {
+//                 // analogWriteFrequency(pwm_pin_, pwm_frequency);
+//                 analogWriteFrequency(pwm_frequency);
+//             }
+//             analogWriteResolution(pwm_bits);
+
+//             //ensure that the motor is in neutral state during bootup
+//             analogWrite(pwm_pin_, abs(0));
+//         }
+
+//         void brake() override
+//         {
+//             analogWrite(pwm_pin_, 0);
+//         }
+// };
+
+// class Generic1: public MotorInterface
+// {
+//     private:
+//         int in_pin_;
+//         int pwm_pin_;
+
+//     protected:
+//         void forward(int pwm) override
+//         {
+//             digitalWrite(in_pin_, HIGH);
+//             analogWrite(pwm_pin_, abs(pwm));
+//         }
+
+//         void reverse(int pwm) override
+//         {
+//             digitalWrite(in_pin_, LOW);
+//             analogWrite(pwm_pin_, abs(pwm));
+//         }
+
+//     public:
+//         Generic1(float pwm_frequency, int pwm_bits, bool invert, int pwm_pin, int in_pin, int unused=-1): 
+//             MotorInterface(invert),
+//             in_pin_(in_pin),
+//             pwm_pin_(pwm_pin)
+//         {
+//             pinMode(in_pin_, OUTPUT);
+//             pinMode(pwm_pin_, OUTPUT);
+
+//             if(pwm_frequency > 0)
+//             {
+//                 analogWriteFrequency(pwm_frequency);
+//             }
+//             analogWriteResolution(pwm_bits);
+
+//             //ensure that the motor is in neutral state during bootup
+//             analogWrite(pwm_pin_, abs(0));
+//         }
+
+//         void brake() override
+//         {
+//             analogWrite(pwm_pin_, 0);
+//         }
+// };
+
+// class BTS7960: public MotorInterface
+// {
+//     private:
+//         int in_a_pin_;
+//         int in_b_pin_;
+
+//     protected:
+//         void forward(int pwm) override
+//         {
+//             analogWrite(in_a_pin_, 0);
+//             analogWrite(in_b_pin_, abs(pwm));
+//         }
+
+//         void reverse(int pwm) override
+//         {
+//             analogWrite(in_b_pin_, 0);
+//             analogWrite(in_a_pin_, abs(pwm));
+//         }
+
+//     public:
+//         BTS7960(float pwm_frequency, int pwm_bits, bool invert, int unused, int in_a_pin, int in_b_pin): 
+//             MotorInterface(invert),
+//             in_a_pin_(in_a_pin),
+//             in_b_pin_(in_b_pin)
+//         {
+//             pinMode(in_a_pin_, OUTPUT);
+//             pinMode(in_b_pin_, OUTPUT);
+
+//             if(pwm_frequency > 0)
+//             {
+//                 analogWriteFrequency(pwm_frequency);
+//                 analogWriteFrequency(pwm_frequency);
+
+//             }
+//             analogWriteResolution(pwm_bits);
+
+//             //ensure that the motor is in neutral state during bootup
+//             analogWrite(in_a_pin_, 0);
+//             analogWrite(in_b_pin_, 0);
+//         }
+    
+//         BTS7960(float pwm_frequency, int pwm_bits, bool invert, int in_a_pin, int in_b_pin): 
+//             MotorInterface(invert),
+//             in_a_pin_(in_a_pin),
+//             in_b_pin_(in_b_pin)
+//         {
+//             pinMode(in_a_pin_, OUTPUT);
+//             pinMode(in_b_pin_, OUTPUT);
+
+//             if(pwm_frequency > 0)
+//             {
+//                 analogWriteFrequency(pwm_frequency);
+//                 // analogWriteFrequency(in_b_pin_, pwm_frequency);
+
+//             }
+//             analogWriteResolution(pwm_bits);
+
+//             //ensure that the motor is in neutral state during bootup
+//             analogWrite(in_a_pin_, 0);
+//             analogWrite(in_b_pin_, 0);
+//         }
+
+//         void brake() override
+//         {
+//             analogWrite(in_b_pin_, 0);
+//             analogWrite(in_a_pin_, 0);            
+//         }
+// };
+
+// class ESC: public MotorInterface
+// {
+//     private:
+//         Servo motor_;
+//         int pwm_pin_;
+
+//     protected:
+//         void forward(int pwm) override
+//         {
+//             motor_.writeMicroseconds(1500 + pwm);
+//         }
+
+//         void reverse(int pwm) override
+//         {
+//             motor_.writeMicroseconds(1500 + pwm);
+//         }
+
+//     public:
+//         ESC(float pwm_frequency, int pwm_bits, bool invert, int pwm_pin, int unused=-1, int unused2=-1): 
+//             MotorInterface(invert),
+//             pwm_pin_(pwm_pin)
+//         {
+//             motor_.attach(pwm_pin);
+            
+//             //ensure that the motor is in neutral state during bootup
+//             motor_.writeMicroseconds(1500);
+//         }
+
+//         void brake() override
+//         {
+//             motor_.writeMicroseconds(1500);         
+//         }
+// };
+
+// #endif

firmware/lib/motor/motor.h

@@ -0,0 +1,43 @@
+// Copyright (c) 2021 Juan Miguel Jimeno
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MOTOR_H
+#define MOTOR_H
+
+// include the header of your new driver here similar to default_motor.h
+#include "default_motor.h"
+
+// now you can create a config constant that you can use in lino_base_config.h
+#ifdef USE_GENERIC_2_IN_MOTOR_DRIVER
+    // pass your built in class to Motor macro
+    #define Motor Generic2
+#endif
+
+#ifdef USE_GENERIC_1_IN_MOTOR_DRIVER
+    // pass your built in class to Motor macro
+    #define Motor Generic1
+#endif
+
+#ifdef USE_BTS7960_MOTOR_DRIVER
+    // pass your built in class to Motor macro
+    #define Motor BTS7960
+#endif
+
+#ifdef USE_ESC_MOTOR_DRIVER
+    // pass your built in class to Motor macro
+    #define Motor ESC
+#endif
+
+
+#endif

firmware/lib/motor/motor_interface.h

@@ -0,0 +1,46 @@
+// Copyright (c) 2021 Juan Miguel Jimeno
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MOTOR_INTERFACE
+#define MOTOR_INTERFACE
+
+class MotorInterface
+{
+    bool invert_;
+    protected:
+        virtual void forward(int pwm) = 0;
+        virtual void reverse(int pwm) = 0;
+
+    public:
+        MotorInterface(int invert):
+            invert_(invert)
+        {
+        }
+
+        virtual void brake() = 0;
+        void spin(int pwm)
+        {
+            if(invert_)
+                pwm *= -1;
+
+            if(pwm > 0)
+                forward(pwm);
+            else if(pwm < 0)
+                reverse(pwm);
+            else
+                brake();
+        }
+};
+
+#endif

firmware/lib/odometry/odometry.cpp

@@ -0,0 +1,92 @@
+// Copyright (c) 2021 Juan Miguel Jimeno
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "odometry.h"
+
+Odometry::Odometry():
+    x_pos_(0.0),
+    y_pos_(0.0),
+    heading_(0.0)
+{
+    odom_msg_.header.frame_id = micro_ros_string_utilities_set(odom_msg_.header.frame_id, "odom");
+    odom_msg_.child_frame_id = micro_ros_string_utilities_set(odom_msg_.child_frame_id, "base_footprint");
+}
+
+void Odometry::update(float vel_dt, float linear_vel_x, float linear_vel_y, float angular_vel_z)
+{
+    float delta_heading = angular_vel_z * vel_dt; //radians
+    float cos_h = cos(heading_);
+    float sin_h = sin(heading_);
+    float delta_x = (linear_vel_x * cos_h - linear_vel_y * sin_h) * vel_dt; //m
+    float delta_y = (linear_vel_x * sin_h + linear_vel_y * cos_h) * vel_dt; //m
+
+    //calculate current position of the robot
+    x_pos_ += delta_x;
+    y_pos_ += delta_y;
+    heading_ += delta_heading;
+
+    //calculate robot's heading in quaternion angle
+    //ROS has a function to calculate yaw in quaternion angle
+    float q[4];
+    euler_to_quat(0, 0, heading_, q);
+
+    //robot's position in x,y, and z
+    odom_msg_.pose.pose.position.x = x_pos_;
+    odom_msg_.pose.pose.position.y = y_pos_;
+    odom_msg_.pose.pose.position.z = 0.0;
+
+    //robot's heading in quaternion
+    odom_msg_.pose.pose.orientation.x = (double) q[1];
+    odom_msg_.pose.pose.orientation.y = (double) q[2];
+    odom_msg_.pose.pose.orientation.z = (double) q[3];
+    odom_msg_.pose.pose.orientation.w = (double) q[0];
+
+    odom_msg_.pose.covariance[0] = 0.001;
+    odom_msg_.pose.covariance[7] = 0.001;
+    odom_msg_.pose.covariance[35] = 0.001;
+
+    //linear speed from encoders
+    odom_msg_.twist.twist.linear.x = linear_vel_x;
+    odom_msg_.twist.twist.linear.y = linear_vel_y;
+    odom_msg_.twist.twist.linear.z = 0.0;
+
+    //angular speed from encoders
+    odom_msg_.twist.twist.angular.x = 0.0;
+    odom_msg_.twist.twist.angular.y = 0.0;
+    odom_msg_.twist.twist.angular.z = angular_vel_z;
+
+    odom_msg_.twist.covariance[0] = 0.0001;
+    odom_msg_.twist.covariance[7] = 0.0001;
+    odom_msg_.twist.covariance[35] = 0.0001;
+}
+
+nav_msgs__msg__Odometry Odometry::getData()
+{
+    return odom_msg_;
+} 
+
+const void Odometry::euler_to_quat(float roll, float pitch, float yaw, float* q) 
+{
+    float cy = cos(yaw * 0.5);
+    float sy = sin(yaw * 0.5);
+    float cp = cos(pitch * 0.5);
+    float sp = sin(pitch * 0.5);
+    float cr = cos(roll * 0.5);
+    float sr = sin(roll * 0.5);
+
+    q[0] = cy * cp * cr + sy * sp * sr;
+    q[1] = cy * cp * sr - sy * sp * cr;
+    q[2] = sy * cp * sr + cy * sp * cr;
+    q[3] = sy * cp * cr - cy * sp * sr;
+}

firmware/lib/odometry/odometry.h

@@ -0,0 +1,39 @@
+// Copyright (c) 2021 Juan Miguel Jimeno
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ODOMETRY_H
+#define ODOMETRY_H
+
+#include <Arduino.h>
+#include <micro_ros_utilities/type_utilities.h>
+#include <micro_ros_utilities/string_utilities.h>
+#include <nav_msgs/msg/odometry.h>
+
+class Odometry
+{
+    public:
+        Odometry();
+        void update(float vel_dt, float linear_vel_x, float linear_vel_y, float angular_vel_z);
+        nav_msgs__msg__Odometry getData();
+
+    private:
+        const void euler_to_quat(float x, float y, float z, float* q);
+
+        nav_msgs__msg__Odometry odom_msg_;
+        float x_pos_;
+        float y_pos_;
+        float heading_;
+};
+
+#endif

firmware/lib/pid/pid.cpp

@@ -0,0 +1,54 @@
+// Copyright (c) 2021 Juan Miguel Jimeno
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "Arduino.h"
+#include "pid.h"
+
+PID::PID(float min_val, float max_val, float kp, float ki, float kd):
+    min_val_(min_val),
+    max_val_(max_val),
+    kp_(kp),
+    ki_(ki),
+    kd_(kd)
+{
+}
+
+double PID::compute(float setpoint, float measured_value)
+{
+    double error;
+    double pid;
+
+    //setpoint is constrained between min and max to prevent pid from having too much error
+    error = setpoint - measured_value;
+    integral_ += error;
+    derivative_ = error - prev_error_;
+
+    if(setpoint == 0 && error == 0)
+    {
+        integral_ = 0;
+        derivative_ = 0;
+    }
+
+    pid = (kp_ * error) + (ki_ * integral_) + (kd_ * derivative_);
+    prev_error_ = error;
+
+    return constrain(pid, min_val_, max_val_);
+}
+
+void PID::updateConstants(float kp, float ki, float kd)
+{
+    kp_ = kp;
+    ki_ = ki;
+    kd_ = kd;
+}

firmware/lib/pid/pid.h

@@ -0,0 +1,38 @@
+// Copyright (c) 2021 Juan Miguel Jimeno
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef PID_H
+#define PID_H
+
+#include "Arduino.h"
+
+class PID
+{
+    public:
+        PID(float min_val, float max_val, float kp, float ki, float kd);
+        double compute(float setpoint, float measured_value);
+        void updateConstants(float kp, float ki, float kd);
+
+    private:
+        float min_val_;
+        float max_val_;
+        float kp_;
+        float ki_;
+        float kd_;
+        double integral_;
+        double derivative_;
+        double prev_error_;
+};
+
+#endif

firmware/platformio.ini

@@ -0,0 +1,94 @@
+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; https://docs.platformio.org/page/projectconf.html
+
+; [env]
+; platform = teensy
+; framework = arduino
+; upload_port = /dev/ttyACM0
+; upload_protocol = teensy-cli
+; board_microros_transport = serial
+; board_microros_distro = ${sysenv.ROS_DISTRO}
+; lib_deps = https://github.com/micro-ROS/micro_ros_platformio
+; build_flags = -I ../config
+
+[env:esp32doit-devkit-v1]
+platform = espressif32
+board = esp32doit-devkit-v1
+framework = arduino
+board_microros_transport = serial
+
+[env:esp32-s2-saola-1]
+platform = espressif32
+board = esp32-s2-saola-1
+monitor_speed = 115200
+framework = arduino
+board_microros_transport = serial
+;board_microros_transport = wifi
+board_microros_distro = ${sysenv.ROS_DISTRO}
+lib_deps =
+    https://github.com/micro-ROS/micro_ros_platformio
+    roboticsbrno/ServoESP32@^1.0.3
+    wollewald/MPU9250_WE@^1.2.6
+build_flags = -I ../config
+
+[env:teensy41] 
+board = teensy41
+
+[env:teensy40] 
+board = teensy40
+
+[env:teensy36]
+board = teensy36
+build_flags = 
+    -I ../config
+    -llibc -lc
+
+[env:teensy35]
+board = teensy35
+build_flags = 
+    -I ../config
+    -llibc -lc
+
+[env:teensy31] 
+board = teensy31
+board_build.f_cpu = 96000000L
+build_flags = 
+    -I ../config
+    -llibc -lc
+
+[env:dev] 
+board = teensy40
+build_flags = 
+    -I ../config
+    -D USE_DEV_CONFIG
+
+[env:beebo] 
+board = teensy31
+board_build.f_cpu = 96000000L
+build_flags = 
+    -I ../config
+    -llibc -lc
+    -D USE_BEEBO_CONFIG
+
+[env:beebo_m] 
+board = teensy31
+board_build.f_cpu = 96000000L
+build_flags = 
+    -I ../config
+    -llibc -lc
+    -D USE_BEEBO_M_CONFIG
+
+[env:square] 
+board = teensy40
+upload_port = /dev/linobase
+build_flags = 
+    -I ../config
+    -D USE_SQUARE_CONFIG
+

firmware/src/firmware.ino

@@ -0,0 +1,311 @@
+// Copyright (c) 2021 Juan Miguel Jimeno
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#include <Arduino.h>
+#include <micro_ros_platformio.h>
+#include <stdio.h>
+
+#include <rcl/rcl.h>
+#include <rcl/error_handling.h>
+#include <rclc/rclc.h>
+#include <rclc/executor.h>
+
+#include <nav_msgs/msg/odometry.h>
+#include <sensor_msgs/msg/imu.h>
+#include <geometry_msgs/msg/twist.h>
+#include <geometry_msgs/msg/vector3.h>
+
+#include "config.h"
+#include "motor.h"
+#include "kinematics.h"
+#include "pid.h"
+#include "odometry.h"
+#include "imu.h"
+#define ENCODER_USE_INTERRUPTS
+#define ENCODER_OPTIMIZE_INTERRUPTS
+#include "encoder.h"
+
+#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){rclErrorLoop();}}
+#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}
+#define EXECUTE_EVERY_N_MS(MS, X)  do { \
+  static volatile int64_t init = -1; \
+  if (init == -1) { init = uxr_millis();} \
+  if (uxr_millis() - init > MS) { X; init = uxr_millis();} \
+} while (0)
+
+rcl_publisher_t odom_publisher;
+rcl_publisher_t imu_publisher;
+rcl_subscription_t twist_subscriber;
+
+nav_msgs__msg__Odometry odom_msg;
+sensor_msgs__msg__Imu imu_msg;
+geometry_msgs__msg__Twist twist_msg;
+
+rclc_executor_t executor;
+rclc_support_t support;
+rcl_allocator_t allocator;
+rcl_node_t node;
+rcl_timer_t control_timer;
+
+unsigned long long time_offset = 0;
+unsigned long prev_cmd_time = 0;
+unsigned long prev_odom_update = 0;
+
+enum states 
+{
+  WAITING_AGENT,
+  AGENT_AVAILABLE,
+  AGENT_CONNECTED,
+  AGENT_DISCONNECTED
+} state;
+
+// Encoder motor1_encoder(MOTOR1_ENCODER_A, MOTOR1_ENCODER_B, COUNTS_PER_REV1, MOTOR1_ENCODER_INV);
+// Encoder motor2_encoder(MOTOR2_ENCODER_A, MOTOR2_ENCODER_B, COUNTS_PER_REV2, MOTOR2_ENCODER_INV);
+// Encoder motor3_encoder(MOTOR3_ENCODER_A, MOTOR3_ENCODER_B, COUNTS_PER_REV3, MOTOR3_ENCODER_INV);
+// Encoder motor4_encoder(MOTOR4_ENCODER_A, MOTOR4_ENCODER_B, COUNTS_PER_REV4, MOTOR4_ENCODER_INV);
+
+// Motor motor1_controller(PWM_FREQUENCY, PWM_BITS, MOTOR1_INV, MOTOR1_PWM, MOTOR1_IN_A, MOTOR1_IN_B);
+// Motor motor2_controller(PWM_FREQUENCY, PWM_BITS, MOTOR2_INV, MOTOR2_PWM, MOTOR2_IN_A, MOTOR2_IN_B);
+// Motor motor3_controller(PWM_FREQUENCY, PWM_BITS, MOTOR3_INV, MOTOR3_PWM, MOTOR3_IN_A, MOTOR3_IN_B);
+// Motor motor4_controller(PWM_FREQUENCY, PWM_BITS, MOTOR4_INV, MOTOR4_PWM, MOTOR4_IN_A, MOTOR4_IN_B);
+
+// PID motor1_pid(PWM_MIN, PWM_MAX, K_P, K_I, K_D);
+// PID motor2_pid(PWM_MIN, PWM_MAX, K_P, K_I, K_D);
+// PID motor3_pid(PWM_MIN, PWM_MAX, K_P, K_I, K_D);
+// PID motor4_pid(PWM_MIN, PWM_MAX, K_P, K_I, K_D);
+
+// Kinematics kinematics(
+//     Kinematics::LINO_BASE, 
+//     MOTOR_MAX_RPM, 
+//     MAX_RPM_RATIO, 
+//     MOTOR_OPERATING_VOLTAGE, 
+//     MOTOR_POWER_MAX_VOLTAGE, 
+//     WHEEL_DIAMETER, 
+//     LR_WHEELS_DISTANCE
+// );
+
+Odometry odometry;
+IMU imu;
+
+#define USE_SERIAL
+
+#ifdef USE_SERIAL
+    void setup() 
+    {
+        pinMode(LED_PIN, OUTPUT);
+
+        Serial.begin(115200);
+        Serial.println("Serial started");
+
+        Serial.println("Checking IMU ... ");
+        bool imu_ok = imu.init();
+        Serial.println("IMU initialized ...");
+        if(!imu_ok)
+        {
+            Serial.println("IMU not found");
+            while(1)
+            {
+                //flashLED(3);
+            }
+        }
+
+        set_microros_serial_transports(Serial);
+
+        Serial.println("Serial Connected ... ");
+        Serial.println("Connecting to MicroROS Agent ... ");
+        Serial.println("Serial Connection to MicroROS Agent established ... ");
+        Serial.println("IMU Datastream open!");
+
+
+    }
+#endif
+
+#ifdef USE_WIFI
+    void setup() 
+    {
+        pinMode(LED_PIN, OUTPUT);
+
+        Serial.begin(115200);
+        Serial.println("Serial started");
+
+        Serial.println("Checking IMU ... ");
+        bool imu_ok = imu.init();
+        Serial.println("IMU initialized ...");
+        if(!imu_ok)
+        {
+            Serial.println("IMU not found");
+            while(1)
+            {
+                //flashLED(3);
+            }
+        }
+        
+        Serial.println("Serial Connected ... ");
+
+        Serial.println("Connecting to MicroROS Agent ... ");
+        IPAddress agent_ip(192, 168, 20, 15);
+        size_t agent_port = 8888;
+
+        char ssid[] = "CPS2.4G";
+        char psk[]= "pac999CPS";
+
+        Serial.println("WiFi Connected ... ");
+        set_microros_wifi_transports(ssid, psk, agent_ip, agent_port);
+
+        Serial.println("MicroROS transport open on WiFi!");
+
+
+    }
+#endif
+
+void loop() {
+    switch (state) 
+    {
+        case WAITING_AGENT:
+            EXECUTE_EVERY_N_MS(500, state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1)) ? AGENT_AVAILABLE : WAITING_AGENT;);
+            break;
+        case AGENT_AVAILABLE:
+            state = (true == createEntities()) ? AGENT_CONNECTED : WAITING_AGENT;
+            if (state == WAITING_AGENT) 
+            {
+                destroyEntities();
+            }
+            break;
+        case AGENT_CONNECTED:
+            EXECUTE_EVERY_N_MS(200, state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1)) ? AGENT_CONNECTED : AGENT_DISCONNECTED;);
+            if (state == AGENT_CONNECTED) 
+            {
+                rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
+            }
+            break;
+        case AGENT_DISCONNECTED:
+            destroyEntities();
+            state = WAITING_AGENT;
+            break;
+        default:
+            break;
+    }
+}
+
+void controlCallback(rcl_timer_t * timer, int64_t last_call_time) 
+{
+    RCLC_UNUSED(last_call_time);
+    if (timer != NULL) 
+    {
+       publishData();
+    }
+}
+
+void twistCallback(const void * msgin) 
+{
+    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
+
+    prev_cmd_time = millis();
+}
+
+bool createEntities()
+{
+    allocator = rcl_get_default_allocator();
+    //create init_options
+    RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
+    // create node
+    RCCHECK(rclc_node_init_default(&node, "linorobot_base_node", "", &support));
+    
+    // create IMU publisher
+    RCCHECK(rclc_publisher_init_default( 
+        &imu_publisher, 
+        &node,
+        ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Imu),
+        "imu/data"
+    ));
+    
+    executor = rclc_executor_get_zero_initialized_executor();
+    RCCHECK(rclc_executor_init(&executor, &support.context, 2, & allocator));
+    RCCHECK(rclc_executor_add_timer(&executor, &control_timer));
+
+    // synchronize time with the agent
+    syncTime();
+    digitalWrite(LED_PIN, HIGH);
+
+    return true;
+}
+
+bool destroyEntities()
+{
+    rmw_context_t * rmw_context = rcl_context_get_rmw_context(&support.context);
+    (void) rmw_uros_set_context_entity_destroy_session_timeout(rmw_context, 0);
+
+    rcl_publisher_fini(&imu_publisher, &node);
+    rcl_node_fini(&node);
+    rcl_timer_fini(&control_timer);
+    rclc_executor_fini(&executor);
+    rclc_support_fini(&support);
+
+    digitalWrite(LED_PIN, HIGH);
+    
+    return true;
+}
+
+void publishData()
+{
+    imu_msg = imu.getData();
+
+    struct timespec time_stamp = getTime();
+
+    imu_msg.header.stamp.sec = time_stamp.tv_sec;
+    imu_msg.header.stamp.nanosec = time_stamp.tv_nsec;
+
+    RCSOFTCHECK(rcl_publish(&imu_publisher, &imu_msg, NULL));
+}
+
+void syncTime()
+{
+    // get the current time from the agent
+    unsigned long now = millis();
+    RCCHECK(rmw_uros_sync_session(10));
+    unsigned long long ros_time_ms = rmw_uros_epoch_millis(); 
+    // now we can find the difference between ROS time and uC time
+    time_offset = ros_time_ms - now;
+}
+
+struct timespec getTime()
+{
+    struct timespec tp = {0};
+    // add time difference between uC time and ROS time to
+    // synchronize time with ROS
+    unsigned long long now = millis() + time_offset;
+    tp.tv_sec = now / 1000;
+    tp.tv_nsec = (now % 1000) * 1000000;
+
+    return tp;
+}
+
+void rclErrorLoop() 
+{
+    while(true)
+    {
+        flashLED(2);
+    }
+}
+
+void flashLED(int n_times)
+{
+    for(int i=0; i<n_times; i++)
+    {
+        digitalWrite(LED_PIN, HIGH);
+        delay(150);
+        digitalWrite(LED_PIN, LOW);
+        delay(150);
+    }
+    delay(1000);
+}

readme.md

@@ -0,0 +1 @@
+CPS micro-ROS for ESP32 with IMU publisher