[module] Re-configure the motors...

conf/settings/modules/configure_mkk.xml

@@ -2,9 +2,19 @@
   <dl_settings>
 
     <dl_settings NAME="mkk">
-      <dl_setting var="config_mkk.addr" min="0" step="1" max="3" module="modules/config/config_mkk" shortname="cmd"  values="0x52|0x54|0x56|0x58"  handler="SetCommand"/>
+      <dl_setting var="config_mkk.addr" min="0" step="1" max="3" module="modules/config/config_mkk" shortname="Get"  values="0x52|0x54|0x56|0x58"  handler="GetConfig"/>
+      <dl_setting var="config_mkk.addr" min="0" step="1" max="3" module="modules/config/config_mkk" shortname="Set"  values="0x52|0x54|0x56|0x58"  handler="SetConfig"/>
       <dl_setting var="config_mkk.nb_err" min="0" step="1" max="3000"  shortname="err" />
+      <dl_setting var="config_mkk_eeprom.revision" min="0" step="1" max="255"  shortname="ee.rev" />
+      <dl_setting var="config_mkk_eeprom.SetMask" min="0" step="1" max="255"  shortname="ee.set.msk" />
+      <dl_setting var="config_mkk_eeprom.PwmScaling" min="0" step="1" max="255"  shortname="ee.pwm" />
+      <dl_setting var="config_mkk_eeprom.CurrentLimit" min="0" step="1" max="255"  shortname="ee.amp.lim" />
+      <dl_setting var="config_mkk_eeprom.TempLimit" min="0" step="1" max="255"  shortname="ee.tmp.lim" />
+      <dl_setting var="config_mkk_eeprom.CurrentScaling" min="0" step="1" max="255"  shortname="ee.amp.scl" />
+      <dl_setting var="config_mkk_eeprom.BitConfig" min="0" step="1" max="255"  shortname="ee.bit.msk" />
+      <dl_setting var="config_mkk_eeprom.crc" min="0" step="1" max="255"  shortname="ee.crc" />
     </dl_settings>
 
   </dl_settings>
 </settings>
+

sw/airborne/modules/config/config_mkk.c

@@ -25,6 +25,11 @@
 
 
 
+void config_mkk_read_eeprom(void);
+void config_mkk_parse_eeprom(void);
+uint8_t config_mkk_crc(uint8_t offset);
+
+
 // Following 2 structs are known from:  http://mikrokopter.de/mikrosvn/FlightCtrl/tags/V0.88n/twimaster.h
 
 
@@ -43,9 +48,10 @@ typedef struct
 
 extern MotorData_t Motor[MAX_MOTORS];
 
+/*
 typedef struct
 {
-  uint8_t Revision;     // must be BL_REVISION
+  uint8_t revision;     // must be BL_revision
   uint8_t SetMask;      // settings mask
   uint8_t PwmScaling;     // maximum value of control pwm, acts like a thrust limit
   uint8_t CurrentLimit;   // current limit in A
@@ -53,18 +59,29 @@ typedef struct
   uint8_t CurrentScaling;   // scaling factor for current measurement
   uint8_t BitConfig;      // see defines above
   uint8_t crc;        // checksum
-}  __attribute__((packed)) BLConfig_t;
+}  __attribute__((packed)) config_mkk_eeprom_t;
 
-extern BLConfig_t BLConfig;
+extern config_mkk_eeprom_t config_mkk_eeprom;
+*/
+
+uint8_t config_mkk_crc(uint8_t offset)
+{
+	uint8_t crc = 0xaa;
+	for(int i=offset; i<(offset+7); i++)
+	{
+		crc += config_mkk.trans.buf[i];
+	}
+	return crc;
+}
 
 
 MotorData_t Motor[MAX_MOTORS];
-BLConfig_t BLConfig;
+config_mkk_eeprom_t config_mkk_eeprom;
 
 
 #define BL_READMODE_CONFIG	       16
 
-#define BLCONFIG_REVISION          2
+#define CONFIG_MKK_EEPROM_REVISION          2
 
 #define MASK_SET_PWM_SCALING       0x01
 #define MASK_SET_CURRENT_LIMIT     0x02
@@ -81,9 +98,8 @@ BLConfig_t BLConfig;
 void init_config_mkk(void)
 {
   config_mkk.nb_err = 0;
+  config_mkk.read_config = 0;
 
-  config_mkk.trans.type = I2CTransRx;
-  config_mkk.trans.len_r = 3;
   config_mkk.trans.status = I2CTransSuccess;
   
   for(int i=0; i < MAX_MOTORS; i++)
@@ -107,29 +123,46 @@ void periodic_config_mkk_read_status(void)
   switch (config_mkk.trans.status) {
     case I2CTransFailed:
       config_mkk.nb_err++;
-      config_mkk.trans.status = I2CTransDone;
       break;
     case I2CTransSuccess:
     case I2CTransDone:
-      config_mkk.trans.status = I2CTransDone;
+      if (config_mkk.trans.len_r == 3)
-      Motor[read_nr].Current = config_mkk.trans.buf[0];
+      {
-      Motor[read_nr].MaxPWM = config_mkk.trans.buf[1];
+          Motor[read_nr].Current = config_mkk.trans.buf[0];
-      Motor[read_nr].Temperature = config_mkk.trans.buf[2];
+          Motor[read_nr].MaxPWM = config_mkk.trans.buf[1];
+          Motor[read_nr].Temperature = config_mkk.trans.buf[2];
+      }
+      else if (config_mkk.trans.len_r == 8)
+      {
+        config_mkk_parse_eeprom();
+      }
       break;
     default:
       config_mkk.nb_err++;
       return;
   }
 
-  read_nr++;
+  // Read Config
-  if (read_nr >= MAX_MOTORS)
+  if (config_mkk.read_config > 0)
-    read_nr = 0;
+  {
-  
+    config_mkk.read_config = 0;
-  const uint8_t actuators_addr[ACTUATORS_MKK2_NB] = ACTUATORS_MKK2_ADDR;
+    config_mkk_read_eeprom();
-  
+
-  //Motor[motor_write].ReadMode = BL_READMODE_STATUS; // normal status request
+
-  config_mkk.trans.slave_addr = actuators_addr[read_nr];
+    i2c_submit(&ACTUATORS_MKK2_DEVICE, &config_mkk.trans);
-  i2c_submit(&ACTUATORS_MKK2_DEVICE, &config_mkk.trans);
+  }
+  // Read Status
+  else
+  {
+    read_nr++;
+    if (read_nr >= MAX_MOTORS)
+      read_nr = 0;
+    const uint8_t actuators_addr[ACTUATORS_MKK2_NB] = ACTUATORS_MKK2_ADDR;
+    config_mkk.trans.type = I2CTransRx;
+    config_mkk.trans.len_r = 3;
+    config_mkk.trans.slave_addr = actuators_addr[read_nr];
+  }  
+
 
 }
 
@@ -153,3 +186,67 @@ void periodic_config_mkk_telemetry(void)
 }
 
 
+#define RETURN_IF_NOT_KILLMODE() {}
+
+void config_mkk_read_eeprom(void)
+{
+    // Do not read config while running
+    RETURN_IF_NOT_KILLMODE();
+
+    // New I2C Write/Read Transaction
+    config_mkk.trans.type = I2CTransTxRx;
+    config_mkk.trans.slave_addr = 0x52 + config_mkk.addr * 2;
+    config_mkk.trans.len_w = 2;
+    config_mkk.trans.buf[0] = 0;
+    config_mkk.trans.buf[1] = (BL_READMODE_CONFIG<<3);
+    config_mkk.trans.len_r = 8;
+}
+
+void config_mkk_parse_eeprom(void)
+{
+    config_mkk_eeprom.crc            = config_mkk.trans.buf[7];   // checksum
+    if (config_mkk_crc(0) != config_mkk_eeprom.crc)
+    {
+        config_mkk.nb_err++;
+    }
+    else
+    {
+        config_mkk_eeprom.revision       = config_mkk.trans.buf[0];   // must be BL_revision
+        config_mkk_eeprom.SetMask        = config_mkk.trans.buf[1];   // settings mask
+        config_mkk_eeprom.PwmScaling     = config_mkk.trans.buf[2];   // maximum value of control pwm, acts like a thrust limit
+        config_mkk_eeprom.CurrentLimit   = config_mkk.trans.buf[3];   // current limit in A
+        config_mkk_eeprom.TempLimit      = config_mkk.trans.buf[4];   // in °C
+        config_mkk_eeprom.CurrentScaling = config_mkk.trans.buf[5];   // scaling factor for current measurement
+        config_mkk_eeprom.BitConfig      = config_mkk.trans.buf[6];   // see defines above
+    }
+}
+
+void config_mkk_send_eeprom(void)
+{
+    // Do not upload while running
+    RETURN_IF_NOT_KILLMODE();
+
+    // Do not upload bad data:
+    if (config_mkk_eeprom.revision != CONFIG_MKK_EEPROM_REVISION)
+      return;
+
+    // New I2C Write Transaction
+    config_mkk.trans.type = I2CTransTx;
+    config_mkk.trans.slave_addr = 0x52 + config_mkk.addr * 2;
+    config_mkk.trans.len_w = 10;
+    config_mkk.trans.buf[0] = 0;
+    config_mkk.trans.buf[1] = (BL_READMODE_CONFIG<<3);
+
+    config_mkk.trans.buf[2] = config_mkk_eeprom.revision;
+    config_mkk.trans.buf[3] = config_mkk_eeprom.SetMask;
+    config_mkk.trans.buf[4] = config_mkk_eeprom.PwmScaling;
+    config_mkk.trans.buf[5] = config_mkk_eeprom.CurrentLimit;
+    config_mkk.trans.buf[6] = config_mkk_eeprom.TempLimit;
+    config_mkk.trans.buf[7] = config_mkk_eeprom.CurrentScaling;
+    config_mkk.trans.buf[8] = config_mkk_eeprom.BitConfig;
+    config_mkk.trans.buf[9] = config_mkk_crc(2);
+
+    i2c_submit(&ACTUATORS_MKK2_DEVICE, &config_mkk.trans);
+
+}
+

sw/airborne/modules/config/config_mkk.h

@@ -32,12 +32,24 @@
 
 #include "mcu_periph/i2c.h"
 
+typedef struct
+{
+  uint8_t revision;
+  uint8_t SetMask;
+  uint8_t PwmScaling;
+  uint8_t CurrentLimit;
+  uint8_t TempLimit;
+  uint8_t CurrentScaling;
+  uint8_t BitConfig;
+  uint8_t crc;
+} config_mkk_eeprom_t;
+
+extern config_mkk_eeprom_t config_mkk_eeprom;
 
 struct config_mkk_struct
 {
+    int read_config;
     int addr;
-    int temp;
-    int current;
 
     int nb_err;
 
@@ -47,11 +59,17 @@ struct config_mkk_struct
 
 extern struct config_mkk_struct config_mkk;
 
-#define config_mkk_SetCommand(_v) {   \
+extern void config_mkk_send_eeprom(void);
+
+#define config_mkk_SetConfig(_v) {    \
     config_mkk.addr = _v;             \
+    config_mkk_send_eeprom();         \
 }
 
-
+#define config_mkk_GetConfig(_v) {    \
+    config_mkk.addr = _v;             \
+    config_mkk.read_config = 1;       \
+}