add CLI diff, fix ICM42688 NED frame

qqqlab · qqqlab · commit 7763d18caf93 · 2025-04-21T15:44:03.000+02:00
diff --git a/examples/00.HelloWorld/madflight_config.h b/examples/00.HelloWorld/madflight_config.h
@@ -17,13 +17,13 @@ You have 3 options to setup the flight controller:
      "#include <madflight_zzz_MTKS-MATEKH743.h>". See library/madflight/src for all available boards. Edit madflight_config
      to fine-tune the configuration.
  
-  4) Bare Metal - Remove "#include <madflight_board.h>", and set the full configuration in madflight_config below.
+  3) Bare Metal - Remove "#include <madflight_board.h>", and set the full configuration in madflight_config below.
 
 Pins and spi/i2c/serial busses use zero-based numbering, i.e. "gps_ser_bus 0" connects the GPS to the first serial bus
 with pins pin_ser0_tx and pin_ser0_rx. Pins use GPIO numbers, not physical pin numbers. Use -1 to disable a pin or bus.
 
 You can also modify the configuration from the CLI, for example "set imu_gizmo MPU6500" or "set imu_spi_bus 1", then
-use "cwrite" to save the config to eeprom and reboot to use the new config.
+use "save" to save the config to eeprom and reboot to use the new config.
 
 If things do not work as expected, have a good look at the startup messages!
 
@@ -69,6 +69,8 @@ const char madflight_config[] = R""(
 //bat_i2c_bus   -1
 //pin_bat_i     -1
 //pin_bat_v     -1
+//bat_cal_v      1 //adc voltage scale, value is: actual_voltage_in_v / adc_reading
+//bat_cal_i,     1 //adc current scale, value is: actual_current_in_a / adc_reading; for ina226/228: rshunt value in ohm
 
 //--- GPS ---
 //gps_gizmo      NONE  // options: NONE, UBLOX
diff --git a/examples/01.Quadcopter/01.Quadcopter.ino b/examples/01.Quadcopter/01.Quadcopter.ino
@@ -54,7 +54,7 @@ const float armed_min_throttle = 0.20; //Minimum throttle when armed, set to a v
 
 //Flight Mode: Uncommment only one
 #define FLIGHTMODE_RATE   //control rate - stick centered will keep current roll/pitch angle
-//#define FLIGHTMODE_ANGLE  //control angle - stick centered will return to horizontal - IMPORTANT: execute CLI 'calimu' and 'cwrite' before using this!!!
+//#define FLIGHTMODE_ANGLE  //control angle - stick centered will return to horizontal - IMPORTANT: execute CLI 'calimu' and 'save' before using this!!!
 
 //Controller parameters (take note of defaults before modifying!): 
 const float i_limit        = 25.0;      //Integrator saturation level, mostly for safety (default 25.0)
diff --git a/examples/01.Quadcopter/madflight_config.h b/examples/01.Quadcopter/madflight_config.h
@@ -17,13 +17,13 @@ You have 3 options to setup the flight controller:
      "#include <madflight_zzz_MTKS-MATEKH743.h>". See library/madflight/src for all available boards. Edit madflight_config
      to fine-tune the configuration.
  
-  4) Bare Metal - Remove "#include <madflight_board.h>", and set the full configuration in madflight_config below.
+  3) Bare Metal - Remove "#include <madflight_board.h>", and set the full configuration in madflight_config below.
 
 Pins and spi/i2c/serial busses use zero-based numbering, i.e. "gps_ser_bus 0" connects the GPS to the first serial bus
 with pins pin_ser0_tx and pin_ser0_rx. Pins use GPIO numbers, not physical pin numbers. Use -1 to disable a pin or bus.
 
 You can also modify the configuration from the CLI, for example "set imu_gizmo MPU6500" or "set imu_spi_bus 1", then
-use "cwrite" to save the config to eeprom and reboot to use the new config.
+use "save" to save the config to eeprom and reboot to use the new config.
 
 If things do not work as expected, have a good look at the startup messages!
 
@@ -69,6 +69,8 @@ const char madflight_config[] = R""(
 //bat_i2c_bus   -1
 //pin_bat_i     -1
 //pin_bat_v     -1
+//bat_cal_v      1 //adc voltage scale, value is: actual_voltage_in_v / adc_reading
+//bat_cal_i,     1 //adc current scale, value is: actual_current_in_a / adc_reading; for ina226/228: rshunt value in ohm
 
 //--- GPS ---
 //gps_gizmo      NONE  // options: NONE, UBLOX
diff --git a/examples/02.QuadcopterAdvanced/madflight_config.h b/examples/02.QuadcopterAdvanced/madflight_config.h
@@ -17,13 +17,13 @@ You have 3 options to setup the flight controller:
      "#include <madflight_zzz_MTKS-MATEKH743.h>". See library/madflight/src for all available boards. Edit madflight_config
      to fine-tune the configuration.
  
-  4) Bare Metal - Remove "#include <madflight_board.h>", and set the full configuration in madflight_config below.
+  3) Bare Metal - Remove "#include <madflight_board.h>", and set the full configuration in madflight_config below.
 
 Pins and spi/i2c/serial busses use zero-based numbering, i.e. "gps_ser_bus 0" connects the GPS to the first serial bus
 with pins pin_ser0_tx and pin_ser0_rx. Pins use GPIO numbers, not physical pin numbers. Use -1 to disable a pin or bus.
 
 You can also modify the configuration from the CLI, for example "set imu_gizmo MPU6500" or "set imu_spi_bus 1", then
-use "cwrite" to save the config to eeprom and reboot to use the new config.
+use "save" to save the config to eeprom and reboot to use the new config.
 
 If things do not work as expected, have a good look at the startup messages!
 
@@ -69,6 +69,8 @@ const char madflight_config[] = R""(
 //bat_i2c_bus   -1
 //pin_bat_i     -1
 //pin_bat_v     -1
+//bat_cal_v      1 //adc voltage scale, value is: actual_voltage_in_v / adc_reading
+//bat_cal_i,     1 //adc current scale, value is: actual_current_in_a / adc_reading; for ina226/228: rshunt value in ohm
 
 //--- GPS ---
 //gps_gizmo      NONE  // options: NONE, UBLOX
diff --git a/examples/03.Plane/madflight_config.h b/examples/03.Plane/madflight_config.h
@@ -17,13 +17,13 @@ You have 3 options to setup the flight controller:
      "#include <madflight_zzz_MTKS-MATEKH743.h>". See library/madflight/src for all available boards. Edit madflight_config
      to fine-tune the configuration.
  
-  4) Bare Metal - Remove "#include <madflight_board.h>", and set the full configuration in madflight_config below.
+  3) Bare Metal - Remove "#include <madflight_board.h>", and set the full configuration in madflight_config below.
 
 Pins and spi/i2c/serial busses use zero-based numbering, i.e. "gps_ser_bus 0" connects the GPS to the first serial bus
 with pins pin_ser0_tx and pin_ser0_rx. Pins use GPIO numbers, not physical pin numbers. Use -1 to disable a pin or bus.
 
 You can also modify the configuration from the CLI, for example "set imu_gizmo MPU6500" or "set imu_spi_bus 1", then
-use "cwrite" to save the config to eeprom and reboot to use the new config.
+use "save" to save the config to eeprom and reboot to use the new config.
 
 If things do not work as expected, have a good look at the startup messages!
 
@@ -69,6 +69,8 @@ const char madflight_config[] = R""(
 //bat_i2c_bus   -1
 //pin_bat_i     -1
 //pin_bat_v     -1
+//bat_cal_v      1 //adc voltage scale, value is: actual_voltage_in_v / adc_reading
+//bat_cal_i,     1 //adc current scale, value is: actual_current_in_a / adc_reading; for ina226/228: rshunt value in ohm
 
 //--- GPS ---
 //gps_gizmo      NONE  // options: NONE, UBLOX
diff --git a/src/madflight.h b/src/madflight.h
@@ -1,4 +1,4 @@
-#define MADFLIGHT_VERSION "madflight v2.0.1"
+#define MADFLIGHT_VERSION "madflight v2.0.2-DEV"
 
 //madflight.h - Flight Controller for ESP32 / ESP32-S3 / RP2350 / RP2040 / STM32
 
diff --git a/src/madflight/cfg/cfg.cpp b/src/madflight/cfg/cfg.cpp
@@ -57,13 +57,20 @@ bool CfgClass::getNameAndValue(uint16_t index, String* name, float* value) {
 float CfgClass::getValue(String namestr, float default_value) {
   int i = getIndex(namestr);
   if(i<0) return default_value;
+  return getValue(i);
+}
+
+//get parameter value as float
+float CfgClass::getValue(int i) {
+  if(i<0 || i>paramCount()) return 0;
   if(Cfg::param_list[i].type == 'f') {
     return param_float[i];
   }else{
-    return  param_int32_t[i];
+    return param_int32_t[i];
   }
 }
 
+
 //print enum option name for parameter pointer
 void CfgClass::printParamOption(const int32_t* param) {
   uint32_t i = ( (uint32_t)param - (uint32_t)(param_int32_t) ) / 4;
@@ -171,15 +178,40 @@ void CfgClass::printValue(uint16_t i) {
   }
 }
 
-//CLI print all config values
-void CfgClass::list(const char* filter) {
-  for(int i=0;i<paramCount();i++) {
+static int _cfg_param_list_name_compare(const void *a, const void *b) {
+  uint16_t i = *(uint16_t*)a;
+  uint16_t j = *(uint16_t*)b;
+  return strcmp( Cfg::param_list[i].name, Cfg::param_list[j].name );
+}
+
+//CLI dump: print all config values, sorted by name
+void CfgClass::cli_dump(const char* filter) {
+  uint16_t arr[paramCount()];
+  for(int i=0; i<paramCount(); i++) arr[i] = i;
+  qsort(arr, paramCount(), 2, _cfg_param_list_name_compare);
+  for(int j=0; j<paramCount(); j++) {
+    uint16_t i = arr[j];
     if(strstr(Cfg::param_list[i].name, filter)) {
       printNameAndValue(i);
     }
   }
 }
 
+//CLI diff: print all modified config values, sorted by name
+void CfgClass::cli_diff(const char* filter) {
+  CfgClass cfgdefault;
+  uint16_t arr[paramCount()];
+  for(int i=0; i<paramCount(); i++) arr[i] = i;
+  qsort(arr, paramCount(), 2, _cfg_param_list_name_compare);
+  for(int j=0; j<paramCount(); j++) {
+    uint16_t i = arr[j];
+    if(strstr(Cfg::param_list[i].name, filter) && getValue(i) != cfgdefault.getValue(i)) {
+      printNameAndValue(i);
+    }
+  }
+}
+
+
 /* non-sorted version
 void CfgClass::printPins() {
   for(int i=0;i<paramCount();i++) {
@@ -280,9 +312,11 @@ int CfgClass::getIndex(String namestr) {
 
 //load defaults
 void CfgClass::clear() {
-  CfgParam cfg2;
+  CfgParam cfg_clear;
   CfgParam *param = this;
-  memcpy(param, &cfg2, sizeof(CfgParam));
+  memcpy(param, &cfg_clear, sizeof(CfgParam));
+  CfgHeader hdr_clear;
+  memcpy(&hdr, &hdr_clear, sizeof(CfgHeader));
 }
 
 //read parameters from eeprom/flash
diff --git a/src/madflight/cfg/cfg.h b/src/madflight/cfg/cfg.h
@@ -186,7 +186,7 @@ SOFTWARE.
   MF_PARAM( bar_gizmo,         0, int32_t, 'e', mf_NONE,mf_BMP280,mf_BMP388,mf_BMP390,mf_MS5611,mf_HP203B) \
   MF_PARAM( bar_i2c_bus,      -1, int32_t, 'i') \
   MF_PARAM( bar_i2c_adr,       0, int32_t, 'i') \
-  MF_PARAM( bar_rate,       1000, float, 'f') /*Barometer sample rate in Hz (default 100)*/ \
+  MF_PARAM( bar_rate,        100, float, 'f') /*Barometer sample rate in Hz (default 100)*/ \
 \
   /*BAT - Battery Monitor*/ \
   MF_PARAM( bat_gizmo,         0, int32_t, 'e', mf_NONE,mf_ADC,mf_INA226,mf_INA228) \
@@ -283,11 +283,16 @@ struct CfgParam {
 
 
 
+
+
 #define CFG_HDR0 'm'
 #define CFG_HDR1 'a'
 #define CFG_HDR2 'd'
 #define CFG_HDR3 '2'
 
+
+
+
 class CfgClass : public CfgParam {
 private:
   //keep CfgHeader 40 bytes long!!!
@@ -307,7 +312,7 @@ class CfgClass : public CfgParam {
   void begin();
   uint16_t paramCount(); //get number of parameters
   bool getNameAndValue(uint16_t index, String* name, float* value); //get parameter name and value for index
-  void list(const char* filter = nullptr); //CLI print all config values
+
   bool setParam(String namestr, String val); //CLI set a parameter value, returns true on success
   bool setParamMavlink(String namestr, float val); //set a parameter value, returns true on success
   int getIndex(String namestr); //get parameter index for a parameter name
@@ -316,7 +321,12 @@ class CfgClass : public CfgParam {
   void loadFromString(const char *batch); //load text unconditional
   void load_madflight(const char *board, const char *config); //load board+config if crc is different
   void writeToEeprom(); //write config to flash
-  float getValue(String namestr, float default_value);
+  float getValue(String namestr, float default_value); //get parameter as float
+  float getValue(int i); //get parameter as float
+
+  //CLI commands
+  void cli_dump(const char* filter = nullptr); //CLI dump: print all config values
+  void cli_diff(const char* filter = nullptr); //CLI diff: print all modified config values
 
   //print
   void printParamOption(const int32_t* param);
diff --git a/src/madflight/cli/cli.cpp b/src/madflight/cli/cli.cpp
@@ -251,9 +251,10 @@ void Cli::help() {
   "bbinfo    Info\n"
   "-- CONFIG --\n"
   "set <name> <value>  Set config parameter\n"
-  "clist <filter>      List config\n"
-  "cclear              Clear config\n"
-  "cwrite              Write config to flash\n"
+  "dump <filter>       List config\n"
+  "diff <filter>       List config changes from default d\n"
+  "save                Save config and reboot\n"
+  "defaults            Reset to defaults and reboot\n"
   "-- CALIBRATE --\n"
   "calinfo   Sensor info\n"
   "calimu    Calibrate IMU error\n"
@@ -367,15 +368,21 @@ void Cli::executeCmd(String cmd, String arg1, String arg2) {
     bbx.bench();
   }else if (cmd == "set") {
     cfg.setParam(arg1, arg2);
-  }else if (cmd == "clist") {
-    cfg.list(arg1.c_str());
-  }else if (cmd == "cclear") {
+  }else if (cmd == "dump") {
+    cfg.cli_dump(arg1.c_str());
+  }else if (cmd == "diff") {
+    cfg.cli_diff(arg1.c_str());
+  }else if (cmd == "defaults") {
+    Serial.println("Resetting to defaults and rebooting, please wait... ");
     cfg.clear();
-    Serial.println("Config cleared, use 'cwrite' to write to flash");
-  }else if (cmd == "cwrite") {
-    Serial.println("writing, please wait... ");
     cfg.writeToEeprom();
-    Serial.println("cwrite completed");
+    delay(1000);
+    hal_reboot();
+  }else if (cmd == "save") {
+    Serial.println("Saving and rebooting, please wait... ");
+    cfg.writeToEeprom();
+    delay(1000);
+    hal_reboot();
   }else if (cmd == "calinfo") {
     cli_print_all(false);
     calibrate_info(arg1.toInt());
@@ -500,7 +507,7 @@ void Cli::calibrate_IMU2(bool gyro_only) {
     cfg.imu_cal_az = azerr;
   }
   
-  Serial.println("Use CLI 'cwrite' to write these values to flash");
+  Serial.println("Use 'save' to save these values to flash");
 }
 
 void Cli::calibrate_Magnetometer() {
@@ -520,7 +527,7 @@ void Cli::calibrate_Magnetometer() {
     Serial.printf("set mag_cal_sx %+f #config %+f\n", scale[0], cfg.mag_cal_sx);
     Serial.printf("set mag_cal_sy %+f #config %+f\n", scale[1], cfg.mag_cal_sy);
     Serial.printf("set mag_cal_sz %+f #config %+f\n", scale[2], cfg.mag_cal_sz);
-    Serial.println("Note: use CLI 'cwrite' to write these values to flash");
+    Serial.println("Note: use 'save' to save these values to flash");
     Serial.println(" ");
     Serial.println("If you are having trouble with your attitude estimate at a new flying location, repeat this process as needed.");
     cfg.mag_cal_x = bias[0];
diff --git a/src/madflight/cli/cli_RclCalibrate.h b/src/madflight/cli/cli_RclCalibrate.h
@@ -273,7 +273,7 @@ bool _calibrate() {
   Serial.println();
   rcl.setup(); //restart rcl to reload config (outputs RCL messages)
 
-  Serial.printf("\n=== Radio Calibration Completed ===\nNow use 'prcl' to check, 'cwrite' to save config, or reboot to restart with old config.\n");
+  Serial.printf("\n=== Radio Calibration Completed ===\nNow use 'prcl' to check, 'save' to save config, or 'reboot' to restart with old config.\n");
   return true;
 }
 
diff --git a/src/madflight/imu/ICM426XX/ICM426XX.cpp b/src/madflight/imu/ICM426XX/ICM426XX.cpp
@@ -335,7 +335,7 @@ ICM426XX::ICM426XX(MPU_Interface *dev, uint8_t whoAmI) {
 }
 
 void ICM426XX::read(int16_t *accgyr) {
-  dev->readRegs(ICM426XX_RA_ACCEL_DATA_X1, (uint8_t*)accgyr, 12); //little endian, no byte juggling needed for RP2,ESP32,STM32
+  dev->readRegs(ICM426XX_RA_ACCEL_DATA_X1, (uint8_t*)accgyr, 12); //ax,ay,az,gx,gy,gz little endian, no byte juggling needed for RP2,ESP32,STM32
 }
 
 /*
diff --git a/src/madflight/imu/ImuGizmoICM426XX.h b/src/madflight/imu/ImuGizmoICM426XX.h
@@ -46,15 +46,30 @@ class ImuGizmoICM426XX : public ImuGizmo {
   }
 
 
+/* Get sensor data in NED frame
+   x=North (forward), y=East (right), z=Down 
+   acc: gravitation force is positive in axis direction: 
+        [ax,ay,az] nose-down:[1,0,0], right-down:[0,1,0], level:[0,0,1]
+   gyr: direction of positive rotation by right hand rule: 
+        [gx,gy,gz] roll-right:[positive,0,0], pitch-up:[0,positive,0], yaw-right:[0,0,positive]
+
+ICM42688 has NWU orientation
+
+  * +--+         Y
+    |  | --> X   ^   Z-up
+    +--+         |
+    
+*/
+  
   void getMotion6NED(float *ax, float *ay, float *az, float *gx, float *gy, float *gz) override {
     int16_t accgyr[7];
     icm->read(accgyr);
-    *ax = accgyr[0] * icm->acc_scale;
-    *ay = accgyr[1] * icm->acc_scale;
-    *az = accgyr[2] * icm->acc_scale;
-    *gx = accgyr[3] * icm->gyr_scale;
-    *gy = accgyr[4] * icm->gyr_scale;
-    *gz = accgyr[5] * icm->gyr_scale;
+    *ax = -accgyr[0] * icm->acc_scale; //-N = -N
+    *ay =  accgyr[1] * icm->acc_scale; //-E =  W
+    *az =  accgyr[2] * icm->acc_scale; //-D =  U
+    *gx =  accgyr[3] * icm->gyr_scale; // N =  N
+    *gy = -accgyr[4] * icm->gyr_scale; // E = -W
+    *gz = -accgyr[5] * icm->gyr_scale; // D = -U
   }
 
   int begin(int gyro_scale_dps, int acc_scale_g, int rate_hz) {
diff --git a/src/madflight_FC1.h b/src/madflight_FC1.h
@@ -33,30 +33,33 @@ imu_i2c_adr   0 //i2c: enter decimal i2c address, not hex (use 0 for default i2c
 rcl_gizmo     NONE  // options: NONE, MAVLINK, CRSF, SBUS, DSM, PPM
 rcl_num_ch    8     // number of channels
 rcl_deadband  0     // center stick deadband
-rcl_ser_bus   -1
+rcl_ser_bus   0
 pin_rcl_ppm   -1
 
 //--- BAR --- Barometer
 bar_gizmo     HP203B  // options: NONE, BMP390, BMP388, BMP280, MS5611, HP203B
 bar_i2c_adr   118 //0x76
 bar_i2c_bus   0
+bar_rate      100
 
 //--- MAG --- Magnetometer
 mag_gizmo     QMC6309  // options: NONE, QMC5883, QMC6309, RM3100
 mag_i2c_adr   124 //0x7C
-mag_i2c_bus   0
+mag_i2c_bus   0 //sample rate [Hz]
 
 //--- BAT --- Battery Monitor  (use i2c bus -OR- adc pins)
 bat_gizmo     INA226  // options: NONE, ADC, INA226, INA228
 bat_i2c_adr   64 //0x40
 bat_i2c_bus   0
 pin_bat_i     -1
 pin_bat_v     -1
+bat_cal_v     1 //adc voltage scale, value is: actual_voltage_in_v / adc_reading
+bat_cal_i     0.0005 //adc current scale, value is: actual_current_in_a / adc_reading; for ina226/228: rshunt value in ohm
 
 //--- GPS ---
 gps_gizmo     NONE  // options: NONE, UBLOX
 gps_baud      0   // use 0 for auto baud
-gps_ser_bus   -1
+gps_ser_bus   1
 
 //--- BBX --- Black Box Data Logger  (use spi -OR- mmc)
 bbx_gizmo     SDSPI  // options: NONE, SDSPI, SDMMC

Original file line number	Diff line number	Diff line change
`@@ -1,4 +1,4 @@`
`1`		`-#define MADFLIGHT_VERSION "madflight v2.0.1"`
	`1`	`+#define MADFLIGHT_VERSION "madflight v2.0.2-DEV"`
`2`	`2`
`3`	`3`	`//madflight.h - Flight Controller for ESP32 / ESP32-S3 / RP2350 / RP2040 / STM32`
`4`	`4`
Original file line number	Diff line number	Diff line change
`@@ -273,7 +273,7 @@ bool _calibrate() {`
`273`	`273`	`Serial.println();`
`274`	`274`	`rcl.setup(); //restart rcl to reload config (outputs RCL messages)`
`275`	`275`
`276`		`- Serial.printf("\n=== Radio Calibration Completed ===\nNow use 'prcl' to check, 'cwrite' to save config, or reboot to restart with old config.\n");`
	`276`	`+ Serial.printf("\n=== Radio Calibration Completed ===\nNow use 'prcl' to check, 'save' to save config, or 'reboot' to restart with old config.\n");`
`277`	`277`	`return true;`
`278`	`278`	`}`
`279`	`279`