From: Nicolas D. <Ba...@us...> - 2011-05-29 22:51:18
|
This is an automated email from the git hooks/post-receive script. It was generated because a ref change was pushed to the repository containing the project "krobot". The branch, master has been updated via 101c37b33f577397fc259e8bb532220ca5c009db (commit) from 1ed43bec7729c6f008dcd6d1fde00c24e44a4854 (commit) Those revisions listed above that are new to this repository have not appeared on any other notification email; so we list those revisions in full, below. - Log ----------------------------------------------------------------- commit 101c37b33f577397fc259e8bb532220ca5c009db Author: Nicolas Dandrimont <Nic...@cr...> Date: Mon May 30 00:49:35 2011 +0200 [Sensor_Actuator] Import Firmware ----------------------------------------------------------------------- Changes: diff --git a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/Makefile b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/Makefile new file mode 100644 index 0000000..ffd4842 --- /dev/null +++ b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/Makefile @@ -0,0 +1,17 @@ +# +# Copyright 2009 Develer S.r.l. (http://www.develer.com/) +# All rights reserved. +# +# Author: Lorenzo Berni <du...@de...> +# + +# Set to 1 for verbose build output, 0 for terse output +V := 0 + +default: all + +include bertos/config.mk + +include sensor_actuator/sensor_actuator.mk + +include bertos/rules.mk diff --git a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/VERSION b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/VERSION new file mode 100644 index 0000000..e520145 --- /dev/null +++ b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/VERSION @@ -0,0 +1 @@ +BeRTOS 2.6.99 [local copy] diff --git a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/crc.c b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/crc.c new file mode 100644 index 0000000..d4293f9 --- /dev/null +++ b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/crc.c @@ -0,0 +1,102 @@ +/** + * \file + * <!-- + * This file is part of BeRTOS. + * + * Bertos is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + * As a special exception, you may use this file as part of a free software + * library without restriction. Specifically, if other files instantiate + * templates or use macros or inline functions from this file, or you compile + * this file and link it with other files to produce an executable, this + * file does not by itself cause the resulting executable to be covered by + * the GNU General Public License. This exception does not however + * invalidate any other reasons why the executable file might be covered by + * the GNU General Public License. + * + * Copyright 2003,2004 Develer S.r.l. (http://www.develer.com/) + * Copyright 1999 Bernie Innocenti <be...@co...> + * + * --> + * + * \brief CRC table and support routines + * + * \author Bernie Innocenti <be...@co...> + */ + +#include "crc.h" + +/* + * The boot on AVR cpu is placed at the end of flash memory, but the avr + * address memory by byte and the pointers are 16bits long, so we are able + * to address 64Kbyte memory max. For this reason we can't read the crctab + * from flash, because it is placed at the end of memory. This is true every + * time we have an AVR cpu with more that 64Kbyte of flash. To fix this problem + * we let the compiler copy the table in RAM at startup. Obviously this solution + * is not efficent, but for now this is the only way. + */ +#if CPU_HARVARD && !(defined(ARCH_BOOT) && (ARCH & ARCH_BOOT)) + #define CRC_TABLE const uint16_t PROGMEM crc16tab[256] +#else + #define CRC_TABLE const uint16_t crc16tab[256] +#endif + +/** + * crctab calculated by Mark G. Mendel, Network Systems Corporation + */ +CRC_TABLE = { + 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7, + 0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef, + 0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6, + 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de, + 0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485, + 0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d, + 0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4, + 0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc, + 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, 0x2802, 0x3823, + 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b, + 0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12, + 0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a, + 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41, + 0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49, + 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70, + 0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78, + 0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f, + 0x1080, 0x00a1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067, + 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e, + 0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256, + 0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d, + 0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, + 0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c, + 0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634, + 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab, + 0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3, + 0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a, + 0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92, + 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9, + 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1, + 0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, + 0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0 +}; + +uint16_t crc16(uint16_t crc, const void *buffer, size_t len) +{ + const unsigned char *buf = (const unsigned char *)buffer; + while(len--) + crc = UPDCRC16(*buf++, crc); + + return crc; +} + diff --git a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/crc.h b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/crc.h new file mode 100644 index 0000000..c5c1eef --- /dev/null +++ b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/crc.h @@ -0,0 +1,115 @@ +/** + * \file + * <!-- + * This file is part of BeRTOS. + * + * Bertos is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + * As a special exception, you may use this file as part of a free software + * library without restriction. Specifically, if other files instantiate + * templates or use macros or inline functions from this file, or you compile + * this file and link it with other files to produce an executable, this + * file does not by itself cause the resulting executable to be covered by + * the GNU General Public License. This exception does not however + * invalidate any other reasons why the executable file might be covered by + * the GNU General Public License. + * + * Copyright 2003, 2004 Develer S.r.l. (http://www.develer.com/) + * Copyright 1999 Bernie Innocenti <be...@co...> + * + * --> + * + * \brief Cyclic Redundancy Check 16 (CRC). This algorithm is the one used by the XMODEM protocol. + * + * \note This algorithm is incompatible with the CCITT-CRC16. + * + * This code is based on the article Copyright 1986 Stephen Satchell. + * + * Programmers may incorporate any or all code into their programs, + * giving proper credit within the source. Publication of the + * source routines is permitted so long as proper credit is given + * to Stephen Satchell, Satchell Evaluations and Chuck Forsberg, + * Omen Technology. + * + * \author Bernie Innocenti <be...@co...> + * + * $WIZ$ module_name = "crc16" + */ + +#ifndef ALGO_CRC_H +#define ALGO_CRC_H + +#include "cfg/cfg_arch.h" + +#include <cfg/compiler.h> +#include <cpu/pgm.h> + +EXTERN_C_BEGIN + +/* CRC table */ +extern const uint16_t crc16tab[256]; + + +/** + * \brief Compute the updated CRC16 value for one octet (macro version) + * + * \note This version is only intended for old/broken compilers. + * Use the inline function in new code. + * + * \param c New octet (range 0-255) + * \param oldcrc Previous CRC16 value (referenced twice, beware of side effects) + */ +#if CPU_HARVARD && !(defined(ARCH_BOOT) && (ARCH & ARCH_BOOT)) + #define UPDCRC16(c, oldcrc) (pgm_read_uint16_t(&crc16tab[((oldcrc) >> 8) ^ ((unsigned char)(c))]) ^ ((oldcrc) << 8)) +#else + #define UPDCRC16(c, oldcrc) ((crc16tab[((oldcrc) >> 8) ^ ((unsigned char)(c))]) ^ ((oldcrc) << 8)) +#endif + +/** CRC-16 init value */ +#define CRC16_INIT_VAL ((uint16_t)0) + +#ifdef INLINE +/** + * \brief Compute the updated CRC16 value for one octet (inline version) + */ +INLINE uint16_t updcrc16(uint8_t c, uint16_t oldcrc) +{ +#if CPU_HARVARD && !(defined(ARCH_BOOT) && (ARCH & ARCH_BOOT)) + return pgm_read_uint16_t(&crc16tab[(oldcrc >> 8) ^ c]) ^ (oldcrc << 8); +#else + return crc16tab[(oldcrc >> 8) ^ c] ^ (oldcrc << 8); +#endif +} +#endif // INLINE + + +/** + * This function implements the CRC 16 calculation on a buffer. + * + * \param crc Current CRC16 value. + * \param buf The buffer to perform CRC calculation on. + * \param len The length of the Buffer. + * + * \return The updated CRC16 value. + */ +extern uint16_t crc16(uint16_t crc, const void *buf, size_t len); + +int crc_testSetup(void); +int crc_testRun(void); +int crc_testTearDown(void); + +EXTERN_C_END + +#endif /* ALGO_CRC_H */ diff --git a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/crc_ccitt.c b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/crc_ccitt.c new file mode 100644 index 0000000..feaf624 --- /dev/null +++ b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/crc_ccitt.c @@ -0,0 +1,83 @@ +/** + * \file + * <!-- + * This file is part of BeRTOS. + * + * Bertos is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + * As a special exception, you may use this file as part of a free software + * library without restriction. Specifically, if other files instantiate + * templates or use macros or inline functions from this file, or you compile + * this file and link it with other files to produce an executable, this + * file does not by itself cause the resulting executable to be covered by + * the GNU General Public License. This exception does not however + * invalidate any other reasons why the executable file might be covered by + * the GNU General Public License. + * + * Copyright 2009 Develer S.r.l. (http://www.develer.com/) + * + * --> + * + * \brief CRC-CCITT table and support routines + * + * \author Francesco Sacchi <ba...@de...> + */ + +#include "crc_ccitt.h" + +const uint16_t PROGMEM crc_ccitt_tab[256] = { + 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, + 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, + 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, + 0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, + 0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd, + 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5, + 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, + 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974, + 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, + 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, + 0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, + 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, + 0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, + 0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, + 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738, + 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, + 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7, + 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, + 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, + 0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, + 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, + 0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, + 0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134, + 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c, + 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, + 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb, + 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, + 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, + 0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, + 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, + 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, + 0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78, +}; + +uint16_t crc_ccitt(uint16_t crc, const void *buffer, size_t len) +{ + const unsigned char *buf = (const unsigned char *)buffer; + while (len--) + crc = updcrc_ccitt(*buf++, crc); + + return crc; +} + diff --git a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/crc_ccitt.h b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/crc_ccitt.h new file mode 100644 index 0000000..240e6e1 --- /dev/null +++ b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/crc_ccitt.h @@ -0,0 +1,78 @@ +/** + * \file + * <!-- + * This file is part of BeRTOS. + * + * Bertos is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + * As a special exception, you may use this file as part of a free software + * library without restriction. Specifically, if other files instantiate + * templates or use macros or inline functions from this file, or you compile + * this file and link it with other files to produce an executable, this + * file does not by itself cause the resulting executable to be covered by + * the GNU General Public License. This exception does not however + * invalidate any other reasons why the executable file might be covered by + * the GNU General Public License. + * + * Copyright 2009 Develer S.r.l. (http://www.develer.com/) + * + * --> + * + * \brief CCITT Cyclic Redundancy Check (CRC-CCITT). + * + * \note This algorithm is incompatible with the CRC16. + * + * \author Francesco Sacchi <ba...@de...> + * + * $WIZ$ module_name = "crc-ccitt" + */ + +#ifndef ALGO_CRC_CCITT_H +#define ALGO_CRC_CCITT_H + +#include <cfg/compiler.h> +#include <cpu/pgm.h> + +EXTERN_C_BEGIN + +/* CRC table */ +extern const uint16_t crc_ccitt_tab[256]; + +/** + * \brief Compute the updated CRC-CCITT value for one octet (inline version) + */ +INLINE uint16_t updcrc_ccitt(uint8_t c, uint16_t oldcrc) +{ + return (oldcrc >> 8) ^ pgm_read16(&crc_ccitt_tab[(oldcrc ^ c) & 0xff]); +} + +/** CRC-CCITT init value */ +#define CRC_CCITT_INIT_VAL ((uint16_t)0xFFFF) + + +/** + * This function implements the CRC-CCITT calculation on a buffer. + * + * \param crc Current CRC-CCITT value. + * \param buf The buffer to perform CRC calculation on. + * \param len The length of the Buffer. + * + * \return The updated CRC-CCITT value. + */ +extern uint16_t crc_ccitt(uint16_t crc, const void *buf, size_t len); + +EXTERN_C_END + +#endif /* ALGO_CRC_CCITT_H */ diff --git a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/crc_test.c b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/crc_test.c new file mode 100644 index 0000000..1a93e3b --- /dev/null +++ b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/crc_test.c @@ -0,0 +1,75 @@ +/** + * \file + * <!-- + * This file is part of BeRTOS. + * + * Bertos is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + * As a special exception, you may use this file as part of a free software + * library without restriction. Specifically, if other files instantiate + * templates or use macros or inline functions from this file, or you compile + * this file and link it with other files to produce an executable, this + * file does not by itself cause the resulting executable to be covered by + * the GNU General Public License. This exception does not however + * invalidate any other reasons why the executable file might be covered by + * the GNU General Public License. + * + * Copyright 2003,2004 Develer S.r.l. (http://www.develer.com/) + * Copyright 1999 Bernie Innocenti <be...@co...> + * + * --> + * + * \brief CRC-CCITT table and support routines + * + * \author Francesco Sacchi <ba...@de...> + */ + +#include "crc_ccitt.h" +#include "crc.h" + +#include <cfg/debug.h> +#include <cfg/test.h> + + +int crc_testSetup(void) +{ + kdbg_init(); + return 0; +} + +int crc_testTearDown(void) +{ + return 0; +} + +int crc_testRun(void) +{ + char vector[9] = "123456789"; + + uint16_t crc = CRC_CCITT_INIT_VAL; + + crc = crc_ccitt(crc, vector, sizeof(vector)); + kprintf("crc_ccitt [%04X]\n", crc); + ASSERT(crc == 0x6F91); + + crc = CRC16_INIT_VAL; + crc = crc16(crc, vector, sizeof(vector)); + kprintf("crc16 [%04X]\n", crc); + ASSERT(crc == 0x31C3); + + return 0; +} + +TEST_MAIN(crc); diff --git a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/md2.c b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/md2.c new file mode 100644 index 0000000..589d989 --- /dev/null +++ b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/md2.c @@ -0,0 +1,336 @@ +/** + * \file + * <!-- + * This file is part of BeRTOS. + * + * Bertos is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + * As a special exception, you may use this file as part of a free software + * library without restriction. Specifically, if other files instantiate + * templates or use macros or inline functions from this file, or you compile + * this file and link it with other files to produce an executable, this + * file does not by itself cause the resulting executable to be covered by + * the GNU General Public License. This exception does not however + * invalidate any other reasons why the executable file might be covered by + * the GNU General Public License. + * + * Copyright 2007 Develer S.r.l. (http://www.develer.com/) + * + * --> + * + * \brief MD2 Message-Digest algorithm. + * + * The MD2 algorithm work with a constant array of 256 permutationt + * defined in RFC1319. If you don't want to use a standard array of + * permutatione you can use a md2_perm() function that generate an + * array of 256 "casual" permutation. To swich from a standard array + * to md2_perm function you must chanche CONFIG_MD2_STD_PERM defined in + * appconfig.h. + * If you need to store array in program memory you must define + * a macro _PROGMEM (for more info see cpu/pgm.h). + * + * + * \author Daniele Basile <as...@de...> + */ + +#include "md2.h" + +#include <string.h> //memset(), memcpy(); +#include <cfg/compiler.h> +#include <cfg/debug.h> //ASSERT() +#include <cfg/macros.h> //MIN(), countof(), ROTR(); +#include <cpu/pgm.h> + + +#if CONFIG_MD2_STD_PERM + /* + * Official array of 256 byte pemutation contructed from digits of pi, defined + * in the RFC 1319. + */ + static const uint8_t PROGMEM md2_perm[256] = + { + 41, 46, 67, 201, 162, 216, 124, 1, 61, 54, 84, 161, 236, 240, 6, + 19, 98, 167, 5, 243, 192, 199, 115, 140, 152, 147, 43, 217, 188, + 76, 130, 202, 30, 155, 87, 60, 253, 212, 224, 22, 103, 66, 111, 24, + 138, 23, 229, 18, 190, 78, 196, 214, 218, 158, 222, 73, 160, 251, + 245, 142, 187, 47, 238, 122, 169, 104, 121, 145, 21, 178, 7, 63, + 148, 194, 16, 137, 11, 34, 95, 33, 128, 127, 93, 154, 90, 144, 50, + 39, 53, 62, 204, 231, 191, 247, 151, 3, 255, 25, 48, 179, 72, 165, + 181, 209, 215, 94, 146, 42, 172, 86, 170, 198, 79, 184, 56, 210, + 150, 164, 125, 182, 118, 252, 107, 226, 156, 116, 4, 241, 69, 157, + 112, 89, 100, 113, 135, 32, 134, 91, 207, 101, 230, 45, 168, 2, 27, + 96, 37, 173, 174, 176, 185, 246, 28, 70, 97, 105, 52, 64, 126, 15, + 85, 71, 163, 35, 221, 81, 175, 58, 195, 92, 249, 206, 186, 197, + 234, 38, 44, 83, 13, 110, 133, 40, 132, 9, 211, 223, 205, 244, 65, + 129, 77, 82, 106, 220, 55, 200, 108, 193, 171, 250, 36, 225, 123, + 8, 12, 189, 177, 74, 120, 136, 149, 139, 227, 99, 232, 109, 233, + 203, 213, 254, 59, 0, 29, 57, 242, 239, 183, 14, 102, 88, 208, 228, + 166, 119, 114, 248, 235, 117, 75, 10, 49, 68, 80, 180, 143, 237, + 31, 26, 219, 153, 141, 51, 159, 17, 131, 20 + }; + + #define MD2_PERM(x) pgm_read8(&md2_perm[x]) +#else + /** + * Md2_perm() function generate an array of 256 "casual" permutation. + */ + + /** + * Costant define for computing an array of 256 "casual" permutation. + * \{ + */ + #define K1 5 + #define K2 3 + #define R 2 + #define X 172 + /*\}*/ + + static uint8_t md2_perm(uint8_t i) + { + + i = i * K1; + i = ROTR(i, R); + i ^= X; + i = i * K2; + + return i; + } + + #define MD2_PERM(x) md2_perm(x) + +#endif + + +/** + * Pad function. Put len_pad unsigned char in + * input block. + */ +static void md2_pad(void *_block, size_t len_pad) +{ + uint8_t *block; + + block = (uint8_t *)_block; + + ASSERT(len_pad <= CONFIG_MD2_BLOCK_LEN); + + /* + * Fill input block with len_pad char. + */ + memset(block, len_pad, len_pad); + +} + +static void md2_compute(void *_state, void *_checksum, void *_block) +{ + int i = 0; + uint16_t t = 0; + uint8_t compute_array[COMPUTE_ARRAY_LEN]; + uint8_t *state; + uint8_t *checksum; + uint8_t *block; + + state = (uint8_t *)_state; + checksum = (uint8_t *)_checksum; + block = (uint8_t *)_block; + + /* + * Copy state and checksum context in compute array. + */ + memcpy(compute_array, state, CONFIG_MD2_BLOCK_LEN); + memcpy(compute_array + CONFIG_MD2_BLOCK_LEN, block, CONFIG_MD2_BLOCK_LEN); + + /* + * Fill compute array with state XOR block + */ + for(i = 0; i < CONFIG_MD2_BLOCK_LEN; i++) + compute_array[i + (CONFIG_MD2_BLOCK_LEN * 2)] = state[i] ^ block[i]; + + /* + * Encryt block. + */ + for(i = 0; i < NUM_COMPUTE_ROUNDS; i++) + { + for(int j = 0; j < COMPUTE_ARRAY_LEN; j++) + { + compute_array[j] ^= MD2_PERM(t); + t = compute_array[j]; + } + + t = (t + i) & 0xff; //modulo 256. + } + /* + * Update checksum. + */ + t = checksum[CONFIG_MD2_BLOCK_LEN - 1]; + + for(i = 0; i < CONFIG_MD2_BLOCK_LEN; i++) + { + checksum[i] ^= MD2_PERM(block[i] ^ t); + t = checksum[i]; + } + + /* + * Update state and clean compute array. + */ + memcpy(state, compute_array, CONFIG_MD2_BLOCK_LEN); + memset(compute_array, 0, sizeof(compute_array)); +} + +/** + * Algorithm initialization. + * + * \param context empty context. + */ +void md2_init(Md2Context *context) +{ + + memset(context, 0, sizeof(Md2Context)); + +} + +/** + * Update block. + */ +void md2_update(Md2Context *context, const void *_block_in, size_t block_len) +{ + + const uint8_t *block_in; + size_t cpy_len; + + + block_in = (const uint8_t *)_block_in; + + while(block_len > 0) + { + /* + * Choose a number of block that fill input context buffer. + */ + cpy_len = MIN(block_len, CONFIG_MD2_BLOCK_LEN - context->counter); + + + /* + * Copy in the buffer input block. + */ + memcpy(&context->buffer[context->counter], block_in, cpy_len); + + /* + * Update a context counter, input block length and remaning + * context buffer block lenght. + */ + context->counter += cpy_len; + block_len -= cpy_len; + block_in += cpy_len; + + /* + * If buffer is full, compute it. + */ + if (context->counter >= CONFIG_MD2_BLOCK_LEN) + { + md2_compute(context->state, context->checksum, context->buffer); + context->counter = 0; + } + } + + +} +/** + * Ends an MD2 message digest operation. + * This fuction take an context and return a pointer + * to context state. + * + * \param context in input. + * \return a pointer to context state (message digest). + */ +uint8_t *md2_end(Md2Context *context) +{ + + uint8_t buf[CONFIG_MD2_BLOCK_LEN]; + + /* + * Fill remaning empty context buffer. + */ + md2_pad(buf, CONFIG_MD2_BLOCK_LEN - context->counter); + + /* + * Update context buffer and compute it. + */ + md2_update(context, buf, CONFIG_MD2_BLOCK_LEN - context->counter); + + /* + * Add context checksum to message input. + */ + md2_update(context, context->checksum, CONFIG_MD2_BLOCK_LEN); + + + return context->state; //return a pointer to message digest. +} +/** + * MD2 test fuction. + * This function test MD2 algorithm with a standard string specified + * in RFC 1319. + * + * \note This test work with official array of 256 byte pemutation + * contructed from digits of pi, defined in the RFC 1319. + * + */ +bool md2_test(void) +{ + + Md2Context context; + + const char *test[] = + { + "", + "message digest", + "abcdefghijklmnopqrstuvwxyz", + "12345678901234567890123456789012345678901234567890123456789012345678901234567890" + }; + + + const char *result[] = { + "\x83\x50\xe5\xa3\xe2\x4c\x15\x3d\xf2\x27\x5c\x9f\x80\x69\x27\x73", + "\xab\x4f\x49\x6b\xfb\x2a\x53\x0b\x21\x9f\xf3\x30\x31\xfe\x06\xb0", + "\x4e\x8d\xdf\xf3\x65\x02\x92\xab\x5a\x41\x08\xc3\xaa\x47\x94\x0b", + "\xd5\x97\x6f\x79\xd8\x3d\x3a\x0d\xc9\x80\x6c\x3c\x66\xf3\xef\xd8", + }; + + + for (size_t i = 0; i < countof(test); i++) + { + md2_init(&context); + md2_update(&context, test[i], strlen(test[i])); + + if(memcmp(result[i], md2_end(&context), MD2_DIGEST_LEN)) + return false; + } + + return true; +} + +#if 0 + +#include <stdio.h> +int main(int argc, char * argv[]) +{ + + if(md2_test()) + printf("MD2 algorithm work well!\n"); + else + printf("MD2 algorithm doesn't work well.\n"); + +} + +#endif + diff --git a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/md2.h b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/md2.h new file mode 100644 index 0000000..f7642b9 --- /dev/null +++ b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/md2.h @@ -0,0 +1,74 @@ +/** + * \file + * <!-- + * This file is part of BeRTOS. + * + * Bertos is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + * As a special exception, you may use this file as part of a free software + * library without restriction. Specifically, if other files instantiate + * templates or use macros or inline functions from this file, or you compile + * this file and link it with other files to produce an executable, this + * file does not by itself cause the resulting executable to be covered by + * the GNU General Public License. This exception does not however + * invalidate any other reasons why the executable file might be covered by + * the GNU General Public License. + * + * Copyright 2007 Develer S.r.l. (http://www.develer.com/) + * + * --> + * + * \brief MD2 Message-Digest algorithm. + * + * The algorithm takes as input a message of arbitrary length and produces + * as output a 128-bit message digest of the input. + * It is conjectured that it is computationally infeasible to produce + * two messages having the same message digest, or to produce any + * message having a given prespecified target message digest. + * + * + * \author Daniele Basile <as...@de...> + * + * $WIZ$ module_name = "md2" + * $WIZ$ module_configuration = "bertos/cfg/cfg_md2.h" + */ + +#ifndef ALGO_MD2_H +#define ALGO_MD2_H + +#include "cfg/cfg_md2.h" +#include <cfg/compiler.h> + +#define NUM_COMPUTE_ROUNDS 18 ///< Number of compute rounds. +#define COMPUTE_ARRAY_LEN CONFIG_MD2_BLOCK_LEN * 3 ///< Lenght of compute array. +#define MD2_DIGEST_LEN CONFIG_MD2_BLOCK_LEN +/** + * Context for MD2 computation. + */ +typedef struct Md2Context +{ + uint8_t buffer[CONFIG_MD2_BLOCK_LEN]; ///< Input buffer. + uint8_t state[CONFIG_MD2_BLOCK_LEN]; ///< Current state buffer. + uint8_t checksum[CONFIG_MD2_BLOCK_LEN]; ///< Checksum. + size_t counter; ///< Counter of remaining bytes. + +} Md2Context; + +void md2_init(Md2Context *context); +void md2_update(Md2Context *context, const void *block_in, size_t block_len); +uint8_t *md2_end(Md2Context *context); +bool md2_test(void); + +#endif /* ALGO_MD2_H */ diff --git a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/mean.h b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/mean.h new file mode 100644 index 0000000..893241e --- /dev/null +++ b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/mean.h @@ -0,0 +1,41 @@ +#warning revise me! + + +/** + * DECLARE_SMEAN(temperature, uint8_t, uint16_t); + * for (i = 0; i < TEMP_MEANS; ++i) + * SMEAN_ADD(temperature, adc_get(), TEMP_MEANS); + * printf("mean temperature = %d\n", SMEAN_GET(temperature)); + */ + +/** + * Instantiate a mean instance + */ +#define DECLARE_SMEAN(name, Type, SumType) \ + struct { \ + SumType sum; \ + Type result; \ + int count; \ + } name = { 0, 0, 0 } + +/** + * Insert a new sample into the mean. + * + * \note \a mean and \a max_samples are evaluated multiple times + */ +#define SMEAN_ADD(mean, sample, max_samples) \ + do { \ + (mean).sum += (sample); \ + if ((mean).count++ >= (max_samples)) \ + { \ + (mean).result = (mean).sum / (max_samples); \ + (mean).sum = 0; \ + (mean).count = 0; \ + } \ + } while (0) + +/** + * Return current mean value. + */ +#define SMEAN_GET(mean) ((mean).result) + diff --git a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/pid_control.c b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/pid_control.c new file mode 100644 index 0000000..73e7ffd --- /dev/null +++ b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/pid_control.c @@ -0,0 +1,115 @@ +/** + * \file + * <!-- + * This file is part of BeRTOS. + * + * Bertos is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + * As a special exception, you may use this file as part of a free software + * library without restriction. Specifically, if other files instantiate + * templates or use macros or inline functions from this file, or you compile + * this file and link it with other files to produce an executable, this + * file does not by itself cause the resulting executable to be covered by + * the GNU General Public License. This exception does not however + * invalidate any other reasons why the executable file might be covered by + * the GNU General Public License. + * + * Copyright 2008 Develer S.r.l. (http://www.develer.com/) + * --> + * + * + * \brief Proportional, integral, derivative controller (PID controller) (implementation) + * + * + * \author Daniele Basile <as...@de...> + */ + +#include "pid_control.h" + +#include "cfg/cfg_pid.h" + +// Define logging setting (for cfg/log.h module). +#define LOG_LEVEL PID_LOG_LEVEL +#define LOG_VERBOSITY PID_LOG_FORMAT + +#include <cfg/log.h> +#include <cfg/debug.h> + +/** + * Compute next value for reaching \a target point. + */ +piddata_t pid_control_update(PidContext *pid_ctx, piddata_t target, piddata_t curr_pos) +{ + piddata_t P; + piddata_t I; + piddata_t D; + piddata_t err; + + //Compute current error. + err = target - curr_pos; + + /* + * Compute Proportional contribute + */ + P = err * pid_ctx->cfg->kp; + + //Update integral state error + pid_ctx->i_state += err; + + //Clamp integral state between i_min and i_max + pid_ctx->i_state = MINMAX(pid_ctx->cfg->i_min, pid_ctx->i_state, pid_ctx->cfg->i_max); + + /* + * Compute Integral contribute + * + * note: for computing the integral contribute we use a sample period in seconds + * and so we divide sample_period in microsenconds for 1000. + */ + I = pid_ctx->i_state * pid_ctx->cfg->ki * ((piddata_t)pid_ctx->cfg->sample_period / 1000); + + + /* + * Compute derivative contribute + */ + D = (err - pid_ctx->prev_err) * pid_ctx->cfg->kd / ((piddata_t)pid_ctx->cfg->sample_period / 1000); + + + LOG_INFO("curr_pos[%lf],tgt[%lf],err[%f],P[%f],I[%f],D[%f]", curr_pos, target, err, P, I, D); + + + //Store the last error value + pid_ctx->prev_err = err; + piddata_t pid = MINMAX(pid_ctx->cfg->out_min, (P + I + D), pid_ctx->cfg->out_max); + + LOG_INFO("pid[%lf]",pid); + + //Clamp out between out_min and out_max + return pid; +} + +/** + * Init PID control. + */ +void pid_control_init(PidContext *pid_ctx, const PidCfg *pid_cfg) +{ + /* + * Init all values of pid control struct + */ + pid_ctx->cfg = pid_cfg; + + pid_control_reset(pid_ctx); + +} + diff --git a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/pid_control.h b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/pid_control.h new file mode 100644 index 0000000..c21319a --- /dev/null +++ b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/pid_control.h @@ -0,0 +1,115 @@ +/** + * \file + * <!-- + * This file is part of BeRTOS. + * + * Bertos is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + * As a special exception, you may use this file as part of a free software + * library without restriction. Specifically, if other files instantiate + * templates or use macros or inline functions from this file, or you compile + * this file and link it with other files to produce an executable, this + * file does not by itself cause the resulting executable to be covered by + * the GNU General Public License. This exception does not however + * invalidate any other reasons why the executable file might be covered by + * the GNU General Public License. + * + * Copyright 2008 Develer S.r.l. (http://www.develer.com/) + * --> + * + * + * \brief Proportional, integral, derivative controller (PID controller). + * + * + * \author Daniele Basile <as...@de...> + * + * $WIZ$ module_name = "pid_control" + * $WIZ$ module_depends = "timer" + */ + +#ifndef ALGO_PID_CONTROL_H +#define ALGO_PID_CONTROL_H + +#include <drv/timer.h> + +/** + * Data type for pid coefficient. + */ +typedef float pidk_t; +typedef float piddata_t; + +/** + * PID context structure. + */ +typedef struct PidCfg +{ + pidk_t kp; ///< Proportional term of PID control method (Gain). + pidk_t ki; ///< Integral term of PID control method (Integral Gain). + pidk_t kd; ///< Derivative of PID control method (Derivative Gain). + + piddata_t i_max; ///< Max value of integral term. + piddata_t i_min; ///< Min value of integral term. + + piddata_t out_max; ///< Man value of output. + piddata_t out_min; ///< Min value of output. + + mtime_t sample_period; ///< Sample period in milliseconds. + +} PidCfg; + + +/** + * PID context structure. + */ +typedef struct PidContext +{ + const PidCfg *cfg; + + piddata_t prev_err; ///< Previous error. + piddata_t i_state; ///< Integrator state (sum of all the preceding errors). + +} PidContext; + +/** + * Set Kp, Ki, Kd constants of PID control. + */ +INLINE void pid_control_setPid(PidCfg *pid_cfg, pidk_t kp, pidk_t ki, pidk_t kd) +{ + pid_cfg->kp = kp; + pid_cfg->ki = ki; + pid_cfg->kd = kd; +} + +/** + * Set sample period for PID control. + */ +INLINE void pid_control_setPeriod(PidCfg *pid_cfg, mtime_t sample_period) +{ + pid_cfg->sample_period = sample_period; +} + +/** + * Clear a pid control structure + */ +INLINE void pid_control_reset(PidContext *pid_ctx) +{ + pid_ctx->i_state = 0; + pid_ctx->prev_err = 0; +} + +piddata_t pid_control_update(PidContext *pid_ctx, piddata_t target, piddata_t curr_pos); +void pid_control_init(PidContext *pid_ctx, const PidCfg *pid_cfg); + +#endif /* ALGO_PID_CONTROL_H */ diff --git a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/ramp.c b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/ramp.c new file mode 100644 index 0000000..fff8a6c --- /dev/null +++ b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/ramp.c @@ -0,0 +1,200 @@ +/*! + * \file + * <!-- + * Copyright 2004, 2008 Develer S.r.l. (http://www.develer.com/) + * All Rights Reserved. + * --> + * + * \brief Compute, save and load ramps for stepper motors (implementation) + * + * + * \author Simone Zinanni <s.z...@de...> + * \author Bernie Innocenti <be...@co...> + * \author Giovanni Bajo <ra...@de...> + * \author Daniele Basile <as...@de...> + * + * + * The formula used by the ramp is the following: + * + * <pre> + * a * b + * f(t) = ------------- + * lerp(a,b,t) + * </pre> + * + * Where <code>a</code> and <code>b</code> are the maximum and minimum speed + * respectively (minimum and maximum wavelength respectively), and <code>lerp</code> + * is a linear interpolation with a factor: + * + * <pre> + * lerp(a,b,t) = a + t * (b - a) = (a * (1 - t)) + (b * t) + * </pre> + * + * <code>t</code> must be in the [0,1] interval. It is easy to see that the + * following holds true: + * + * <pre> + * f(0) = b, f(1) = a + * </pre> + * + * And that the function is monotonic. So, the function effectively interpolates + * between the maximum and minimum speed through its domain ([0,1] -> [b,a]). + * + * The curve drawn by this function is similar to 1 / (sqrt(n)), so it is slower + * than a linear acceleration (which would be 1/n). + * + * The floating point version uses a slightly modified function which accepts + * the parameter in the domain [0, MT] (where MT is maxTime, the length of the + * ramp, which is a setup parameter for the ramp). This is done to reduce the + * number of operations per step. The formula looks like this: + * + * <pre> + * a * b * MT + * g(t) = ---------------------------- + * (a * MT) + t * (b - a) + * </pre> + * + * It can be shown that this <code>g(t) = f(t * MT)</code>. The denominator + * is a linear interpolation in the range [b*MT, a*MT], as t moves in the + * interval [0, MT]. So the interpolation interval of the function is again + * [b, a]. The implementation caches the value of the numerator and parts + * of the denominator, so that the formula becomes: + * + * <pre> + * alpha = a * b * MT + * beta = a * MT + * gamma = b - a + * + * alpha + * g(t) = ---------------------- + * beta + t * gamma + * </pre> + * + * and <code>t</code> is exactly the parameter that ramp_evaluate() gets, + * that is the current time (in range [0, MT]). The operations performed + * for each step are just an addition, a multiplication and a division. + * + * The fixed point version of the formula instead transforms the original + * function as follows: + * + * <pre> + * a * b a + * f(t) = ------------------------- = -------------------- + * a a + * b * ( - * (1 - t) + t ) - * (1 - t) + t + * b b + * </pre> + * + * <code>t</code> must be computed by dividing the current time (24 bit integer) + * by the maximum time (24 bit integer). This is done by precomputing the + * reciprocal of the maximum time as a 0.32 fixed point number, and multiplying + * it to the current time. Multiplication is performed 8-bits a time by + * FIX_MULT32(), so that we end up with a 0.16 fixed point number for + * <code>t</code> (and <code>1-t</code> is just its twos-complement negation). + * <code>a/b</code> is in the range [0,1] (because a is always less than b, + * being the minimum wavelength), so it is precomputed as a 0.16 fixed point. + * The final step is then computing the denominator and executing the division + * (32 cycles using the 1-step division instruction in the DSP). + * + * The assembly implementation is needed for efficiency, but a C version of it + * can be easily written, in case it is needed in the future. + * + */ + +#include "ramp.h" +#include <cfg/debug.h> + +#include <string.h> // memcpy() + +void ramp_compute(struct Ramp *ramp, uint32_t clocksRamp, uint16_t clocksMinWL, uint16_t clocksMaxWL) +{ + ASSERT(clocksMaxWL >= clocksMinWL); + + // Save values in ramp struct + ramp->clocksRamp = clocksRamp; + ramp->clocksMinWL = clocksMinWL; + ramp->clocksMaxWL = clocksMaxWL; + +#if RAMP_USE_FLOATING_POINT + ramp->precalc.gamma = ramp->clocksMaxWL - ramp->clocksMinWL; + ramp->precalc.beta = (float)ramp->clocksMinWL * (float)ramp->clocksRamp; + ramp->precalc.alpha = ramp->precalc.beta * (float)ramp->clocksMaxWL; + +#else + ramp->precalc.max_div_min = ((uint32_t)clocksMinWL << 16) / (uint32_t)clocksMaxWL; + + /* Calcola 1/total_time in fixed point .32. Assumiamo che la rampa possa al + * massimo avere 25 bit (cioé valore in tick fino a 2^25, che con il + * prescaler=3 sono circa 7 secondi). Inoltre, togliamo qualche bit di precisione + * da destra (secondo quanto specificato in RAMP_CLOCK_SHIFT_PRECISION). + */ + ASSERT(ramp->clocksRamp < (1UL << (24 + RAMP_CLOCK_SHIFT_PRECISION))); + ramp->precalc.inv_total_time = 0xFFFFFFFFUL / (ramp->clocksRamp >> RAMP_CLOCK_SHIFT_PRECISION); + ASSERT(ramp->precalc.inv_total_time < 0x1000000UL); + +#endif +} + + +void ramp_setup(struct Ramp* ramp, uint32_t length, uint32_t minFreq, uint32_t maxFreq) +{ + uint32_t minWL, maxWL; + + minWL = TIME2CLOCKS(FREQ2MICROS(maxFreq)); + maxWL = TIME2CLOCKS(FREQ2MICROS(minFreq)); + + ASSERT2(minWL < 65536UL, "Maximum frequency too high"); + ASSERT2(maxWL < 65536UL, "Minimum frequency too high"); + ASSERT(maxFreq > minFreq); + + ramp_compute( + ramp, + TIME2CLOCKS(length), + TIME2CLOCKS(FREQ2MICROS(maxFreq)), + TIME2CLOCKS(FREQ2MICROS(minFreq)) + ); +} + +void ramp_default(struct Ramp *ramp) +{ + ramp_setup(ramp, RAMP_DEF_TIME, RAMP_DEF_MINFREQ, RAMP_DEF_MAXFREQ); +} + +#if RAMP_USE_FLOATING_POINT + +float ramp_evaluate(const struct Ramp* ramp, float curClock) +{ + return ramp->precalc.alpha / (curClock * ramp->precalc.gamma + ramp->precalc.beta); +} + +#else + +INLINE uint32_t fix_mult32(uint32_t m1, uint32_t m2) +{ + uint32_t accum = 0; + accum += m1 * ((m2 >> 0) & 0xFF); + accum >>= 8; + accum += m1 * ((m2 >> 8) & 0xFF); + accum >>= 8; + accum += m1 * ((m2 >> 16) & 0xFF); + return accum; +} + +// a*b >> 16 +INLINE uint16_t fix_mult16(uint16_t a, uint32_t b) +{ + return (b*(uint32_t)a) >> 16; +} + +uint16_t FAST_FUNC ramp_evaluate(const struct Ramp* ramp, uint32_t curClock) +{ + uint16_t t = FIX_MULT32(curClock >> RAMP_CLOCK_SHIFT_PRECISION, ramp->precalc.inv_total_time); + uint16_t denom = fix_mult16((uint16_t)~t + 1, ramp->precalc.max_div_min) + t; + uint16_t cur_delta = ((uint32_t)ramp->clocksMinWL << 16) / denom; + + return cur_delta; +} + +#endif + + diff --git a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/ramp.h b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/ramp.h new file mode 100644 index 0000000..ad3c919 --- /dev/null +++ b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/ramp.h @@ -0,0 +1,166 @@ +/** + * \file + * <!-- + * Copyright 2004, 2008 Develer S.r.l. (http://www.develer.com/) + * All Rights Reserved. + * --> + * + * \brief Compute, save and load ramps for stepper motors. + * + * The acceleration ramp is used to properly accelerate a stepper motor. The main + * entry point is the function ramp_evaluate(), which must be called at every step + * of the motor: it gets as input the time elapsed since the stepper started + * accelerating, and returns the time to wait before sending the next step. A pseudo + * usage pattern is as follows: + * + * <pre> + * float time = 0; + * while (1) + * { + * float delta = ramp_evaluate(&my_ramp, time); + * sleep(delta); + * do_motor_step(); + * time += delta; + * } + * </pre> + * + * A similar pattern can be used to decelerate (it is sufficient to move the total + * time backward, such as "time -= delta"). + * + * The ramp can be configured with ramp_setup(), providing it with the minimum and + * maximum operating frequency of the motor, and the total acceleration time in + * milliseconds (that is, the time that will be needed to accelerate from the + * minimum frequency to the maximum frequency). + * + * Both a very precise floating point and a very fast fixed point implementation + * of the ramp evaluation are provided. The fixed point is hand-optimized assembly + * for DSP56000 (but a portable C version of it can be easily written, see the + * comments in the code). + * + * + * \author Simone Zinanni <s.z...@de...> + * \author Giovanni Bajo <ra...@de...> + * \author Daniele Basile <as...@de...> + * + * $WIZ$ module_name = "ramp" + * $WIZ$ module_configuration = "bertos/cfg/cfg_ramp.h" + */ + +#ifndef ALGO_RAMP_H +#define ALGO_RAMP_H + +#include "hw/hw_stepper.h" + +#include "cfg/cfg_ramp.h" + +#include <cfg/compiler.h> + + +/** + * Convert microseconds to timer clock ticks + */ +#define TIME2CLOCKS(micros) ((uint32_t)(micros) * (STEPPER_CLOCK / 1000000)) + +/** + * Convert timer clock ticks back to microseconds + */ +#define CLOCKS2TIME(clocks) ((uint32_t)(clocks) / (STEPPER_CLOCK / 1000000)) + +/** + * Convert microseconds to Hz + */ +#define MICROS2FREQ(micros) (1000000UL / ((uint32_t)(micros))) + +/** + * Convert frequency (in Hz) to time (in microseconds) + */ +#define FREQ2MICROS(hz) (1000000UL / ((uint32_t)(hz))) + +/** + * Multiply \p a and \p b two integer at 32 bit and extract the high 16 bit word. + */ +#define FIX_MULT32(a,b) (((uint64_t)(a)*(uint32_t)(b)) >> 16) + +/** + * Structure holding pre-calculated data for speeding up real-time evaluation + * of the ramp. This structure is totally different between the fixed and the + * floating point version of the code. + * + * Consult the file-level documentation of ramp.c for more information about + * the values of this structure. + */ +struct RampPrecalc +{ +#if RAMP_USE_FLOATING_POINT + float beta; + float alpha; + float gamma; +#else + uint16_t max_div_min; + uint32_t inv_total_time; +#endif +}; + + +/** + * Ramp structure + */ +struct Ramp +{ + uint32_t clocksRamp; + uint16_t clocksMinWL; + uint16_t clocksMaxWL; + + struct RampPrecalc precalc; ///< pre-calculated values for speed +}; + + +/* + * Function prototypes + */ +void ramp_compute( + struct Ramp * ramp, + uint32_t clocksInRamp, + uint16_t clocksInMinWavelength, + uint16_t clocksInMaxWavelength); + + +/** Setup an acceleration ramp for a stepper motor + * + * \param ramp Ramp to fill + * \param length Length of the ramp (milliseconds) + * \param minFreq Minimum operating frequency of the motor (hertz) + * \param maxFreq Maximum operating frequency of the motor (hertz) + * + */ +void ramp_setup(struct Ramp* ramp, uint32_t length, uint32_t minFreq, uint32_t maxFreq); + + +/** + * Initialize a new ramp with default values + */ +void ramp_default(struct Ramp *ramp); + + +/** + * Evaluate the ramp at the given point. Given a \a ramp, and the current \a clock since + * the start of the acceleration, compute the next step, that is the interval at which + * send the signal to the motor. + * + * \note The fixed point version does not work when curClock is zero. Anyway, + * the first step is always clocksMaxWL, as stored within the ramp structure. + */ +#if RAMP_USE_FLOATING_POINT + float ramp_evaluate(const struct Ramp* ramp, float curClock); +#else + uint16_t ramp_evaluate(const struct Ramp* ramp, uint32_t curClock); +#endif + + +/** Self test */ +int ramp_testSetup(void); +int ramp_testRun(void); +int ramp_testTearDown(void); + +#endif /* ALGO_RAMP_H */ + diff --git a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/ramp_test.c b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/ramp_test.c new file mode 100644 index 0000000..0e7d58c --- /dev/null +++ b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/ramp_test.c @@ -0,0 +1,186 @@ +/*! + * \file + * <!-- + * Copyright 2004, 2008 Develer S.r.l. (http://www.develer.com/) + * All Rights Reserved. + * --> + * + * \brief Test for compute, save and load ramps for stepper motors (implementation) + * + * + * \author Simone Zinanni <s.z...@de...> + * \author Bernie Innocenti <be...@co...> + * \author Giovanni Bajo <ra...@de...> + * \author Daniele Basile <as...@de...> + * + * + * The formula used by the ramp is the following: + * + * <pre> + * a * b + * f(t) = ------------- + * lerp(a,b,t) + * </pre> + * + * Where <code>a</code> and <code>b</code> are the maximum and minimum speed + * respectively (minimum and maximum wavelength respectively), and <code>lerp</code> + * is a linear interpolation with a factor: + * + * <pre> + * lerp(a,b,t) = a + t * (b - a) = (a * (1 - t)) + (b * t) + * </pre> + * + * <code>t</code> must be in the [0,1] interval. It is easy to see that the + * following holds true: + * + * <pre> + * f(0) = b, f(1) = a + * </pre> + * + * And that the function is monotonic. So, the function effectively interpolates + * between the maximum and minimum speed through its domain ([0,1] -> [b,a]). + * + * The curve drawn by this function is similar to 1 / (sqrt(n)), so it is slower + * than a linear acceleration (which would be 1/n). + * + * The floating point version uses a slightly modified function which accepts + * the parameter in the domain [0, MT] (where MT is maxTime, the length of the + * ramp, which is a setup parameter for the ramp). This is done to reduce the + * number of operations per step. The formula looks like this: + * + * <pre> + * a * b * MT + * g(t) = ---------------------------- + * (a * MT) + t * (b - a) + * </pre> + * + * It can be shown that this <code>g(t) = f(t * MT)</code>. The denominator + * is a linear interpolation in the range [b*MT, a*MT], as t moves in the + * interval [0, MT]. So the interpolation interval of the function is again + * [b, a]. The implementation caches the value of the numerator and parts + * of the denominator, so that the formula becomes: + * + * <pre> + * alpha = a * b * MT + * beta = a * MT + * gamma = b - a + * + * alpha + * g(t) = ---------------------- + * beta + t * gamma + * </pre> + * + * and <code>t</code> is exactly the parameter that ramp_evaluate() gets, + * that is the current time (in range [0, MT]). The operations performed + * for each step are just an addition, a multiplication and a division. + * + * The fixed point version of the formula instead transforms the original + * function as follows: + * + * <pre> + * a * b a + * f(t) = ------------------------- = -------------------- + * a a + * b * ( - * (1 - t) + t ) - * (1 - t) + t + * b b + * </pre> + * + * <code>t</code> must be computed by dividing the current time (24 bit integer) + * by the maximum time (24 bit integer). This is done by precomputing the + * reciprocal of the maximum time as a 0.32 fixed point number, and multiplying + * it to the current time. Multiplication is performed 8-bits a time by + * FIX_MULT32(), so that we end up with a 0.16 fixed point number for + * <code>t</code> (and <code>1-t</code> is just its twos-complement negation). + * <code>a/b</code> is in the range [0,1] (because a is always less than b, + * being the minimum wavelength), so it is precomputed as a 0.16 fixed point. + * The final step is then computing the denominator and executing the division + * (32 cycles using the 1-step division instruction in the DSP). + * + * The assembly implementation is needed for efficiency, but a C version of it + * can be easily written, in case it is needed in the future. + * + */ + +#include "ramp.h" +#include <cfg/debug.h> +#include <cfg/test.h> + + +static bool ramp_test_single(uint32_t minFreq, uint32_t maxFreq, uint32_t length) +{ + struct Ramp r; + uint16_t cur, old; + uint32_t clock; + uint32_t oldclock; + + ramp_setup(&r, length, minFreq, maxFreq); + + cur = old = r.clocksMaxWL; + clock = 0; + oldclock = 0; + + kprintf("testing ramp: (length=%lu, min=%lu, max=%lu)\n", (unsigned long)length, (unsigned long)minFreq, (unsigned long)maxFreq); + kprintf(" [length=%lu, max=%04x, min=%04x]\n", (unsigned long)r.clocksRamp, r.clocksMaxWL, r.clocksMinWL); + + int i = 0; + int nonbyte = 0; + + while (clock + cur < r.clocksRamp) + { + oldclock = clock; + old = cur; + + clock += cur; + cur = ramp_evaluate(&r, clock); + + if (old < cur) + { + uint16_t t1 = FIX_MULT32(oldclock >> RAMP_CLOCK_SHIFT_PRECISION, r.precalc.inv_total_time); + uint16_t t2 = FIX_MULT32(clock >> RAMP_CLOCK_SHIFT_PRECISION, r.precalc.inv_total_time); + uint16_t denom1 = FIX_MULT32((uint16_t)((~t1) + 1), r.precalc.max_div_min) + t1; + uint16_t denom2 = FIX_MULT32((uint16_t)((~t2) + 1), r.precalc.max_div_min) + t2; + + kprintf(" Failed: %04x @ %lu --> %04x @ %lu\n", old, (unsigned long)oldclock, cur, (unsigned long)clock); + kprintf(" T: %04x -> %04x\n", t1, t2); + kprintf(" DENOM: %04x -> %04x\n", denom1, denom2); + + cur = ramp_evaluate(&r, clock); + return false; + } + i++; + if ((old-cur) >= 256) + nonbyte++; + } + + + + kprintf("Test finished: %04x @ %lu [min=%04x, totlen=%lu, numsteps:%d, nonbyte:%d]\n", cur, (unsigned long)clock, r.clocksMinWL, (unsigned long)r.clocksRamp, i, nonbyte); + + return true; +} + +int ramp_testSetup(void) +{ + kdbg_init(); + return 0; +} + +int ramp_testTearDown(void) +{ + return 0; +} + +int ramp_testRun(void) +{ + #define TEST_RAMP(min, max, len) do { \ + if (!ramp_test_single(min, max, len)) \ + return -1; \ + } while(0) + + TEST_RAMP(200, 5000, 3000000); + TEST_RAMP(1000, 2000, 1000000); + + return 0; +} + +TEST_MAIN(ramp); diff --git a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/rand.c b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/rand.c new file mode 100644 index 0000000..901e23f --- /dev/null +++ b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/algo/rand.c @@ -0,0 +1,52 @@ +/** + * \file + * <!-- + * This file is part of BeRTOS. + * + * Bertos is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + * As a special exception, you may use this file as part of a free software + * library without restriction. Specifically, if other files ins... [truncated message content] |