From 07495d33b184fddc3e7da0d82c12839dcf850379 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Thu, 28 Feb 2013 09:57:41 +0100 Subject: [PATCH] [module] Re-configure the motors... --- conf/settings/modules/configure_mkk.xml | 12 +- sw/airborne/modules/config/config_mkk.c | 139 ++++++++++++++++++++---- sw/airborne/modules/config/config_mkk.h | 26 ++++- 3 files changed, 151 insertions(+), 26 deletions(-) diff --git a/conf/settings/modules/configure_mkk.xml b/conf/settings/modules/configure_mkk.xml index 66af5fb8529..fd853fcb5c3 100644 --- a/conf/settings/modules/configure_mkk.xml +++ b/conf/settings/modules/configure_mkk.xml @@ -2,9 +2,19 @@ - + + + + + + + + + + + diff --git a/sw/airborne/modules/config/config_mkk.c b/sw/airborne/modules/config/config_mkk.c index 7b6a2abb3f1..699a1582b98 100644 --- a/sw/airborne/modules/config/config_mkk.c +++ b/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 config_mkk_eeprom_t config_mkk_eeprom; +*/ -extern BLConfig_t BLConfig; +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; - Motor[read_nr].Current = config_mkk.trans.buf[0]; - Motor[read_nr].MaxPWM = config_mkk.trans.buf[1]; - Motor[read_nr].Temperature = config_mkk.trans.buf[2]; + 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].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++; - if (read_nr >= MAX_MOTORS) - read_nr = 0; - - const uint8_t actuators_addr[ACTUATORS_MKK2_NB] = ACTUATORS_MKK2_ADDR; - - //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); + // Read Config + if (config_mkk.read_config > 0) + { + config_mkk.read_config = 0; + config_mkk_read_eeprom(); + + + 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); + +} + diff --git a/sw/airborne/modules/config/config_mkk.h b/sw/airborne/modules/config/config_mkk.h index 97b81486ddd..897090b088f 100644 --- a/sw/airborne/modules/config/config_mkk.h +++ b/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; \ +}