From 203401d8e656aaf704953288ed5efcd5ca60ffe4 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 19 May 2009 08:52:25 +0200 Subject: digital/asserv, digital/mimot: use new eeprom code --- digital/asserv/src/asserv/Makefile | 4 + digital/asserv/src/asserv/eeprom.avr.c | 317 ++++++++++++++++++--------------- digital/asserv/src/asserv/eeprom.h | 4 +- digital/asserv/src/asserv/eeprom.mak | 9 + digital/asserv/src/asserv/main.c | 4 +- digital/asserv/src/asserv/simu.host.c | 3 + 6 files changed, 194 insertions(+), 147 deletions(-) create mode 100644 digital/asserv/src/asserv/eeprom.mak (limited to 'digital/asserv/src') diff --git a/digital/asserv/src/asserv/Makefile b/digital/asserv/src/asserv/Makefile index d19d9909..80159026 100644 --- a/digital/asserv/src/asserv/Makefile +++ b/digital/asserv/src/asserv/Makefile @@ -16,4 +16,8 @@ AVR_MCU = atmega128 OPTIMIZE = -O2 HOST_LIBS = -lm +AVR_DEFS = $(EEPROM_DEFAULTS:%=-DEEPROM_DEFAULTS=\"%\") + include $(BASE)/make/Makefile.gen + +include eeprom.mak diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c index 004a2ecb..70e45dca 100644 --- a/digital/asserv/src/asserv/eeprom.avr.c +++ b/digital/asserv/src/asserv/eeprom.avr.c @@ -25,182 +25,211 @@ #include "common.h" #include "eeprom.h" -#include "modules/utils/byte.h" +#include "modules/utils/utils.h" +#include "modules/utils/crc.h" #include +#include #include "cs.h" #include "postrack.h" #include "traj.h" -#define EEPROM_START 0 +#define EEPROM_INDEX_NB (2 + AC_ASSERV_AUX_NB) /* WARNING: * If you change EEPROM format, be sure to change the EEPROM_KEY in header if - * your new format is not compatible with the old one or you will load + * your new format is not compatible with the old one or you may load * garbages in parameters. */ - -static uint32_t -eeprom_read_32 (uint8_t *addr) +struct eeprom_t { - uint8_t dw[4]; - dw[0] = eeprom_read_byte (addr++); - dw[1] = eeprom_read_byte (addr++); - dw[2] = eeprom_read_byte (addr++); - dw[3] = eeprom_read_byte (addr++); - return v8_to_v32 (dw[3], dw[2], dw[1], dw[0]); -} + /** Identify parameters version. */ + uint8_t key; + /** Saved parameters. */ + uint32_t encoder_right_correction; + struct { + uint8_t max; + uint8_t slow; + uint16_t acc; + } speed[EEPROM_INDEX_NB]; + struct { + uint16_t kp; + uint16_t ki; + uint16_t kd; + uint16_t e_sat; + uint16_t i_sat; + uint16_t d_sat; + } pos[EEPROM_INDEX_NB]; + struct { + uint16_t error_limit; + uint16_t speed_limit; + uint8_t counter_limit; + } bd[EEPROM_INDEX_NB]; + struct { + uint8_t reverse; + } output[EEPROM_INDEX_NB]; + uint16_t postrack_footing; + uint16_t traj_eps; + uint16_t traj_aeps; + uint16_t traj_angle_limit; + /** CRC of the whole structure. */ + uint8_t crc; +}; + +/* If EEPROM_DEFAULTS is defined, it should be the name of a file to include + * which defines the structure eeprom_defaults in PROGMEM and + * EEPROM_DEFAULTS_KEY which should match EEPROM_KEY. Those defaults values + * are used when no EEPROM set is good. */ +#ifdef EEPROM_DEFAULTS +# include EEPROM_DEFAULTS +# if EEPROM_DEFAULTS_KEY != EEPROM_KEY +# error "EEPROM defaults are not compatible (key mismatch)" +# endif +#endif + +EEMEM struct eeprom_t eeprom_params[4]; + +/** Index of loaded eeprom block. */ +int8_t eeprom_loaded; static void -eeprom_write_32 (uint8_t *addr, uint32_t dw) +eeprom_read_params_helper (struct eeprom_t *loaded, uint8_t index, + speed_control_t *speed, pos_control_t *pos, + blocking_detection_t *bd, output_t *output) { - eeprom_write_byte (addr++, v32_to_v8 (dw, 0)); - eeprom_write_byte (addr++, v32_to_v8 (dw, 1)); - eeprom_write_byte (addr++, v32_to_v8 (dw, 2)); - eeprom_write_byte (addr++, v32_to_v8 (dw, 3)); + speed->max = loaded->speed[index].max; + speed->slow = loaded->speed[index].slow; + speed->acc = loaded->speed[index].acc; + pos->kp = loaded->pos[index].kp; + pos->ki = loaded->pos[index].ki; + pos->kd = loaded->pos[index].kd; + pos->e_sat = loaded->pos[index].e_sat; + pos->i_sat = loaded->pos[index].i_sat; + pos->d_sat = loaded->pos[index].d_sat; + bd->error_limit = loaded->bd[index].error_limit; + bd->speed_limit = loaded->bd[index].speed_limit; + bd->counter_limit = loaded->bd[index].counter_limit; + output_set_reverse (output, loaded->output[index].reverse); } /* Read parameters from eeprom. */ void eeprom_read_params (void) { - uint8_t *p8 = (uint8_t *) EEPROM_START; - uint16_t *p16; - if (eeprom_read_byte (p8++) != EEPROM_KEY) - return; - cs_main.speed_theta.max = eeprom_read_byte (p8++); - cs_main.speed_alpha.max = eeprom_read_byte (p8++); - cs_aux[0].speed.max = eeprom_read_byte (p8++); - cs_aux[1].speed.max = eeprom_read_byte (p8++); - cs_main.speed_theta.slow = eeprom_read_byte (p8++); - cs_main.speed_alpha.slow = eeprom_read_byte (p8++); - cs_aux[0].speed.slow = eeprom_read_byte (p8++); - cs_aux[1].speed.slow = eeprom_read_byte (p8++); - output_set_reverse (&output_left, eeprom_read_byte (p8++)); - output_set_reverse (&output_right, eeprom_read_byte (p8++)); - output_set_reverse (&output_aux[0], eeprom_read_byte (p8++)); - output_set_reverse (&output_aux[1], eeprom_read_byte (p8++)); - encoder_corrector_set_correction (&encoder_right_corrector, - eeprom_read_32 (p8)); p8 += 4; - p16 = (uint16_t *) p8; - postrack_set_footing (eeprom_read_word (p16++)); - cs_main.speed_theta.acc = eeprom_read_word (p16++); - cs_main.speed_alpha.acc = eeprom_read_word (p16++); - cs_aux[0].speed.acc = eeprom_read_word (p16++); - cs_aux[1].speed.acc = eeprom_read_word (p16++); - cs_main.pos_theta.kp = eeprom_read_word (p16++); - cs_main.pos_alpha.kp = eeprom_read_word (p16++); - cs_aux[0].pos.kp = eeprom_read_word (p16++); - cs_aux[1].pos.kp = eeprom_read_word (p16++); - cs_main.pos_theta.ki = eeprom_read_word (p16++); - cs_main.pos_alpha.ki = eeprom_read_word (p16++); - cs_aux[0].pos.ki = eeprom_read_word (p16++); - cs_aux[1].pos.ki = eeprom_read_word (p16++); - cs_main.pos_theta.kd = eeprom_read_word (p16++); - cs_main.pos_alpha.kd = eeprom_read_word (p16++); - cs_aux[0].pos.kd = eeprom_read_word (p16++); - cs_aux[1].pos.kd = eeprom_read_word (p16++); - cs_main.blocking_detection_theta.error_limit = eeprom_read_word (p16++); - cs_main.blocking_detection_theta.speed_limit = eeprom_read_word (p16++); - cs_main.blocking_detection_theta.counter_limit = eeprom_read_word (p16++); - cs_main.blocking_detection_alpha.error_limit = eeprom_read_word (p16++); - cs_main.blocking_detection_alpha.speed_limit = eeprom_read_word (p16++); - cs_main.blocking_detection_alpha.counter_limit = eeprom_read_word (p16++); - cs_aux[0].blocking_detection.error_limit = eeprom_read_word (p16++); - cs_aux[0].blocking_detection.speed_limit = eeprom_read_word (p16++); - cs_aux[0].blocking_detection.counter_limit = eeprom_read_word (p16++); - cs_aux[1].blocking_detection.error_limit = eeprom_read_word (p16++); - cs_aux[1].blocking_detection.speed_limit = eeprom_read_word (p16++); - cs_aux[1].blocking_detection.counter_limit = eeprom_read_word (p16++); - cs_main.pos_theta.e_sat = eeprom_read_word (p16++); - cs_main.pos_theta.i_sat = eeprom_read_word (p16++); - cs_main.pos_theta.d_sat = eeprom_read_word (p16++); - cs_main.pos_alpha.e_sat = eeprom_read_word (p16++); - cs_main.pos_alpha.i_sat = eeprom_read_word (p16++); - cs_main.pos_alpha.d_sat = eeprom_read_word (p16++); - cs_aux[0].pos.e_sat = eeprom_read_word (p16++); - cs_aux[0].pos.i_sat = eeprom_read_word (p16++); - cs_aux[0].pos.d_sat = eeprom_read_word (p16++); - cs_aux[1].pos.e_sat = eeprom_read_word (p16++); - cs_aux[1].pos.i_sat = eeprom_read_word (p16++); - cs_aux[1].pos.d_sat = eeprom_read_word (p16++); - traj_eps = eeprom_read_word (p16++); - traj_aeps = eeprom_read_word (p16++); - traj_set_angle_limit (eeprom_read_word (p16++)); + uint8_t i; + struct eeprom_t loaded; + eeprom_loaded = -1; + /* Load first good set. */ + for (i = 0; i < UTILS_COUNT (eeprom_params); i++) + { + /* Read EEPROM. */ + eeprom_read_block (&loaded, &eeprom_params[i], + sizeof (struct eeprom_t)); + /* Check CRC. */ + if (loaded.key == EEPROM_KEY + && crc_compute ((uint8_t *) &loaded, + sizeof (struct eeprom_t)) == 0) + { + /* Ok. */ + eeprom_loaded = i; + break; + } + } + /* Load defaults if no set is good. */ +#ifdef EEPROM_DEFAULTS + if (eeprom_loaded == -1) + { + memcpy_P (&loaded, &eeprom_defaults, sizeof (struct eeprom_t)); + eeprom_loaded = 0xDF; /* DeFaults. */ + } +#endif + if (eeprom_loaded != -1) + { + /* Ok, load parameters. */ + encoder_corrector_set_correction (&encoder_right_corrector, + loaded.encoder_right_correction); + eeprom_read_params_helper (&loaded, 0, &cs_main.speed_theta, + &cs_main.pos_theta, + &cs_main.blocking_detection_theta, + &output_left); + eeprom_read_params_helper (&loaded, 1, &cs_main.speed_alpha, + &cs_main.pos_alpha, + &cs_main.blocking_detection_alpha, + &output_right); + for (i = 0; i < AC_ASSERV_AUX_NB; i++) + eeprom_read_params_helper (&loaded, 2 + i, &cs_aux[i].speed, + &cs_aux[i].pos, + &cs_aux[i].blocking_detection, + &output_aux[i]); + postrack_set_footing (loaded.postrack_footing); + traj_eps = loaded.traj_eps; + traj_aeps = loaded.traj_aeps; + traj_set_angle_limit (loaded.traj_angle_limit); + } +} + +static void +eeprom_write_params_helper (struct eeprom_t *param, uint8_t index, + speed_control_t *speed, pos_control_t *pos, + blocking_detection_t *bd, output_t *output) +{ + param->speed[index].max = speed->max; + param->speed[index].slow = speed->slow; + param->speed[index].acc = speed->acc; + param->pos[index].kp = pos->kp; + param->pos[index].ki = pos->ki; + param->pos[index].kd = pos->kd; + param->pos[index].e_sat = pos->e_sat; + param->pos[index].i_sat = pos->i_sat; + param->pos[index].d_sat = pos->d_sat; + param->bd[index].error_limit = bd->error_limit; + param->bd[index].speed_limit = bd->speed_limit; + param->bd[index].counter_limit = bd->counter_limit; + param->output[index].reverse = output->reverse; } /* Write parameters to eeprom. */ void eeprom_write_params (void) { - uint8_t *p8 = (uint8_t *) EEPROM_START; - uint16_t *p16; - eeprom_write_byte (p8++, EEPROM_KEY); - eeprom_write_byte (p8++, cs_main.speed_theta.max); - eeprom_write_byte (p8++, cs_main.speed_alpha.max); - eeprom_write_byte (p8++, cs_aux[0].speed.max); - eeprom_write_byte (p8++, cs_aux[1].speed.max); - eeprom_write_byte (p8++, cs_main.speed_theta.slow); - eeprom_write_byte (p8++, cs_main.speed_alpha.slow); - eeprom_write_byte (p8++, cs_aux[0].speed.slow); - eeprom_write_byte (p8++, cs_aux[1].speed.slow); - eeprom_write_byte (p8++, output_left.reverse); - eeprom_write_byte (p8++, output_right.reverse); - eeprom_write_byte (p8++, output_aux[0].reverse); - eeprom_write_byte (p8++, output_aux[1].reverse); - eeprom_write_32 (p8, encoder_right_corrector.correction); p8 += 4; - p16 = (uint16_t *) p8; - eeprom_write_word (p16++, postrack_footing); - eeprom_write_word (p16++, cs_main.speed_theta.acc); - eeprom_write_word (p16++, cs_main.speed_alpha.acc); - eeprom_write_word (p16++, cs_aux[0].speed.acc); - eeprom_write_word (p16++, cs_aux[1].speed.acc); - eeprom_write_word (p16++, cs_main.pos_theta.kp); - eeprom_write_word (p16++, cs_main.pos_alpha.kp); - eeprom_write_word (p16++, cs_aux[0].pos.kp); - eeprom_write_word (p16++, cs_aux[1].pos.kp); - eeprom_write_word (p16++, cs_main.pos_theta.ki); - eeprom_write_word (p16++, cs_main.pos_alpha.ki); - eeprom_write_word (p16++, cs_aux[0].pos.ki); - eeprom_write_word (p16++, cs_aux[1].pos.ki); - eeprom_write_word (p16++, cs_main.pos_theta.kd); - eeprom_write_word (p16++, cs_main.pos_alpha.kd); - eeprom_write_word (p16++, cs_aux[0].pos.kd); - eeprom_write_word (p16++, cs_aux[1].pos.kd); - eeprom_write_word (p16++, cs_main.blocking_detection_theta.error_limit); - eeprom_write_word (p16++, cs_main.blocking_detection_theta.speed_limit); - eeprom_write_word (p16++, cs_main.blocking_detection_theta.counter_limit); - eeprom_write_word (p16++, cs_main.blocking_detection_alpha.error_limit); - eeprom_write_word (p16++, cs_main.blocking_detection_alpha.speed_limit); - eeprom_write_word (p16++, cs_main.blocking_detection_alpha.counter_limit); - eeprom_write_word (p16++, cs_aux[0].blocking_detection.error_limit); - eeprom_write_word (p16++, cs_aux[0].blocking_detection.speed_limit); - eeprom_write_word (p16++, cs_aux[0].blocking_detection.counter_limit); - eeprom_write_word (p16++, cs_aux[1].blocking_detection.error_limit); - eeprom_write_word (p16++, cs_aux[1].blocking_detection.speed_limit); - eeprom_write_word (p16++, cs_aux[1].blocking_detection.counter_limit); - eeprom_write_word (p16++, cs_main.pos_theta.e_sat); - eeprom_write_word (p16++, cs_main.pos_theta.i_sat); - eeprom_write_word (p16++, cs_main.pos_theta.d_sat); - eeprom_write_word (p16++, cs_main.pos_alpha.e_sat); - eeprom_write_word (p16++, cs_main.pos_alpha.i_sat); - eeprom_write_word (p16++, cs_main.pos_alpha.d_sat); - eeprom_write_word (p16++, cs_aux[0].pos.e_sat); - eeprom_write_word (p16++, cs_aux[0].pos.i_sat); - eeprom_write_word (p16++, cs_aux[0].pos.d_sat); - eeprom_write_word (p16++, cs_aux[1].pos.e_sat); - eeprom_write_word (p16++, cs_aux[1].pos.i_sat); - eeprom_write_word (p16++, cs_aux[1].pos.d_sat); - eeprom_write_word (p16++, traj_eps); - eeprom_write_word (p16++, traj_aeps); - eeprom_write_word (p16++, traj_angle_limit); + uint8_t i; + struct eeprom_t p; + /* Prepare parameters. */ + p.key = EEPROM_KEY; + p.encoder_right_correction = encoder_right_corrector.correction; + eeprom_write_params_helper (&p, 0, &cs_main.speed_theta, + &cs_main.pos_theta, + &cs_main.blocking_detection_theta, + &output_left); + eeprom_write_params_helper (&p, 1, &cs_main.speed_alpha, + &cs_main.pos_alpha, + &cs_main.blocking_detection_alpha, + &output_right); + for (i = 0; i < AC_ASSERV_AUX_NB; i++) + eeprom_write_params_helper (&p, 2 + i, &cs_aux[i].speed, + &cs_aux[i].pos, + &cs_aux[i].blocking_detection, + &output_aux[i]); + p.postrack_footing = postrack_footing; + p.traj_eps = traj_eps; + p.traj_aeps = traj_aeps; + p.traj_angle_limit = traj_angle_limit; + p.crc = crc_compute ((uint8_t *) &p, sizeof (p) - 1); + /* Write every sets. */ + for (i = 0; i < UTILS_COUNT (eeprom_params); i++) + { + eeprom_write_block (&p, &eeprom_params[i], + sizeof (struct eeprom_t)); + } } /* Clear eeprom parameters. */ void eeprom_clear_params (void) { - uint8_t *p = (uint8_t *) EEPROM_START; - eeprom_write_byte (p, 0xff); + uint8_t i; + /* Clear every sets. */ + for (i = 0; i < UTILS_COUNT (eeprom_params); i++) + eeprom_write_byte (&eeprom_params[i].key, 0); } diff --git a/digital/asserv/src/asserv/eeprom.h b/digital/asserv/src/asserv/eeprom.h index c4cec931..6da2a083 100644 --- a/digital/asserv/src/asserv/eeprom.h +++ b/digital/asserv/src/asserv/eeprom.h @@ -26,7 +26,9 @@ * }}} */ /** Change the eeprom key each time you change eeprom format. */ -#define EEPROM_KEY 0x4f +#define EEPROM_KEY 0x50 + +extern int8_t eeprom_loaded; void eeprom_read_params (void); diff --git a/digital/asserv/src/asserv/eeprom.mak b/digital/asserv/src/asserv/eeprom.mak new file mode 100644 index 00000000..b41d5537 --- /dev/null +++ b/digital/asserv/src/asserv/eeprom.mak @@ -0,0 +1,9 @@ +EEPROM_DEFAULTS_new := $(EEPROM_DEFAULTS) +-include obj/eeprom_defaults +EXTRA_CLEAN_FILES += obj/eeprom_defaults +ifneq (was $(EEPROM_DEFAULTS_new),$(EEPROM_DEFAULTS_old)) +obj/eeprom_defaults: | $(OBJDIR) + echo "EEPROM_DEFAULTS_old = was $(EEPROM_DEFAULTS_new)" > $@ +.PHONY: obj/eeprom_defaults +endif +obj/eeprom.avr.avr.o: obj/eeprom_defaults diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index 5d91d90e..5fbd3a60 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -699,7 +699,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) break; case c ('P', 1): /* Print current settings. */ - proto_send1b ('E', EEPROM_KEY); + proto_send2b ('E', EEPROM_KEY, eeprom_loaded); proto_send1d ('c', encoder_right_corrector.correction); proto_send1w ('f', postrack_footing); proto_send2w ('e', traj_eps, traj_aeps); @@ -712,7 +712,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) case c ('P', 2): /* Print current settings for selected control. * - b: index. */ - proto_send1b ('E', EEPROM_KEY); + proto_send2b ('E', EEPROM_KEY, eeprom_loaded); proto_send1w ('a', speed->acc); proto_send2b ('s', speed->max, speed->slow); proto_send3w ('b', bd->error_limit, bd->speed_limit, diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c index 1c609cc6..82f628ca 100644 --- a/digital/asserv/src/asserv/simu.host.c +++ b/digital/asserv/src/asserv/simu.host.c @@ -49,6 +49,9 @@ /** Simulate some AVR regs. */ uint8_t DDRF, PORTC, PORTD, PORTE, PORTF, PORTG, PINC; +/** Index of loaded eeprom block. */ +int8_t eeprom_loaded = -1; + /* Robot model. */ const struct robot_t *simu_robot; -- cgit v1.2.3