diff --git a/examples/01.Quadcopter/01.Quadcopter.ino b/examples/01.Quadcopter/01.Quadcopter.ino
index b3f6eae9..fd938a15 100644
--- a/examples/01.Quadcopter/01.Quadcopter.ino
+++ b/examples/01.Quadcopter/01.Quadcopter.ino
@@ -29,7 +29,7 @@ OFF - not powered
 startup: a couple blinks then ON while running gyro calibration (don't move)
 blinking long OFF short ON - DISARMED
 blinking long ON short OFF - ARMED
-blink interval longer than 1 second - imu_onUpdate() is taking too much time
+blink interval longer than 1 second - imu_loop() is taking too much time
 fast blinking - something is wrong, connect USB serial for info
 ##########################################################################################################################*/
 
@@ -37,49 +37,60 @@ fast blinking - something is wrong, connect USB serial for info
 //========================================================================================================================//
 //                                                 PINS                                                                   //
 //========================================================================================================================//
-// PINS are setup in the board header file library/src/madflight_board_default_*.h, but you can use these defines to 
-// override the pins. The pin numbers below are the default for Raspberry Pi Pico
+// PINS are defined in the board header file library/src/madflight_board_default_XXX.h, but you can use these defines to 
+// override the pins. 
+
+/*
+//The pin numbers below are an example for an easy soldering ESP32 WeMos LOLIN32-Lite with GY-6500/GY-271/GY-BPM280 sensor modules
 
 //LED:
-//#define HW_PIN_LED       25
-//#define HW_LED_ON         1 //0:low is on, 1:high is on
+#define HW_PIN_LED       22
+#define HW_LED_ON         0 //0:low is on, 1:high is on
 
 //IMU SPI:
-//#define HW_PIN_SPI_MISO  16
-//#define HW_PIN_SPI_MOSI  19
-//#define HW_PIN_SPI_SCLK  18
-//#define HW_PIN_IMU_CS    17
-//#define HW_PIN_IMU_EXTI  22 //external interrupt pin
+#define HW_PIN_SPI_MISO  25
+#define HW_PIN_SPI_MOSI  14
+#define HW_PIN_SPI_SCLK  12
+#define HW_PIN_IMU_CS    32
+#define HW_PIN_IMU_EXTI  33 //external interrupt pin
 
 //I2C for BARO, MAG, BAT sensors and for IMU if not using SPI
-//#define HW_PIN_I2C_SDA   20
-//#define HW_PIN_I2C_SCL   21
+#define HW_PIN_I2C_SDA   23
+#define HW_PIN_I2C_SCL   19
 
-//Outputs:
-//#define HW_OUT_COUNT     12 //number of outputs
-//#define HW_PIN_OUT_LIST  {2,3,4,5,6,7,10,11,12,13,14,15} //list of output pins
+//Motor/Servo Outputs:
+#define HW_OUT_COUNT     4 //number of outputs
+#define HW_PIN_OUT_LIST  {13,15,2,0} //list of output pins
 
 //Serial debug on USB Serial port (no GPIO pins)
 
 //RC Receiver:
-//#define HW_PIN_RCIN_RX     1
-//#define HW_PIN_RCIN_TX     0
-//#define HW_PIN_RCIN_INVERTER -1 //only used for STM32 targets
+#define HW_PIN_RCIN_RX    16
+#define HW_PIN_RCIN_TX     4
+#define HW_PIN_RCIN_INVERTER -1 //only used for STM32 targets
 
 //GPS:
-//#define HW_PIN_GPS_RX      9 
-//#define HW_PIN_GPS_TX      8
-//#define HW_PIN_GPS_INVERTER -1 //only used for STM32 targets
+#define HW_PIN_GPS_RX     -1 
+#define HW_PIN_GPS_TX     -1
+#define HW_PIN_GPS_INVERTER -1 //only used for STM32 targets
 
 //Battery ADC
-//#define HW_PIN_BAT_V      28
-//#define HW_PIN_BAT_I      -1
+#define HW_PIN_BAT_V      -1
+#define HW_PIN_BAT_I      -1
 
 //BlackBox SPI:
-//#define HW_PIN_SPI2_MISO  -1
-//#define HW_PIN_SPI2_MOSI  -1
-//#define HW_PIN_SPI2_SCLK  -1
-//#define HW_PIN_BB_CS      -1
+#define HW_PIN_SPI2_MISO  -1
+#define HW_PIN_SPI2_MOSI  -1
+#define HW_PIN_SPI2_SCLK  -1
+#define HW_PIN_BB_CS      -1
+*/
+
+//RP2040 specific options
+//#define HW_RP2040_SYS_CLK_KHZ 200000 //overclocking
+//#define HW_RP2040_USE_FREERTOS //enable use of FreeRTOS - experimental
+
+//ESP32 specific options
+//#define USE_ESP32_SOFTWIRE //use bitbang I2C (not hardware I2C) See https://github.com/espressif/esp-idf/issues/4999
 
 //========================================================================================================================//
 //                                                 BOARD                                                                  //
@@ -101,7 +112,7 @@ fast blinking - something is wrong, connect USB serial for info
 #define IMU_USE  IMU_USE_SPI_MPU6500 // IMU_USE_SPI_BMI270, IMU_USE_SPI_MPU9250, IMU_USE_SPI_MPU6500, IMU_USE_SPI_MPU6000, IMU_USE_I2C_MPU9250, IMU_USE_I2C_MPU9150, IMU_USE_I2C_MPU6500, IMU_USE_I2C_MPU6050, IMU_USE_I2C_MPU6000
 //Set sensor orientation. The label is yaw / roll (in that order) needed to rotate the sensor from it's normal position to it's mounted position.
 //If not sure what is needed: use CLI 'proll' and try each setting until roll-right gives positive ahrs_roll, pitch-up gives positive ahrs_pitch, and yaw-right gives positive ahrs_yaw
-#define IMU_ALIGN  IMU_ALIGN_CW90 //IMU_ALIGN_CW0, IMU_ALIGN_CW90, IMU_ALIGN_CW180, IMU_ALIGN_CW270, IMU_ALIGN_CW0FLIP, IMU_ALIGN_CW90FLIP, IMU_ALIGN_CW180FLIP, IMU_ALIGN_CW270FLIP
+#define IMU_ALIGN  IMU_ALIGN_CW0 //IMU_ALIGN_CW0, IMU_ALIGN_CW90, IMU_ALIGN_CW180, IMU_ALIGN_CW270, IMU_ALIGN_CW0FLIP, IMU_ALIGN_CW90FLIP, IMU_ALIGN_CW180FLIP, IMU_ALIGN_CW270FLIP
 #define IMU_I2C_ADR  0x68 //IMU I2C address. If unknown, use CLI 'i2c'
 
 //--- GPS
@@ -136,7 +147,7 @@ const int rcin_cfg_aux_channel   = 6; //Fight mode - 6 position switch
 //throttle pwm values
 const int rcin_cfg_thro_low      = 1250; //used to set rcin_thro_is_low flag when pwm is below. Note: your craft won't arm if this is too low.
 const int rcin_cfg_thro_max      = 1900;
-const float out_armed_speed      = 0.2; //Safety feature: make props spin when armed, the motors spin at this speed when armed and thottle is low. The default 0.2 is probably fairly high, set lower as needed.
+const float out_armed_speed      = 0.2; //Safety feature: make props spin when armed, the motors spin at this speed when armed and throttle is low. The default 0.2 is probably fairly high, set lower as needed.
 
 //roll, pitch, yaw pwm values
 const int rcin_cfg_pwm_min       = 1150;
@@ -162,11 +173,14 @@ const int out_MOTOR_COUNT = 4;
 //name the outputs, to make code more readable
 enum out_enum {MOTOR1,MOTOR2,MOTOR3,MOTOR4,SERVO1,SERVO2,SERVO3,SERVO4,SERVO5,SERVO6,SERVO7,SERVO8,SERVO9,SERVO10,SERVO11,SERVO12}; 
 
-//Low Pass Filter parameters. Do not touch unless you know what you are doing.
-float LP_accel       = 70;        //Accelerometer lowpass filter cutoff frequency in Hz (default MPU6050: 50Hz, MPU9250: 70Hz)
-float LP_gyro        = 60;        //Gyro lowpass filter cutoff frequency in Hz (default MPU6050: 35Hz, MPU9250: 60Hz)
-float LP_mag         = 1e10;      //Magnetometer lowpass filter cutoff frequency in Hz (default 1e10Hz, i.e. no filtering)
-float LP_radio       = 400;       //Radio input lowpass filter cutoff frequency in Hz (default 400Hz)
+const uint32_t imu_sample_rate = 1000; //imu sample rate in Hz (default 1000) NOTE: not all IMU drivers support this
+const uint32_t baro_sample_rate = 100; //baro sample rate in Hz (default 100)
+
+//Low Pass Filter cutoff frequency in Hz. Do not touch unless you know what you are doing.
+float LP_accel       = 70;        //Accelerometer  (default MPU6050: 50Hz, MPU9250: 70Hz)
+float LP_gyro        = 60;        //Gyro           (default MPU6050: 35Hz, MPU9250: 60Hz)
+float LP_mag         = 1e10;      //Magnetometer   (default 1e10Hz, i.e. no filtering)
+float LP_radio       = 400;       //Radio Input    (default 400Hz)
 
 //AHRS Parameters
 float ahrs_MadgwickB = 0.041;     //Madgwick filter parameter
@@ -216,7 +230,7 @@ float roll_PID = 0, pitch_PID = 0, yaw_PID = 0;
 bool out_armed = false; //motors will only run if this flag is true
 
 //Low pass filter parameters
-float B_accel ,B_gyro, B_mag, B_radio;
+float B_accel, B_gyro, B_mag, B_radio;
 
 const float rad_to_deg = 57.29577951; //radians to degrees conversion constant
 
@@ -226,19 +240,6 @@ const float rad_to_deg = 57.29577951; //radians to degrees conversion constant
 //Note: most madflight modules are header only. By placing the madflight include here allows the modules to access the global variables without declaring them extern.
 #include <madflight.h>
 
-//========================================================================================================================//
-//                  SETUP1() LOOP1() EXECUTING ON SECOND CORE (only for dual core MCUs like ESP32 and RP2040)             //
-//========================================================================================================================//
-//Uncomment setup1() and/or loop1() to use the second core on ESP32 / RP2040
-/*
-void setup1() {
-  Serial.println("setup1()");
-}
-void loop1() {
-  Serial.println("loop1()"); delay(100);
-}
-//*/
-
 //========================================================================================================================//
 //                                                       SETUP()                                                          //
 //========================================================================================================================//
@@ -252,7 +253,7 @@ void setup() {
     Serial.printf(MADFLIGHT_VERSION " starting %d ...\n",i);
     delay(300);
     led.toggle();
-  }
+  } 
   led.on();
 
   hw_setup(); //hardware specific setup for spi and Wire (see hw_xxx.h)
@@ -263,7 +264,7 @@ void setup() {
 
   //IMU: keep on trying until no error is returned (some sensors need a couple of tries...)
   while(true) {
-    int rv = imu.setup(1000); //request 1000 Hz sample rate, returns 0 on success, positive on error, negative on warning
+    int rv = imu.setup(imu_sample_rate); //request 1000 Hz sample rate, returns 0 on success, positive on error, negative on warning
     if(rv<=0) break;
     warn("IMU: init failed rv= " + String(rv) + ". Retrying...\n");
   }
@@ -274,7 +275,7 @@ void setup() {
   B_mag = lowpass_to_beta(LP_mag, imu.getSampleRate());
   B_radio = lowpass_to_beta(LP_radio, imu.getSampleRate());
 
-  baro.setup(); //Barometer
+  baro.setup(baro_sample_rate); //Barometer sample rate 100Hz
   mag.setup(); //External Magnetometer
   bat.setup(); //Battery Monitor
   bb.setup(); //Black Box
@@ -299,15 +300,15 @@ void setup() {
     out[i].writeFactor(out_command[i]); //start the PWM output to the motors
   }
 
-  //Calibrate for zero gyro readings, assuming vehicle not moving when powered up. Comment out to only use cfg values. (Use CLI to calibrate acc.)
-  cli.calibrate_gyro();
+  //start IMU update handler
+  imu.onUpdate = imu_loop;
+  if(!imu.waitNewSample()) die("IMU interrupt not firing.");
 
   //set quarterion to initial yaw, so that AHRS settles faster
   ahrs_Setup();
 
-  //start IMU update handler - do this last in setup(), as the handler needs all modules configured
-  imu.onUpdate = imu_onUpdate;
-  if(!imu.waitNewSample()) die("IMU interrupt not firing.");
+  //Calibrate for zero gyro readings, assuming vehicle not moving when powered up. Comment out to only use cfg values. (Use CLI to calibrate acc.)
+  cli.calibrate_gyro();
 
   cli.welcome();
 
@@ -319,13 +320,10 @@ void setup() {
 //========================================================================================================================//
 
 void loop() {
-  #ifdef USE_IMU_BUS_SPI
-    //if IMU uses SPI bus, then read slower i2c sensors here in loop() to keep imu_interrupt_handler() as fast as possible
-    i2c_sensors_update();
-  #endif
+  //if IMU uses SPI bus (not I2C bus), then read slower i2c sensors here in loop() to keep imu_loop() as fast as possible
+  if (!imu.usesI2C()) i2c_sensors_update();
 
   gps_loop(); //update gps
-  if(bat.update()) bb.log_bat(); //update battery, and log if battery was updated
 
   //send telemetry
   static uint32_t rcin_telem_ts = 0;
@@ -333,8 +331,8 @@ void loop() {
   if(millis() - rcin_telem_ts > 100) {
     rcin_telem_ts = millis();
     rcin_telem_cnt++;
-    rcin_telemetry_flight_mode("madflight"); //only first 14 char get transmitted
-    rcin_telemetry_attitude(ahrs_pitch, ahrs_roll, ahrs_yaw);
+    if(out_armed) rcin_telemetry_flight_mode("ARMED"); else rcin_telemetry_flight_mode("madflight"); //only first 14 char get transmitted
+    rcin_telemetry_attitude(ahrs_pitch, ahrs_roll, ahrs_yaw);  
     if(rcin_telem_cnt % 10 == 0) rcin_telemetry_battery(bat.v, bat.i, bat.mah, 100);
     if(rcin_telem_cnt % 10 == 5) rcin_telemetry_gps(gps.lat, gps.lon, gps.sog/278, gps.cog/1000, (gps.alt<0 ? 0 : gps.alt/1000), gps.sat); // sog/278 is conversion from mm/s to km/h 
   }
@@ -342,7 +340,9 @@ void loop() {
   cli.loop(); //process CLI commands
 }
 
+//update all I2C sensors, called from loop() with SPI IMU, or called from imu_loop() with I2C IMU
 void i2c_sensors_update() {
+  if(bat.update()) bb.log_bat(); //update battery, and log if battery was updated. 
   if(baro.update()) bb.log_baro(); //log if pressure updated
   mag.update();
 }
@@ -351,8 +351,8 @@ void i2c_sensors_update() {
 //                                                   IMU UPDATE LOOP                                                      //
 //========================================================================================================================//
 
-//This is _MAIN_ function of this program. It is called when new IMU data is available.
-void imu_onUpdate() {
+//This is __MAIN__ function of this program. It is called when new IMU data is available.
+void imu_loop() {
   //Blink LED
   led_Blink();
 
@@ -363,7 +363,7 @@ void imu_onUpdate() {
   //ahrs_Madgwick(GyroX, GyroY, GyroZ, AccX, AccY, AccZ, MagX, MagY, MagZ, imu.dt); //Madgwick filter quaternion update
   ahrs_Mahony(GyroX, GyroY, GyroZ, AccX, AccY, AccZ, MagX, MagY, MagZ, imu.dt); //Mahony filter quaternion update
 
-  //get ahrs_roll, ahrs_pitch, and ahrs_yaw angle estimates (degrees) from quaterion
+  //get ahrs_roll, ahrs_pitch, and ahrs_yaw angle estimates (degrees) from quaternion
   ahrs_ComputeAngles(&ahrs_roll, &ahrs_pitch, &ahrs_yaw);
 
   //Get radio state
@@ -374,8 +374,7 @@ void imu_onUpdate() {
   //rcin_thro = 0.5; rcin_thro_is_low = false; rcin_roll = 0; rcin_pitch = 0; rcin_yaw = 0; rcin_armed = true; rcin_aux = 0; out_armed = true;
 
   //PID Controller - SELECT ONE:
-  control_Angle(rcin_thro_is_low); //Stabilize on pitch/roll angle setpoint, stabilize yaw on rate setpoint
-  //control_Angle2(rcin_thro_is_low); //Stabilize on pitch/roll setpoint using cascaded method. Rate controller must be tuned well first!
+  control_Angle(rcin_thro_is_low); //Stabilize on pitch/roll angle setpoint, stabilize yaw on rate setpoint  //control_Angle2(rcin_thro_is_low); //Stabilize on pitch/roll setpoint using cascaded method. Rate controller must be tuned well first!
   //control_Rate(rcin_thro_is_low); //Stabilize on rate setpoint
 
   //Actuator mixing
@@ -385,16 +384,14 @@ void imu_onUpdate() {
   out_KillSwitchAndFailsafe(); //Cut all motor outputs if DISARMED or failsafe triggered.
   out_SetCommands(); //Sends command pulses to motors (only if out_armed=true) and servos
 
-  //if IMU uses i2c bus, then get i2c sensor readings in imu_interrupt_handler() to prevent i2c bus collisions. Alternatively, put the IMU on a separate i2c bus.
-  #ifdef USE_IMU_BUS_I2C
-    i2c_sensors_update();
-  #endif
+  //if IMU uses I2C bus, then get I2C sensor readings in imu_interrupt_handler() to prevent I2C bus collisions. Alternatively, put the IMU on a separate I2C bus.
+  if (imu.usesI2C()) i2c_sensors_update();
 
   //bb.log_imu(); //full speed black box logging of IMU data, memory fills up quickly...
 }
 
 //========================================================================================================================//
-//                      IMU UPDATE LOOP FUNCTIONS - in same order as they are called from imu_onUpdate()                           //
+//                      IMU UPDATE LOOP FUNCTIONS - in same order as they are called from imu_loop()                           //
 //========================================================================================================================//
 
 void led_Blink() {
@@ -407,8 +404,8 @@ void led_Blink() {
 }
 
 // Reads accelerometer, gyro, and magnetometer data from IMU and stores it as AccX, AccY, AccZ, GyroX, GyroY, GyroZ, MagX, MagY, MagZ. 
-// A simple first-order low-pass filter is used to get rid of high frequency noise in these raw signals. 
-// Finally, the constant errors found in calibrate_IMU_error() on startup are subtracted from the accelerometer and gyro readings.
+// The constant calibraton errors are subtracted from the raw readings.
+// A simple first-order low-pass filter is used to get rid of high frequency noise. 
 void imu_Filter() {
   //Accelerometer
   //LP filter corrected accelerometer data
@@ -479,9 +476,9 @@ void rcin_Normalize() {
    */
 
   //normalized values
-  //throttle: when stick is back below rcin_cfg_thro_low then rcin_thro = rcin_cfg_thro_low value (approx 0.1), and rcin_thro = 1.0 on full throttle
+  //throttle: 0.0 in range from stick full back to rcin_cfg_thro_low, 1.0 on full throttle
   int pwm = rcin_pwm[rcin_cfg_thro_channel-1];
-  rcin_thro = constrain( ((float)(pwm - rcin_cfg_thro_low)) / (rcin_cfg_thro_max - rcin_cfg_thro_low), out_armed_speed, 1.0);
+  rcin_thro = constrain( ((float)(pwm - rcin_cfg_thro_low)) / (rcin_cfg_thro_max - rcin_cfg_thro_low), 0.0, 1.0);
   rcin_thro_is_low = (pwm <= rcin_cfg_thro_low); 
 
   //roll,pitch,yaw
@@ -530,8 +527,6 @@ void control_Angle(bool zero_integrators) {
   float pitch_des = rcin_pitch * maxPitch; //Between -maxPitch and +maxPitch
   float yawRate_des = rcin_yaw * maxYawRate; //Between -maxYawRate and +maxYawRate
 
-  //Serial.printf("r_des:%+.2f p_des:%+.2f y_des:%+.2f",roll_des,pitch_des,yawRate_des);
-
   //state vars
   static float integral_roll, integral_pitch, error_yaw_prev, integral_yaw;
 
@@ -752,7 +747,7 @@ Yaw right               (CCW+ CW-)       -++-
 }
 
 void out_KillSwitchAndFailsafe() {
-  static bool rcin_armed_prev = false; 
+  static bool rcin_armed_prev = true; //initial value is true: forces out_armed false on startup even if arm switch is ON
 
   //Change to ARMED when throttle is low and radio armed switch was flipped from disamed to armed position
   if (!out_armed && rcin_thro_is_low && rcin_armed && !rcin_armed_prev) {
@@ -772,7 +767,7 @@ void out_KillSwitchAndFailsafe() {
     }
   }
 
-  //If armed and throttle is low -> set motor outputs to approx 0.1 (out_armed_speed)
+  //If armed and throttle is low -> set motor outputs to out_armed_speed
   if(out_armed && rcin_thro_is_low) for(int i=0;i<out_MOTOR_COUNT;i++) out_command[i] = out_armed_speed; 
 
   //IF DISARMED -> STOP MOTORS
@@ -830,7 +825,7 @@ float lowpass_to_beta(float f0, float fs) {
 void warn_or_die(String msg, bool never_return) {
   bool do_print = true;
   do{
-    if(do_print) Serial.print(msg);
+    if(do_print) Serial.print(msg + "\n");
     for(int i=0;i<20;i++) {
       led.toggle();
       uint32_t ts = millis();
@@ -842,3 +837,16 @@ void warn_or_die(String msg, bool never_return) {
 }
 void die(String msg) { warn_or_die(msg, true); }
 void warn(String msg) { warn_or_die(msg, false); }
+
+//========================================================================================================================//
+//                  SETUP1() LOOP1() EXECUTING ON SECOND CORE                                                             //
+//========================================================================================================================//
+//Uncomment setup1() and/or loop1() to use the second core on ESP32 / RP2040 with FreeRTOS
+/*
+void setup1() {
+  Serial.println("setup1()");
+}
+void loop1() {
+  Serial.println("loop1()"); delay(100);
+}
+//*/
diff --git a/src/madflight.h b/src/madflight.h
index 7872bfd3..bf179953 100644
--- a/src/madflight.h
+++ b/src/madflight.h
@@ -1,4 +1,4 @@
-#define MADFLIGHT_VERSION "madflight v1.1.1"
+#define MADFLIGHT_VERSION "madflight v1.1.2-DEV"
 
 /*==========================================================================================
 madflight - Flight Controller for ESP32 / RP2040 / STM32
diff --git a/src/madflight/baro/BMP280.h b/src/madflight/baro/BMP280.h
index 05765ba4..40a7cf37 100644
--- a/src/madflight/baro/BMP280.h
+++ b/src/madflight/baro/BMP280.h
@@ -232,7 +232,6 @@ class Adafruit_BMP280 {
   uint16_t read16_LE(byte reg);
   int16_t readS16_LE(byte reg);
 
-  uint8_t _i2caddr;
 
   int32_t _sensorID = 0;
   int32_t t_fine;
diff --git a/src/madflight/baro/baro.h b/src/madflight/baro/baro.h
index 1d28cea0..ffe80d7d 100644
--- a/src/madflight/baro/baro.h
+++ b/src/madflight/baro/baro.h
@@ -8,19 +8,12 @@ Each BARO_USE_xxx section in this file defines a specific Barometer class
 #define BARO_USE_BMP280 2
 #define BARO_USE_MS5611 3
 
-#include "../interface.h"
-
-/* INTERFACE
-class Barometer {
+class BarometerSensor {
 public:
-  float press_pa = 0; //pressure in Pascal
-  float temp_c = 0; //temperature in Celcius
   virtual int setup() = 0;
-  virtual bool update() = 0; //returns true if pressure was updated
+  virtual bool update(float *press, float *temp) = 0; //returns true if pressure was updated
 };
 
-extern Barometer &baro;
-*/
 
 #ifndef BARO_I2C_ADR
   #define BARO_I2C_ADR 0
@@ -30,23 +23,22 @@ extern Barometer &baro;
 // None or undefined
 //=================================================================================================
 #if BARO_USE == BARO_USE_NONE || !defined BARO_USE
-class BarometerNone: public Barometer {
+class BarometerNone: public BarometerSensor {
 public:
-  //float press_pa = 0; //pressure in Pascal
-  //float temp_c = 0; //temperature in Celcius
-
   int setup() {
     Serial.println("BARO_USE_NONE");
     return 0;
   }
 
   //returns true if pressure was updated
-  bool update() {
+  bool update(float *press, float *temp) {
+    (void) press;
+    (void) temp;
     return false;
   }
 };
 
-BarometerNone baro_instance;
+BarometerNone baro_sensor;
 
 //=================================================================================================
 // BMP280
@@ -57,12 +49,9 @@ BarometerNone baro_instance;
 
 Adafruit_BMP280 baro_BMP280(i2c);
 
-class BarometerBMP280: public Barometer {
+class BarometerBMP280: public BarometerSensor {
 
 public:
-  //float press_pa = 0;
-  //float temp_c = 0;
-
   int setup() {
     Serial.println();
     unsigned status;
@@ -70,9 +59,9 @@ class BarometerBMP280: public Barometer {
     Serial.printf("BARO_USE_BMP280   BARO_I2C_ADR: 0x%02X  SensorID: 0x%02X\n", BARO_I2C_ADR, baro_BMP280.sensorID());
 
     if (!status) {
-      Serial.println(F("Could not find a valid BMP280 sensor, check wiring or "
-                        "try a different address!"));
-      Serial.print("SensorID was: 0x"); Serial.println(baro_BMP280.sensorID(),16);
+      Serial.println(F("Could not find a valid BMP280 sensor, check wiring or try a different address!"));
+      Serial.print("SensorID was: 0x");
+      Serial.println(baro_BMP280.sensorID(),16);
       Serial.print("        ID of 0xFF probably means a bad address, a BMP 180 or BMP 085\n");
       Serial.print("   ID of 0x56-0x58 represents a BMP 280,\n");
       Serial.print("        ID of 0x60 represents a BME 280.\n");
@@ -87,17 +76,17 @@ class BarometerBMP280: public Barometer {
     return status;
   }
 
-  bool update() {
+  bool update(float *press, float *temp) {
     //driver does not return whether data is fresh, return true if pressure changed
-    float pressure_pa_new = baro_BMP280.readPressure();
-    bool rv = (pressure_pa_new != press_pa);
-    press_pa = pressure_pa_new;
-    temp_c = baro_BMP280.readTemperature();
+    float press_new = baro_BMP280.readPressure();
+    bool rv = (press_new != *press);
+    *press = press_new;
+    *temp = baro_BMP280.readTemperature();
     return rv;
   }
 };
 
-BarometerBMP280 baro_instance;
+BarometerBMP280 baro_sensor;
 
 //=================================================================================================
 // MS5611
@@ -106,7 +95,7 @@ BarometerBMP280 baro_instance;
 
 #include "MS5611.h"
 
-class BarometerMS5611: public Barometer {
+class BarometerMS5611: public BarometerSensor {
 private: 
   MS5611 ms5611;
 
@@ -130,12 +119,12 @@ class BarometerMS5611: public Barometer {
     return 0;
   }
 
-  bool update() {
-      return (ms5611.getMeasurements(&press_pa, &temp_c) == 1); //ms5611.getMeasurements returns: 0=no update, 1=pressure updated, 2=temp updated
+  bool update(float *press, float *temp) {
+      return (ms5611.getMeasurements(press, temp) == 1); //ms5611.getMeasurements returns: 0=no update, 1=pressure updated, 2=temp updated
   }
 };
 
-BarometerMS5611 baro_instance;
+BarometerMS5611 baro_sensor;
 
 //=================================================================================================
 // Invalid value
@@ -144,4 +133,32 @@ BarometerMS5611 baro_instance;
   #error "invalid BARO_USE value"
 #endif
 
-Barometer &baro = baro_instance;
+
+//========================================================================================================================//
+// Barometer Class Implementation
+//========================================================================================================================//
+
+#include "../interface.h"
+
+int Barometer::setup(uint32_t sampleRate) {
+  _sampleRate = sampleRate;
+  _samplePeriod = 1000000 / sampleRate;
+  dt = 0;
+  ts = micros();
+  return baro_sensor.setup();
+}
+
+bool Barometer::update() {
+  if (micros() - ts >= _samplePeriod) {
+    baro_sensor.update(&press, &temp);
+    alt = (101325.0 - press) / 12.0;
+    uint32_t tsnew = micros();
+    dt = (tsnew - ts) / 1000000.0;
+    ts = tsnew;
+    return true;
+  }
+  return false;
+}
+
+//global Barometer class instance
+Barometer baro;
diff --git a/src/madflight/bb/bb.h b/src/madflight/bb/bb.h
index f03c870e..0e0df52a 100644
--- a/src/madflight/bb/bb.h
+++ b/src/madflight/bb/bb.h
@@ -89,8 +89,8 @@ class BlackBox {
     if(bbw.isBusy()) return;
     bbw.writeBeginRecord(BB_REC_BARO, "BARO");
     bbw.writeU("ts",micros());
-    bbw.writeU("baro_pa",baro.press_pa); //Barometer pressure (Pa)
-    bbw.writeI("baro_t*100",baro.temp_c*100); //barometer temp (C)
+    bbw.writeU("baro_pa",baro.press); //Barometer pressure (Pa)
+    bbw.writeI("baro_t*100",baro.temp*100); //barometer temp (C)
     bbw.writeEndrecord();
   }
 
diff --git a/src/madflight/cli/cli.h b/src/madflight/cli/cli.h
index a7aad7b5..a3e4f2f4 100644
--- a/src/madflight/cli/cli.h
+++ b/src/madflight/cli/cli.h
@@ -2,12 +2,124 @@
 
 #pragma once
 
-class CLI {
+void cli_print_overview() {
+  Serial.printf("CH%d:%d\t",1,rcin_pwm[0]);  
+  Serial.printf("rcin_roll:%+.2f\t",rcin_roll);
+  Serial.printf("gx:%+.2f\t",GyroX);
+  Serial.printf("ax:%+.2f\t",AccX);
+  Serial.printf("mx:%+.2f\t",MagX);
+  Serial.printf("ahrs_roll:%+.1f\t",ahrs_roll);
+  Serial.printf("roll_PID:%+.3f\t",roll_PID);  
+  Serial.printf("m%d%%:%1.0f\t", 1, 100*out_command[0]);
+  Serial.printf("sats:%d\t",(int)gps.sat);
+  Serial.printf("imu%%:%d\t",(int)(100 * imu.runtime_tot_max / imu.getSamplePeriod()));
+  Serial.printf("imu_cnt:%d\t",(int)imu.update_cnt);
+}
+
+void cli_print_rcin_RadioPWM() {
+  Serial.printf("rcin_con:%d\t",rcin.connected());
+  for(int i=0;i<RCIN_NUM_CHANNELS;i++) Serial.printf("pwm%d:%d\t",i+1,rcin_pwm[i]);
+}
+
+void cli_print_rcin_RadioScaled() {
+  Serial.printf("rcin_thro:%.2f\t",rcin_thro);
+  Serial.printf("rcin_roll:%+.2f\t",rcin_roll);
+  Serial.printf("rcin_pitch:%+.2f\t",rcin_pitch);
+  Serial.printf("rcin_yaw:%+.2f\t",rcin_yaw);
+  Serial.printf("rcin_arm:%d\t",rcin_armed);
+  Serial.printf("rcin_aux:%d\t",rcin_aux);
+  Serial.printf("out_armed:%d\t",out_armed);
+}
+
+void cli_print_imu_GyroData() {
+  Serial.printf("gx:%+.2f\tgy:%+.2f\tgz:%+.2f\t",GyroX,GyroY,GyroZ);
+}
+
+void cli_print_imu_AccData() {
+  Serial.printf("ax:%+.2f\tay:%+.2f\taz:%+.2f\t",AccX,AccY,AccZ);
+}
+
+void cli_print_imu_MagData() {
+  Serial.printf("mx:%+.2f\tmy:%+.2f\tmz:%+.2f\t",MagX,MagY,MagZ); 
+}
+
+void cli_print_ahrs_RollPitchYaw() {
+  Serial.printf("roll:%+.1f\tpitch:%+.1f\tyaw:%+.1f\t",ahrs_roll,ahrs_pitch,ahrs_yaw);
+  Serial.printf("yaw_mag:%+.1f\t",-atan2(MagY, MagX) * rad_to_deg);
+}
+
+void cli_print_control_PIDoutput() {
+  Serial.printf("roll_PID:%+.3f\tpitch_PID:%+.3f\tyaw_PID:%+.3f\t",roll_PID,pitch_PID,yaw_PID);
+}
+
+void cli_print_out_MotorCommands() {
+  Serial.printf("out_armed:%d\t", out_armed);
+  for(int i=0;i<out_MOTOR_COUNT;i++) Serial.printf("m%d%%:%1.0f\t", i+1, 100*out_command[i]);
+}
+
+void cli_print_out_ServoCommands() {
+  for(int i=out_MOTOR_COUNT;i<HW_OUT_COUNT;i++) Serial.printf("s%d%%:%1.0f\t", i-out_MOTOR_COUNT+1, 100*out_command[i]);
+}
+
+void cli_print_imu_Rate() {
+  static uint32_t update_cnt_last = 0;
+  Serial.printf("imu%%:%d\t",(int)(100 * imu.runtime_tot_max / imu.getSamplePeriod()));
+  Serial.printf("period:%d\t",(int)imu.getSamplePeriod());
+  Serial.printf("dt:%d\t",(int)(imu.dt * 1000000.0));
+  Serial.printf("rt:%d\t",(int)imu.runtime_tot_max);
+  Serial.printf("rt_int:%d\t",(int)imu.runtime_int);
+  Serial.printf("rt_bus:%d\t",(int)imu.runtime_bus);
+  Serial.printf("overruns:%d\t",(int)(imu.overrun_cnt));
+  Serial.printf("cnt:%d\t",(int)imu.update_cnt);
+  Serial.printf("loops:%d\t",(int)(imu.update_cnt - update_cnt_last));
+  update_cnt_last = imu.update_cnt;
+}
+
+void cli_print_bat() {
+  Serial.printf("bat.v:%.2f\t",bat.v);
+  Serial.printf("bat.i:%+.2f\t",bat.i);
+  Serial.printf("bat.mah:%+.2f\t",bat.mah);
+  Serial.printf("bat.wh:%+.2f\t",bat.wh); 
+}
+
+void cli_print_baro() {
+  Serial.printf("baro.alt:%.2f\t", baro.alt);
+  Serial.printf("baro.press:%.1f\t", baro.press);
+  Serial.printf("baro.temp:%.2f\t", baro.temp);
+}
+
+struct cli_print_s {
+  String cmd;
+  String info;
+  void (*function)(void);
+};
+
+
+#define CLI_PRINT_FLAG_COUNT 13
+bool cli_print_flag[CLI_PRINT_FLAG_COUNT] = {false};
+
+struct cli_print_s cli_print_options[] = {
+  {"po", "Overview: pwm1, rcin_roll, gyroX, accX, magX, ahrs_roll, pid_roll, motor1, imu%", cli_print_overview},
+  {"ppwm", "Radio pwm (expected: 1000 to 2000)", cli_print_rcin_RadioPWM},
+  {"pradio", "Scaled radio (expected: -1 to 1)", cli_print_rcin_RadioScaled},
+  {"pimu", "IMU loop timing (expected: imu%% < 50)", cli_print_imu_Rate},
+  {"pgyro", "Filtered gyro (expected: -250 to 250, 0 at rest)", cli_print_imu_GyroData},
+  {"pacc", "Filtered accelerometer (expected: -2 to 2; x,y 0 when level, z 1 when level)", cli_print_imu_AccData},
+  {"pmag", "Filtered magnetometer (expected: -300 to 300)", cli_print_imu_MagData},
+  {"proll", "AHRS roll, pitch, and yaw (expected: degrees, 0 when level)", cli_print_ahrs_RollPitchYaw},
+  {"ppid", "PID output (expected: -1 to 1)", cli_print_control_PIDoutput},
+  {"pmot", "Motor output (expected: 0 to 1)", cli_print_out_MotorCommands},
+  {"pservo", "Servo output (expected: 0 to 1)", cli_print_out_ServoCommands},
+  {"pbat", "Battery voltage, current, Ah used and Wh used", cli_print_bat},
+  {"pbaro", "Barometer", cli_print_baro},
+};
+
 
+class CLI {
 public:
 
   void setup() {
-    print_all(false);
+    cli_print_all(false);
   }
 
   //returns true if a command was processed (even an invalid one)
@@ -16,7 +128,7 @@ class CLI {
     bool rv = false;
     while(Serial.available()) {
       char c = Serial.read();
-      if( (c=='\r' && prev_c!='\n') || (c=='\n' && prev_c!='\n') ) { //accept \n, \r, \r\n, \n\r as end of command
+      if ( (c=='\r' && prev_c!='\n') || (c=='\n' && prev_c!='\n') ) { //accept \n, \r, \r\n, \n\r as end of command
         processCmd();
         rv = true;
       }else{
@@ -26,7 +138,7 @@ class CLI {
     }
     
     //handle output for pxxx commands
-    print_loop();
+    cli_print_loop();
     
     return rv;
   }
@@ -45,18 +157,15 @@ class CLI {
     "-- PRINT --\n"
     "poff      Printing off\n"
     "pall      Print all\n"
-    "po        Overview: pwm1, rcin_roll, gyroX, accX, magX, ahrs_roll, pid_roll, motor1, loop_rt\n"
-    "ppwm      Radio pwm (expected: 1000 to 2000)\n"
-    "pradio    Scaled radio (expected: -1 to 1)\n"
-    "pimu      IMU loop timing (expected: imu%% < 50)\n"
-    "pgyro     Filtered gyro (expected: -250 to 250, 0 at rest)\n"
-    "pacc      Filtered accelerometer (expected: -2 to 2; x,y 0 when level, z 1 when level)\n"
-    "pmag      Filtered magnetometer (expected: -300 to 300)\n"
-    "proll     AHRS roll, pitch, and yaw (expected: degrees, 0 when level)\n"
-    "ppid      PID output (expected: -1 to 1)\n"
-    "pmot      Motor output (expected: 0 to 1)\n"
-    "pservo    Servo output (expected: 0 to 1)\n"
-    "pbat      Battery voltage, current, Ah used and Wh used\n"
+    );
+    for(int i=0;i<CLI_PRINT_FLAG_COUNT;i++) {
+      Serial.print(cli_print_options[i].cmd);
+      for(int j=cli_print_options[i].cmd.length();j<9;j++) Serial.print(' ');
+      Serial.print(' ');
+      Serial.print(cli_print_options[i].info);
+      Serial.println();
+    }
+    Serial.printf(
     "-- BLACK BOX --\n"
     "bbls      List files\n"
     "bbdump n  Dump file n in CSV format\n"
@@ -96,76 +205,61 @@ class CLI {
     String arg2 = getCmdPart(pos);
     cmd.toLowerCase();
     cmd.trim();
+    cmdline = ""; //clear command line
 
     Serial.println( "> " + cmd + " " + arg1 + " " + arg2 );
 
-    if(cmd=="help" || cmd=="?") {
+    //process print commands
+    for (int i=0;i<CLI_PRINT_FLAG_COUNT;i++) {
+      if (cmd == cli_print_options[i].cmd) {
+        cli_print_flag[i] = !cli_print_flag[i];
+        return;
+      }
+    }
+
+    if (cmd=="help" || cmd=="?") {
       help();
-    }else if(cmd == "board") {
+    }else if (cmd == "board") {
       print_boardInfo();
-    }else if(cmd == "i2c") {
+    }else if (cmd == "i2c") {
       print_i2cScan();
-    }else if(cmd == "reboot") {
+    }else if (cmd == "reboot") {
       hw_reboot();
-    }else if(cmd == "poff") {
-      print_all(false);
-    }else if(cmd == "pall") {
-      print_all(true); 
-    }else if(cmd == "po") {
-      print_flag[0] = !print_flag[0];
-    }else if(cmd == "ppwm") {
-      print_flag[1] = !print_flag[1];
-    }else if(cmd == "pradio") {
-      print_flag[2] = !print_flag[2];
-    }else if(cmd == "pgyro") {
-      print_flag[3] = !print_flag[3];
-    }else if(cmd == "pacc") {
-      print_flag[4] = !print_flag[4];
-    }else if(cmd == "pmag") {
-      print_flag[5] = !print_flag[5];
-    }else if(cmd == "proll") {
-      print_flag[6] = !print_flag[6];
-    }else if(cmd == "ppid") {
-      print_flag[7] = !print_flag[7];
-    }else if(cmd == "pmot") {
-      print_flag[8] = !print_flag[8];
-    }else if(cmd == "pservo") {
-      print_flag[9] = !print_flag[9];
-    }else if(cmd == "pimu") {
-      print_flag[10] = !print_flag[10];
-    }else if(cmd == "pbat") {
-      print_flag[11] = !print_flag[11];
-    }else if(cmd == "bbls") {
+    }else if (cmd == "poff") {
+      cli_print_all(false);
+    }else if (cmd == "pall") {
+      cli_print_all(true);
+    }else if (cmd == "bbls") {
       bb.dir();
-    }else if(cmd == "bbdump") {
+    }else if (cmd == "bbdump") {
       bb.csvDump(arg1.toInt());
-    }else if(cmd == "bbstart") {
+    }else if (cmd == "bbstart") {
       bb.start();
-    }else if(cmd == "bbstop") {
+    }else if (cmd == "bbstop") {
       bb.stop();
-    }else if(cmd == "bberase") {
+    }else if (cmd == "bberase") {
       bb.erase();
-    }else if(cmd == "set") {
+    }else if (cmd == "set") {
       cfg.set(arg1, arg2);
-    }else if(cmd == "clist") {
+    }else if (cmd == "clist") {
       cfg.list();
-    }else if(cmd == "cclear") {
+    }else if (cmd == "cclear") {
       cfg.clear();
       Serial.println("Config cleared, use 'cwrite' to write to flash");
-    }else if(cmd == "cwrite") {
+    }else if (cmd == "cwrite") {
       Serial.println("writing, please wait... ");
       cfg.write();
       Serial.println("cwrite completed");
-    }else if(cmd == "cread") {
+    }else if (cmd == "cread") {
       cfg.read();
-    }else if(cmd == "calimu") {
+    }else if (cmd == "calimu") {
       calibrate_IMU();
-    }else if(cmd == "calmag") {
+    }else if (cmd == "calmag") {
       calibrate_Magnetometer();
-    }else if(cmd != "") {
+    }else if (cmd != "") {
       Serial.println("ERROR Unknown command - Type help for help");
     }
-    cmdline = "";
+
   }
 
 
@@ -205,7 +299,7 @@ class CLI {
         count++;
       }
     }
-    Serial.printf("I2C: Found %d device(s)\n", count);      
+    Serial.printf("I2C: Found %d device(s)\n", count);
   }
 
 //========================================================================================================================//
@@ -260,7 +354,7 @@ class CLI {
 
     bool apply_gyro = true;
     
-    if(gyro_only) {
+    if (gyro_only) {
       //only apply reasonable gyro errors
       float gtol = 10;
       apply_gyro = ( -gtol < gxerr && gxerr < gtol  &&  -gtol < gyerr && gyerr < gtol  &&  -gtol < gzerr && gzerr < gtol );
@@ -326,13 +420,13 @@ class CLI {
     float bias[3], scale[3];
 
 
-    if(mag.installed()) {
+    if (mag.installed()) {
       Serial.print("EXT ");
-    }else if(imu.hasMag()) {
+    }else if (imu.hasMag()) {
       Serial.print("IMU ");
     }
     Serial.println("Magnetometer calibration. Rotate the IMU about all axes until complete.");
-    if( _calibrate_Magnetometer(bias, scale) ) {
+    if ( _calibrate_Magnetometer(bias, scale) ) {
       Serial.println("Calibration Successful!");
       Serial.printf("set mag_cal_x  %+f #config %+f\n", bias[0], cfg.mag_cal_x);
       Serial.printf("set mag_cal_y  %+f #config %+f\n", bias[1], cfg.mag_cal_y);
@@ -359,7 +453,7 @@ class CLI {
 
   //get a reading from the external or imu magnetometer
   bool _calibrate_Magnetometer_ReadMag(float *m) {
-    if(mag.installed()) {
+    if (mag.installed()) {
       mag.update();
       m[0] = mag.x;
       m[1] = mag.y;
@@ -399,7 +493,7 @@ class CLI {
       delayMicroseconds(sample_interval);
       _calibrate_Magnetometer_ReadMag(m);
       delayMicroseconds(sample_interval);
-      if( abs(m[0] - mlast[0]) < 20 && abs(m[1] - mlast[1]) && abs(m[2] - mlast[2]) && m[0] != 0  && m[1] != 0 && m[2] != 0) break;
+      if ( abs(m[0] - mlast[0]) < 20 && abs(m[1] - mlast[1]) && abs(m[2] - mlast[2]) && m[0] != 0  && m[1] != 0 && m[2] != 0) break;
     }
     for(int i=0;i<3;i++) mlast[i] = m[i];
     
@@ -419,7 +513,7 @@ class CLI {
       while(micros() - sample_time < sample_interval); //sample at 100Hz
       sample_time = micros();
       _calibrate_Magnetometer_ReadMag(m);
-      if( abs(m[0] - mlast[0]) < 20 && abs(m[1] - mlast[1]) && abs(m[2] - mlast[2]) && m[0] != 0  && m[1] != 0 && m[2] != 0) {
+      if ( abs(m[0] - mlast[0]) < 20 && abs(m[1] - mlast[1]) && abs(m[2] - mlast[2]) && m[0] != 0  && m[1] != 0 && m[2] != 0) {
         for(int i=0;i<3;i++) mlast[i] = m[i];
         for(int i=0;i<3;i++) {
           m_filt[i] = m_filt[i] * (1 - B_coeff) + m[i] * B_coeff;
@@ -438,7 +532,7 @@ class CLI {
       }
       
       //print progress
-      if(millis() - start_time > 1000) {
+      if (millis() - start_time > 1000) {
         start_time = millis();
         Serial.printf("cnt:%d\txmin:%+.2f\txmax:%+.2f\tymin:%+.2f\tymax:%+.2f\tzmin:%+.2f\tzmax:%+.2f\n", counter, m_min[0], m_max[0], m_min[1], m_max[1], m_min[2], m_max[2]);
       }
@@ -463,135 +557,33 @@ class CLI {
 //                                                PRINT FUNCTIONS                                                         //
 //========================================================================================================================//
 
-public:
+private:
+
+  uint32_t cli_print_time = 0;
+
 
-  uint32_t print_time = 0;
-  bool print_need_newline = false;
-#define CLI_PRINT_FLAG_COUNT 12
-  bool print_flag[CLI_PRINT_FLAG_COUNT];
 
-  void print_all(bool val) {
-    for(int i=0;i<CLI_PRINT_FLAG_COUNT;i++) print_flag[i] = val;
+  void cli_print_all(bool val) {
+    for(int i=0;i<CLI_PRINT_FLAG_COUNT;i++) cli_print_flag[i] = val;
   }
 
-  void print_loop() {
-    //Debugging - Print data at print_interval microseconds, uncomment line(s) for troubleshooting
-    uint32_t print_interval = 100000;
-    if (micros() - print_time > print_interval) {
-      print_time = micros();
-      print_need_newline = false;
+  void cli_print_loop() {
+    uint32_t cli_print_interval = 100000; //Print data at cli_print_interval microseconds
+    if (micros() - cli_print_time > cli_print_interval) {
+      cli_print_time = micros();
+      bool cli_print_need_newline = false;
       //Serial.printf("loop_time:%d\t",loop_time); //print loop time stamp
-      if(print_flag[0])  print_overview(); //prints: pwm1, rcin_roll, gyroX, accX, magX, ahrs_roll, pid_roll, motor1, imu%
-      if(print_flag[1])  print_rcin_RadioPWM();     //Prints radio pwm values (expected: 1000 to 2000)
-      if(print_flag[2])  print_rcin_RadioScaled();     //Prints scaled radio values (expected: -1 to 1)
-      if(print_flag[10]) print_imu_Rate();      //Prints imu loop rate (expected: imu% < 50)
-      if(print_flag[3])  print_imu_GyroData();      //Prints filtered gyro data direct from IMU (expected: -250 to 250, 0 at rest)
-      if(print_flag[4])  print_imu_AccData();     //Prints filtered accelerometer data direct from IMU (expected: -2 to 2; x,y 0 when level, z 1 when level)
-      if(print_flag[5])  print_imu_MagData();       //Prints filtered magnetometer data direct from IMU (expected: -300 to 300)
-      if(print_flag[6])  print_ahrs_RollPitchYaw();  //Prints roll, pitch, and yaw angles in degrees from ahrs_Madgwick filter (expected: degrees, 0 when level)
-      if(print_flag[7])  print_control_PIDoutput();     //Prints computed stabilized PID variables from controller and desired setpoint (expected: ~ -1 to 1)
-      if(print_flag[8])  print_out_MotorCommands(); //Prints the values being written to the motors (expected: 0 to 1)
-      if(print_flag[9])  print_out_ServoCommands(); //Prints the values being written to the servos (expected: 0 to 1)
-      if(print_flag[11]) print_bat(); //Prints battery voltage, current, Ah used and Wh used
-      //Serial.printf("press:%.1f\ttemp:%.2f\t",baro.press_pa, baro.temp_c); //Prints barometer data
-      if(print_need_newline) Serial.println();
+      for (int i=0;i<CLI_PRINT_FLAG_COUNT;i++) {
+        if (cli_print_flag[i]) {
+          cli_print_options[i].function();
+          cli_print_need_newline = true;
+        }
+      }
+      if (cli_print_need_newline) Serial.println();
       imu.runtime_tot_max = 0; //reset maximum runtime
     }
   }
 
-  void print_overview() {
-    Serial.printf("CH%d:%d\t",1,rcin_pwm[0]);  
-    Serial.printf("rcin_roll:%+.2f\t",rcin_roll);
-    Serial.printf("gx:%+.2f\t",GyroX);
-    Serial.printf("ax:%+.2f\t",AccX);
-    Serial.printf("mx:%+.2f\t",MagX);
-    Serial.printf("ahrs_roll:%+.1f\t",ahrs_roll);
-    Serial.printf("roll_PID:%+.3f\t",roll_PID);  
-    Serial.printf("m%d%%:%1.0f\t", 1, 100*out_command[0]);
-    Serial.printf("sats:%d\t",(int)gps.sat);
-    Serial.printf("imu%%:%d\t",(int)(100 * imu.runtime_tot_max / imu.getSamplePeriod()));
-    Serial.printf("imu_cnt:%d\t",(int)imu.update_cnt);
-    print_need_newline = true;
-  }
-
-  void print_rcin_RadioPWM() {
-    Serial.printf("rcin_con:%d\t",rcin.connected());
-    for(int i=0;i<RCIN_NUM_CHANNELS;i++) Serial.printf("pwm%d:%d\t",i+1,rcin_pwm[i]);
-    print_need_newline = true;
-  }
-
-  void print_rcin_RadioScaled() {
-    Serial.printf("rcin_thro:%.2f\t",rcin_thro);
-    Serial.printf("rcin_roll:%+.2f\t",rcin_roll);
-    Serial.printf("rcin_pitch:%+.2f\t",rcin_pitch);
-    Serial.printf("rcin_yaw:%+.2f\t",rcin_yaw);
-    Serial.printf("rcin_arm:%d\t",rcin_armed);
-    Serial.printf("rcin_aux:%d\t",rcin_aux);
-    Serial.printf("out_armed:%d\t",out_armed);
-    print_need_newline = true;
-  }
-
-  void print_imu_GyroData() {
-    Serial.printf("gx:%+.2f\tgy:%+.2f\tgz:%+.2f\t",GyroX,GyroY,GyroZ);
-    print_need_newline = true;
-  }
-
-  void print_imu_AccData() {
-    Serial.printf("ax:%+.2f\tay:%+.2f\taz:%+.2f\t",AccX,AccY,AccZ);
-    print_need_newline = true;
-  }
-
-  void print_imu_MagData() {
-    Serial.printf("mx:%+.2f\tmy:%+.2f\tmz:%+.2f\t",MagX,MagY,MagZ);
-    print_need_newline = true;  
-  }
-
-  void print_ahrs_RollPitchYaw() {
-    Serial.printf("roll:%+.1f\tpitch:%+.1f\tyaw:%+.1f\t",ahrs_roll,ahrs_pitch,ahrs_yaw);
-    Serial.printf("yaw_mag:%+.1f\t",-atan2(MagY, MagX) * rad_to_deg);
-    print_need_newline = true;
-  }
-
-  void print_control_PIDoutput() {
-    Serial.printf("roll_PID:%+.3f\tpitch_PID:%+.3f\tyaw_PID:%+.3f\t",roll_PID,pitch_PID,yaw_PID);  
-    print_need_newline = true;
-  }
-
-  void print_out_MotorCommands() {
-    Serial.printf("out_armed:%d", out_armed);  
-    for(int i=0;i<out_MOTOR_COUNT;i++) Serial.printf("m%d%%:%1.0f\t", i+1, 100*out_command[i]);
-    print_need_newline = true;    
-  }
-
-  void print_out_ServoCommands() {
-    for(int i=out_MOTOR_COUNT;i<HW_OUT_COUNT;i++) Serial.printf("s%d%%:%1.0f\t", i-out_MOTOR_COUNT+1, 100*out_command[i]);
-    print_need_newline = true;  
-  }
-
-  void print_imu_Rate() {
-    static uint32_t update_cnt_last = 0;
-    Serial.printf("imu%%:%d\t",(int)(100 * imu.runtime_tot_max / imu.getSamplePeriod()));
-    Serial.printf("period:%d\t",(int)imu.getSamplePeriod());
-    Serial.printf("dt:%d\t",(int)(imu.dt * 1000000.0));
-    Serial.printf("rt:%d\t",(int)imu.runtime_tot_max);
-    Serial.printf("rt_int:%d\t",(int)imu.runtime_int);
-    Serial.printf("rt_bus:%d\t",(int)imu.runtime_bus);
-    Serial.printf("overruns:%d\t",(int)(imu.overrun_cnt));
-    Serial.printf("cnt:%d\t",(int)imu.update_cnt);
-    Serial.printf("loops:%d\t",(int)(imu.update_cnt - update_cnt_last));
-    update_cnt_last = imu.update_cnt;
-    print_need_newline = true;
-  }
-
-  void print_bat() {
-    Serial.printf("bat.v:%.2f\t",bat.v);
-    Serial.printf("bat.i:%+.2f\t",bat.i);
-    Serial.printf("bat.mah:%+.2f\t",bat.mah);
-    Serial.printf("bat.wh:%+.2f\t",bat.wh); 
-    print_need_newline = true;  
-  }
-
-
 };
 
 
diff --git a/src/madflight/hw_RP2040/hw_RP2040.h b/src/madflight/hw_RP2040/hw_RP2040.h
index 414b1499..8b245f03 100644
--- a/src/madflight/hw_RP2040/hw_RP2040.h
+++ b/src/madflight/hw_RP2040/hw_RP2040.h
@@ -43,7 +43,15 @@ void hw_eeprom_commit() {
 //                    hw_setup()
 //======================================================================================================================//
 
-#define HW_USE_FREERTOS //RP2040 optionally uses FreeRTOS
+//overclocking, supposedly works up to 270 MHz.
+//#ifndef HW_RP2040_SYS_CLK_KHZ
+//  #define HW_RP2040_SYS_CLK_KHZ 200000
+//#endif
+
+#ifdef HW_RP2040_USE_FREERTOS
+  #define HW_USE_FREERTOS //RP2040 optionally uses FreeRTOS
+#endif
+
 #define HW_RTOS_IMUTASK_PRIORITY 7 //IMU Interrupt task priority, higher number is higher priority. Max priority on RP2040 is 7
 
 #ifdef HW_USE_FREERTOS
@@ -55,8 +63,9 @@ void hw_eeprom_commit() {
 
 void hw_setup() 
 { 
-  //Uncomment for overclocking, supposedly works up to 270 MHz.
-  //set_sys_clock_khz(200000, true); 
+#ifdef HW_RP2040_SYS_CLK_KHZ
+  set_sys_clock_khz(HW_RP2040_SYS_CLK_KHZ, true); 
+#endif
 
   Serial.println("USE_HW_RP2040");
   
diff --git a/src/madflight/imu/imu.h b/src/madflight/imu/imu.h
index c094a66a..af0b9e7b 100644
--- a/src/madflight/imu/imu.h
+++ b/src/madflight/imu/imu.h
@@ -28,30 +28,6 @@ configures gyro and accel with 1000 Hz sample rate (with on sensor 200 Hz low pa
 #define IMU_USE_I2C_MPU6050 8
 #define IMU_USE_I2C_MPU6000 9
 
-#include "../interface.h"
-
-/* INTERFACE
-class Imu {
-  public:
-    float ax = 0; //"North" acceleration in G
-    float ay = 0; //"East" acceleration in G
-    float az = 0; //"Down" acceleration in G
-    float gx = 0; //"North" rotation speed in deg/s
-    float gy = 0; //"East" rotation speed in deg/s
-    float gz = 0; //"Down" rotation speed in deg/s
-    float mx = 0; //"North" magnetic flux in uT
-    float my = 0; //"East" magnetic flux in uT
-    float mz = 0; //"Down" magnetic flux in uT
-    virtual bool hasMag() = 0; //returns true if IMU has a magnetometer
-    virtual int setup(uint32_t sampleRate) = 0;
-    virtual void update() = 0;
-    uint32_t getSampleRate() {return _sampleRate;}
-  protected:
-    uint32_t _sampleRate = 0; //sensor sample rate in Hz
-};
-
-extern Imu &imu;
-*/
 
 //Available aligns
 #define IMU_ALIGN_CW0 1
@@ -96,14 +72,14 @@ extern Imu &imu;
 
 #if IMU_USE == IMU_USE_SPI_BMI270
   #define IMU_TYPE "IMU_USE_SPI_BMI270"
-  #define IMU_USE_BUS_SPI
+  #define IMU_IS_I2C 0
   #define IMU_HAS_MAG 0
   #include "BMI270/BMI270.h"
   BMI270 imu_Sensor(spi, HW_PIN_IMU_CS);
 
 #elif IMU_USE == IMU_USE_SPI_MPU9250
   #define IMU_TYPE "IMU_USE_SPI_MPU9250"
-  #define IMU_USE_BUS_SPI
+  #define IMU_IS_I2C 0
   #define IMU_HAS_MAG 1
   #include "MPUxxxx/MPU_interface.h"
   #include "MPUxxxx/MPUxxxx.h"
@@ -112,7 +88,7 @@ extern Imu &imu;
 
 #elif IMU_USE == IMU_USE_SPI_MPU6500
   #define IMU_TYPE "IMU_USE_SPI_MPU6500"
-  #define IMU_USE_BUS_SPI
+  #define IMU_IS_I2C 0
   #define IMU_HAS_MAG 0
   #include "MPUxxxx/MPU_interface.h"
   #include "MPUxxxx/MPUxxxx.h"
@@ -121,7 +97,7 @@ extern Imu &imu;
 
 #elif IMU_USE == IMU_USE_SPI_MPU6000
   #define IMU_TYPE "IMU_USE_SPI_MPU6000"
-  #define IMU_USE_BUS_SPI
+  #define IMU_IS_I2C 0
   #define IMU_HAS_MAG 0
   #include "MPUxxxx/MPU_interface.h"
   #include "MPUxxxx/MPUxxxx.h"
@@ -130,7 +106,7 @@ extern Imu &imu;
 
 #elif IMU_USE == IMU_USE_I2C_MPU9250
   #define IMU_TYPE "IMU_USE_I2C_MPU9250"
-  #define IMU_USE_BUS_I2C
+  #define IMU_IS_I2C 1
   #define IMU_HAS_MAG 1
   #include "MPUxxxx/MPU_interface.h"
   #include "MPUxxxx/MPUxxxx.h"
@@ -139,7 +115,7 @@ extern Imu &imu;
 
 #elif IMU_USE == IMU_USE_I2C_MPU9150
   #define IMU_TYPE "IMU_USE_I2C_MPU9150"
-  #define IMU_USE_BUS_I2C
+  #define IMU_IS_I2C 1
   #define IMU_HAS_MAG 1
   #include "MPUxxxx/MPU_interface.h"
   #include "MPUxxxx/MPUxxxx.h"
@@ -148,7 +124,7 @@ extern Imu &imu;
 
 #elif IMU_USE == IMU_USE_I2C_MPU6500
   #define IMU_TYPE "IMU_USE_I2C_MPU6500"
-  #define IMU_USE_BUS_I2C
+  #define IMU_IS_I2C 1
   #define IMU_HAS_MAG 0
   #include "MPUxxxx/MPU_interface.h"
   #include "MPUxxxx/MPUxxxx.h"
@@ -157,7 +133,7 @@ extern Imu &imu;
 
 #elif IMU_USE == IMU_USE_I2C_MPU6050
   #define IMU_TYPE "IMU_USE_I2C_MPU6050"
-  #define IMU_USE_BUS_I2C
+  #define IMU_IS_I2C 1
   #define IMU_HAS_MAG 0
   #include "MPUxxxx/MPU_interface.h"
   #include "MPUxxxx/MPUxxxx.h"
@@ -166,7 +142,7 @@ extern Imu &imu;
 
 #elif IMU_USE == IMU_USE_I2C_MPU6000
   #define IMU_TYPE "IMU_USE_I2C_MPU6000"
-  #define IMU_USE_BUS_I2C
+  #define IMU_IS_I2C 1
   #define IMU_HAS_MAG 0
   MPU_InterfaceI2C<HW_WIRETYPE> mpu_iface(i2c, IMU_I2C_ADR);
   MPUXXXX imu_Sensor(MPUXXXX::MPU6000, &mpu_iface);
@@ -184,12 +160,15 @@ extern Imu &imu;
 // Imu Class Implementation
 //========================================================================================================================//
 
+#include "../interface.h" //Imu class declaration
+
 void _imu_ll_interrupt_setup(); //prototype
 volatile bool _imu_ll_interrupt_busy = false;
 volatile uint32_t _imu_ll_interrupt_ts = 0;
 
+bool Imu::usesI2C() { return IMU_IS_I2C; } //returns true if IMU uses I2C bus (not SPI bus)
 bool Imu::hasMag() { return IMU_HAS_MAG; }
-  
+ 
 //returns 0 on success, positive on error, negative on warning
 int Imu::setup(uint32_t sampleRate) {
   int rv = imu_Sensor.begin(IMU_GYRO_DPS, IMU_ACCEL_G, sampleRate);
diff --git a/src/madflight/interface.h b/src/madflight/interface.h
index 812e5d76..666996ca 100644
--- a/src/madflight/interface.h
+++ b/src/madflight/interface.h
@@ -55,6 +55,7 @@ class Imu {
     int setup(uint32_t sampleRate);
     bool waitNewSample(); //wait for new sample, returns false on fail
     bool hasMag(); //returns true if IMU has a magnetometer
+    bool usesI2C(); //returns true if IMU uses I2C bus (not SPI bus)    
     uint32_t getSampleRate() {return _sampleRate;}  //sensor sample rate in Hz
     uint32_t getSamplePeriod() {return (_sampleRate != 0 ? 1000000 / _sampleRate : 1000000);} //sensor sample period in us
 
@@ -73,13 +74,24 @@ extern Imu imu;
 
 class Barometer {
   public:
-    float press_pa = 0; //pressure in Pascal
-    float temp_c = 0; //temperature in Celcius
-    virtual int setup() = 0;
-    virtual bool update() = 0; //returns true if pressure was updated
+    //sample data
+    uint32_t ts = 0; //sample timestamp in us
+    float dt = 0; //time since last sample in seconds
+    float press = 0; //pressure in Pascal
+    float alt = 0; // Approximate International Standard Atmosphere (ISA) Altitude in meter
+    float temp = 0; //temperature in Celcius
+
+    int setup(uint32_t sampleRate);
+    bool update(); //returns true if pressure was updated
+    uint32_t getSampleRate() {return _sampleRate;}  //sensor sample rate in Hz
+    uint32_t getSamplePeriod() {return _samplePeriod;} //sensor sample period in us
+
+  protected:
+    uint32_t _sampleRate = 0; //sensor sample rate in Hz
+    uint32_t _samplePeriod = 0; //sensor sample period in us
 };
 
-extern Barometer &baro;
+extern Barometer baro;
 
 //=================================================================================================
 // Magnetometer
diff --git a/src/madflight/rcin/rcin.h b/src/madflight/rcin/rcin.h
index ce43a8b4..7d331ce4 100644
--- a/src/madflight/rcin/rcin.h
+++ b/src/madflight/rcin/rcin.h
@@ -11,6 +11,7 @@ rcin_GetPWM(int *pwm) -> fills pwm[0..RCIN_NUM_CHANNELS-1] received PWM values,
 #define RCIN_USE_DSM 3
 #define RCIN_USE_PPM 4
 #define RCIN_USE_PWM 5
+#define RCIN_USE_DEBUG 6
 
 
 #define RCIN_TIMEOUT 3000 // lost connection timeout in milliseconds
@@ -79,7 +80,6 @@ class RcinCSRF : public Rcin {
           rv = true;
         }
       }
-
       return rv;
     }
 };
@@ -368,6 +368,27 @@ class RcinPWM : public Rcin {
 
 RcinPWM rcin_instance;
 
+//=================================================================================================
+// DEBUG Receiver
+//=================================================================================================
+#elif RCIN_USE == RCIN_USE_DEBUG
+class RcinDebug : public Rcin {
+  public:
+    void setup() {
+      Serial.printf("RCIN_USE_DEBUG\n");
+      pwm = pwm_instance;
+    };
+  private:
+    bool _update() { //returns true if channel pwm data was updated
+      for(int i=0;i<RCIN_NUM_CHANNELS;i++) pwm[i]=1500;
+      return true;
+    }
+  private:
+    uint16_t pwm_instance[RCIN_NUM_CHANNELS];
+};
+
+RcinDebug rcin_instance;
+
 //=================================================================================================
 // Invalid value
 //=================================================================================================