From 27a0072e1bb6a82417c9e541448c8610c1731386 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Wed, 6 Apr 2016 23:56:01 +0200 Subject: [PATCH 1/3] removed FastSerial & other unneeded libs - removed FastSerial (unneeded) - BetterStream part of FastSerial spawned into separate lib. - removed EEPROM lib (use default lib instead) - removed AP_Common & AP_Math libs (unneeded) --- libraries/AP_Common/AP_Common.cpp | 17 - libraries/AP_Common/AP_Common.h | 255 ----- libraries/AP_Common/AP_Loop.cpp | 52 - libraries/AP_Common/AP_Loop.h | 60 -- libraries/AP_Common/AP_MetaClass.cpp | 36 - libraries/AP_Common/AP_MetaClass.h | 259 ----- libraries/AP_Common/AP_Param.cpp | 892 ------------------ libraries/AP_Common/AP_Param.h | 473 ---------- libraries/AP_Common/AP_Test.h | 127 --- libraries/AP_Common/AP_Var_menufuncs.cpp | 15 - libraries/AP_Common/AP_Vector.h | 382 -------- libraries/AP_Common/Arduino.mk | 654 ------------- libraries/AP_Common/c++.cpp | 91 -- libraries/AP_Common/c++.h | 7 - libraries/AP_Common/examples/menu/menu.pde | 43 - libraries/AP_Common/include/menu.h | 143 --- libraries/AP_Common/keywords.txt | 4 - libraries/AP_Common/menu.cpp | 155 --- libraries/AP_Common/tools/eedump.c | 84 -- libraries/AP_Common/tools/eedump.pl | 47 - libraries/AP_Common/tools/eedump_apparam.c | 181 ---- libraries/AP_Common/tools/eedump_apparam.pl | 78 -- libraries/AP_Math/AP_Math.cpp | 69 -- libraries/AP_Math/AP_Math.h | 32 - libraries/AP_Math/examples/eulers/Makefile | 4 - libraries/AP_Math/examples/eulers/eulers.pde | 281 ------ libraries/AP_Math/examples/polygon/Makefile | 1 - .../AP_Math/examples/polygon/polygon.pde | 115 --- libraries/AP_Math/examples/rotations/Makefile | 4 - .../AP_Math/examples/rotations/rotations.pde | 205 ---- libraries/AP_Math/keywords.txt | 29 - libraries/AP_Math/matrix3.cpp | 199 ---- libraries/AP_Math/matrix3.h | 153 --- libraries/AP_Math/polygon.cpp | 91 -- libraries/AP_Math/polygon.h | 22 - libraries/AP_Math/quaternion.cpp | 89 -- libraries/AP_Math/quaternion.h | 48 - libraries/AP_Math/rotations.h | 46 - libraries/AP_Math/vector2.h | 157 --- libraries/AP_Math/vector3.cpp | 115 --- libraries/AP_Math/vector3.h | 199 ---- .../BetterStream.cpp | 8 +- .../BetterStream.h | 10 +- .../ftoa_engine.S | 0 .../ftoa_engine.h | 0 .../{FastSerial => BetterStream}/macros.inc | 0 libraries/{FastSerial => BetterStream}/ntz.h | 0 .../ultoa_invert.S | 0 .../{FastSerial => BetterStream}/vprintf.cpp | 4 +- .../{FastSerial => BetterStream}/xtoa_fast.h | 0 libraries/EEPROM/EEPROM.cpp | 50 - libraries/EEPROM/EEPROM.h | 35 - .../examples/eeprom_clear/eeprom_clear.ino | 23 - .../examples/eeprom_read/eeprom_read.ino | 43 - .../examples/eeprom_write/eeprom_write.ino | 38 - libraries/EEPROM/keywords.txt | 18 - libraries/FastSerial/FastSerial.cpp | 304 ------ libraries/FastSerial/FastSerial.h | 334 ------- .../examples/FastSerial/FastSerial.pde | 71 -- .../FastSerial/examples/FastSerial/Makefile | 2 - libraries/FastSerial/keywords.txt | 7 - rx5808_pro_osd/ArduCam_Max7456.cpp | 2 +- rx5808_pro_osd/ArduCam_Max7456.h | 1 + rx5808_pro_osd/rx5808_pro_osd.ino | 13 +- 64 files changed, 17 insertions(+), 6860 deletions(-) delete mode 100644 libraries/AP_Common/AP_Common.cpp delete mode 100644 libraries/AP_Common/AP_Common.h delete mode 100644 libraries/AP_Common/AP_Loop.cpp delete mode 100644 libraries/AP_Common/AP_Loop.h delete mode 100644 libraries/AP_Common/AP_MetaClass.cpp delete mode 100644 libraries/AP_Common/AP_MetaClass.h delete mode 100644 libraries/AP_Common/AP_Param.cpp delete mode 100644 libraries/AP_Common/AP_Param.h delete mode 100644 libraries/AP_Common/AP_Test.h delete mode 100644 libraries/AP_Common/AP_Var_menufuncs.cpp delete mode 100644 libraries/AP_Common/AP_Vector.h delete mode 100644 libraries/AP_Common/Arduino.mk delete mode 100644 libraries/AP_Common/c++.cpp delete mode 100644 libraries/AP_Common/c++.h delete mode 100644 libraries/AP_Common/examples/menu/menu.pde delete mode 100644 libraries/AP_Common/include/menu.h delete mode 100644 libraries/AP_Common/keywords.txt delete mode 100644 libraries/AP_Common/menu.cpp delete mode 100644 libraries/AP_Common/tools/eedump.c delete mode 100644 libraries/AP_Common/tools/eedump.pl delete mode 100644 libraries/AP_Common/tools/eedump_apparam.c delete mode 100644 libraries/AP_Common/tools/eedump_apparam.pl delete mode 100644 libraries/AP_Math/AP_Math.cpp delete mode 100644 libraries/AP_Math/AP_Math.h delete mode 100644 libraries/AP_Math/examples/eulers/Makefile delete mode 100644 libraries/AP_Math/examples/eulers/eulers.pde delete mode 100644 libraries/AP_Math/examples/polygon/Makefile delete mode 100644 libraries/AP_Math/examples/polygon/polygon.pde delete mode 100644 libraries/AP_Math/examples/rotations/Makefile delete mode 100644 libraries/AP_Math/examples/rotations/rotations.pde delete mode 100644 libraries/AP_Math/keywords.txt delete mode 100644 libraries/AP_Math/matrix3.cpp delete mode 100644 libraries/AP_Math/matrix3.h delete mode 100644 libraries/AP_Math/polygon.cpp delete mode 100644 libraries/AP_Math/polygon.h delete mode 100644 libraries/AP_Math/quaternion.cpp delete mode 100644 libraries/AP_Math/quaternion.h delete mode 100644 libraries/AP_Math/rotations.h delete mode 100644 libraries/AP_Math/vector2.h delete mode 100644 libraries/AP_Math/vector3.cpp delete mode 100644 libraries/AP_Math/vector3.h rename libraries/{FastSerial => BetterStream}/BetterStream.cpp (83%) rename libraries/{FastSerial => BetterStream}/BetterStream.h (77%) rename libraries/{FastSerial => BetterStream}/ftoa_engine.S (100%) rename libraries/{FastSerial => BetterStream}/ftoa_engine.h (100%) rename libraries/{FastSerial => BetterStream}/macros.inc (100%) rename libraries/{FastSerial => BetterStream}/ntz.h (100%) rename libraries/{FastSerial => BetterStream}/ultoa_invert.S (100%) rename libraries/{FastSerial => BetterStream}/vprintf.cpp (99%) rename libraries/{FastSerial => BetterStream}/xtoa_fast.h (100%) delete mode 100644 libraries/EEPROM/EEPROM.cpp delete mode 100644 libraries/EEPROM/EEPROM.h delete mode 100644 libraries/EEPROM/examples/eeprom_clear/eeprom_clear.ino delete mode 100644 libraries/EEPROM/examples/eeprom_read/eeprom_read.ino delete mode 100644 libraries/EEPROM/examples/eeprom_write/eeprom_write.ino delete mode 100644 libraries/EEPROM/keywords.txt delete mode 100644 libraries/FastSerial/FastSerial.cpp delete mode 100644 libraries/FastSerial/FastSerial.h delete mode 100644 libraries/FastSerial/examples/FastSerial/FastSerial.pde delete mode 100644 libraries/FastSerial/examples/FastSerial/Makefile delete mode 100644 libraries/FastSerial/keywords.txt diff --git a/libraries/AP_Common/AP_Common.cpp b/libraries/AP_Common/AP_Common.cpp deleted file mode 100644 index 5b476c4..0000000 --- a/libraries/AP_Common/AP_Common.cpp +++ /dev/null @@ -1,17 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -// -// This is free software; you can redistribute it and/or modify it under -// the terms of the GNU Lesser General Public License as published by the -// Free Software Foundation; either version 2.1 of the License, or (at -// your option) any later version. -// - -/// @file AP_Common.cpp -/// @brief Common utility routines for the ArduPilot libraries. -/// -/// @note Exercise restraint adding code here; everything in this -/// library will be linked with any sketch using it. -/// - -#include "AP_Common.h" - diff --git a/libraries/AP_Common/AP_Common.h b/libraries/AP_Common/AP_Common.h deleted file mode 100644 index 9b235d9..0000000 --- a/libraries/AP_Common/AP_Common.h +++ /dev/null @@ -1,255 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -// -// This is free software; you can redistribute it and/or modify it under -// the terms of the GNU Lesser General Public License as published by the -// Free Software Foundation; either version 2.1 of the License, or (at -// your option) any later version. -// - -/// -/// @file AP_Common.h -/// @brief Common definitions and utility routines for the ArduPilot -/// libraries. -/// - -#ifndef _AP_COMMON_H -#define _AP_COMMON_H - -// Get the common arduino functions -#if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" -#else - #include "wiring.h" -#endif -// ... and remove some of their stupid macros -#undef round -#undef abs - -// prog_char_t is used as a wrapper type for prog_char, which is -// a character stored in flash. By using this wrapper type we can -// auto-detect at compile time if a call to a string function is using -// a flash-stored string or not -typedef struct { - char c; -} prog_char_t; - -#include -#include "include/menu.h" /// simple menu subsystem -#include "c++.h" // c++ additions -//#include "AP_Vector.h" -//#include "AP_Loop.h" - -// default to AP_Param system, unless USE_AP_VAR is defined -#ifdef USE_AP_VAR -#include "AP_Var.h" -#else -#include "AP_Param.h" -#endif - -//////////////////////////////////////////////////////////////////////////////// -/// @name Warning control -//@{ -// -// Turn on/off warnings of interest. -// -// These warnings are normally suppressed by the Arduino IDE, -// but with some minor hacks it's possible to have warnings -// emitted. This helps greatly when diagnosing subtle issues. -// -#pragma GCC diagnostic warning "-Wall" -#pragma GCC diagnostic warning "-Wextra" -#pragma GCC diagnostic warning "-Wlogical-op" -#pragma GCC diagnostic ignored "-Wredundant-decls" - -// Make some dire warnings into errors -// -// Some warnings indicate questionable code; rather than let -// these slide, we force them to become errors so that the -// developer has to find a safer alternative. -// -//#pragma GCC diagnostic error "-Wfloat-equal" - -// The following is strictly for type-checking arguments to printf_P calls -// in conjunction with a suitably modified Arduino IDE; never define for -// production as it generates bad code. -// -#if PRINTF_FORMAT_WARNING_DEBUG -# undef PSTR -# define PSTR(_x) _x // help the compiler with printf_P -# define float double // silence spurious format warnings for %f -#else -// This is a workaround for GCC bug c++/34734. -// -// The C++ compiler normally emits many spurious warnings for the use -// of PSTR (even though it generates correct code). This workaround -// has an equivalent effect but avoids the warnings, which otherwise -// make finding real issues difficult. -// -#ifdef DESKTOP_BUILD -# undef PROGMEM -# define PROGMEM __attribute__(()) -#else -# undef PROGMEM -# define PROGMEM __attribute__(( section(".progmem.data") )) -#endif - -# undef PSTR -# define PSTR(s) (__extension__({static prog_char __c[] PROGMEM = (s); \ - (prog_char_t *)&__c[0];})) -#endif - -// a varient of PSTR() for progmem strings passed to %S in printf() -// this gets the gcc __format__ checking right -#define FPSTR(s) (wchar_t *)(s) - - -static inline int strcasecmp_P(const char *str1, const prog_char_t *pstr) -{ - return strcasecmp_P(str1, (const prog_char *)pstr); -} - -static inline int strcmp_P(const char *str1, const prog_char_t *pstr) -{ - return strcmp_P(str1, (const prog_char *)pstr); -} - -static inline size_t strlen_P(const prog_char_t *pstr) -{ - return strlen_P((const prog_char *)pstr); -} - -static inline void *memcpy_P(void *dest, const prog_char_t *src, size_t n) -{ - return memcpy_P(dest, (const prog_char *)src, n); -} - -// strlcat_P() in AVR libc seems to be broken -static inline size_t strlcat_P(char *d, const prog_char_t *s, size_t bufsize) -{ - size_t len1 = strlen(d); - size_t len2 = strlen_P(s); - size_t ret = len1 + len2; - - if (len1+len2 >= bufsize) { - if (bufsize < (len1+1)) { - return ret; - } - len2 = bufsize - (len1+1); - } - if (len2 > 0) { - memcpy_P(d+len1, s, len2); - d[len1+len2] = 0; - } - return ret; -} - -static inline char *strncpy_P(char *buffer, const prog_char_t *pstr, size_t buffer_size) -{ - return strncpy_P(buffer, (const prog_char *)pstr, buffer_size); -} - - -// read something the size of a pointer. This makes the menu code more -// portable -static inline uintptr_t pgm_read_pointer(const void *s) -{ - if (sizeof(uintptr_t) == sizeof(uint16_t)) { - return (uintptr_t)pgm_read_word(s); - } else { - union { - uintptr_t p; - uint8_t a[sizeof(uintptr_t)]; - } u; - uint8_t i; - for (i=0; i< sizeof(uintptr_t); i++) { - u.a[i] = pgm_read_byte(i + (const prog_char *)s); - } - return u.p; - } -} - -//@} - - -/// -/// @name Macros -/// @{ - -/// Define a constant string in program memory. This is a little more obvious -/// and less error-prone than typing the declaration out by hand. It's required -/// when passing PROGMEM strings to static object constructors because the PSTR -/// hack can't be used at global scope. -/// -#define PROGMEM_STRING(_v, _s) static const char _v[] PROGMEM = _s - -#define ToRad(x) (x*0.01745329252) // *pi/180 -#define ToDeg(x) (x*57.2957795131) // *180/pi -// @} - - -//////////////////////////////////////////////////////////////////////////////// -/// @name Types -/// -/// Data structures and types used throughout the libraries and applications. 0 = default -/// bit 0: Altitude is stored 0: Absolute, 1: Relative -/// bit 1: Chnage Alt between WP 0: Gradually, 1: ASAP -/// bit 2: -/// bit 3: Req.to hit WP.alt to continue 0: No, 1: Yes -/// bit 4: Relative to Home 0: No, 1: Yes -/// bit 5: -/// bit 6: -/// bit 7: Move to next Command 0: YES, 1: Loiter until commanded - -//@{ - -struct Location { - uint8_t id; ///< command id - uint8_t options; ///< options bitmask (1<<0 = relative altitude) - uint8_t p1; ///< param 1 - int32_t alt; ///< param 2 - Altitude in centimeters (meters * 100) - int32_t lat; ///< param 3 - Lattitude * 10**7 - int32_t lng; ///< param 4 - Longitude * 10**7 -}; - -//@} - -//////////////////////////////////////////////////////////////////////////////// -/// @name Conversions -/// -/// Conversion macros and factors. -/// -//@{ - -/// XXX this should probably be replaced with radians()/degrees(), but their -/// inclusion in wiring.h makes doing that here difficult. -#define ToDeg(x) (x*57.2957795131) // *180/pi -#define ToRad(x) (x*0.01745329252) // *pi/180 - -//@} - -#ifdef DESKTOP_BUILD -// used to report serious errors in autotest -# define SITL_debug(fmt, args...) fprintf(stdout, "%s:%u " fmt, __FUNCTION__, __LINE__, ##args) -#else -# define SITL_debug(fmt, args...) -#endif - -/* Product IDs for all supported products follow */ - -#define AP_PRODUCT_ID_NONE 0x00 // Hardware in the loop -#define AP_PRODUCT_ID_APM1_1280 0x01 // APM1 with 1280 CPUs -#define AP_PRODUCT_ID_APM1_2560 0x02 // APM1 with 2560 CPUs -#define AP_PRODUCT_ID_SITL 0x03 // Software in the loop -#define AP_PRODUCT_ID_APM2ES_REV_C4 0x14 // APM2 with MPU6000ES_REV_C4 -#define AP_PRODUCT_ID_APM2ES_REV_C5 0x15 // APM2 with MPU6000ES_REV_C5 -#define AP_PRODUCT_ID_APM2ES_REV_D6 0x16 // APM2 with MPU6000ES_REV_D6 -#define AP_PRODUCT_ID_APM2ES_REV_D7 0x17 // APM2 with MPU6000ES_REV_D7 -#define AP_PRODUCT_ID_APM2ES_REV_D8 0x18 // APM2 with MPU6000ES_REV_D8 -#define AP_PRODUCT_ID_APM2_REV_C4 0x54 // APM2 with MPU6000_REV_C4 -#define AP_PRODUCT_ID_APM2_REV_C5 0x55 // APM2 with MPU6000_REV_C5 -#define AP_PRODUCT_ID_APM2_REV_D6 0x56 // APM2 with MPU6000_REV_D6 -#define AP_PRODUCT_ID_APM2_REV_D7 0x57 // APM2 with MPU6000_REV_D7 -#define AP_PRODUCT_ID_APM2_REV_D8 0x58 // APM2 with MPU6000_REV_D8 -#define AP_PRODUCT_ID_APM2_REV_D9 0x59 // APM2 with MPU6000_REV_D9 - -#endif // _AP_COMMON_H diff --git a/libraries/AP_Common/AP_Loop.cpp b/libraries/AP_Common/AP_Loop.cpp deleted file mode 100644 index 031c59c..0000000 --- a/libraries/AP_Common/AP_Loop.cpp +++ /dev/null @@ -1,52 +0,0 @@ -/* - * AP_Loop.pde - * Copyright (C) James Goppert 2010 - * - * This file 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 3 of the License, or - * (at your option) any later version. - * - * This file 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, see . - */ - -#include "AP_Loop.h" - -Loop::Loop(float _frequency, void (*fptr)(void *), void * data) : - _fptr(fptr), - _data(data), - _period(1.0e6/_frequency), - _subLoops(), - _timeStamp(micros()), - _load(0), - _dt(0) -{ -} - -void Loop::update() -{ - // quick exit if not ready - if (micros() - _timeStamp < _period) return; - - // update time stamp - uint32_t timeStamp0 = _timeStamp; - _timeStamp = micros(); - _dt = (_timeStamp - timeStamp0)/1.0e6; - - // update sub loops - for (uint8_t i=0; i<_subLoops.getSize(); i++) _subLoops[i]->update(); - - // callback function - if (_fptr) _fptr(_data); - - // calculated load with a low pass filter - _load = 0.9*_load + 10*(float(micros()-_timeStamp)/(_timeStamp-timeStamp0)); -} - -// vim:ts=4:sw=4:expandtab diff --git a/libraries/AP_Common/AP_Loop.h b/libraries/AP_Common/AP_Loop.h deleted file mode 100644 index 313ce43..0000000 --- a/libraries/AP_Common/AP_Loop.h +++ /dev/null @@ -1,60 +0,0 @@ -/* - * AP_Loop.h - * Copyright (C) James Goppert 2010 - * - * This file 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 3 of the License, or - * (at your option) any later version. - * - * This file 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, see . - */ - -#ifndef AP_Loop_H -#define AP_Loop_H - -#include "AP_Vector.h" - -class Loop -{ -public: - Loop() : _fptr(), _data(), _period(), _subLoops(), _timeStamp(), _load(), _dt() {}; - Loop(float frequency, void (*fptr)(void *) = NULL, void * data = NULL); - void update(); - Vector & subLoops() { - return _subLoops; - } - float frequency() { - return 1.0e6/_period; - } - void frequency(float _frequency) { - _period = 1e6/_frequency; - } - uint32_t timeStamp() { - return _timeStamp; - } - float dt() { - return _dt; - } - uint8_t load() { - return _load; - } -protected: - void (*_fptr)(void *); - void * _data; - uint32_t _period; - Vector _subLoops; - uint32_t _timeStamp; - uint8_t _load; - float _dt; -}; - -#endif - -// vim:ts=4:sw=4:expandtab diff --git a/libraries/AP_Common/AP_MetaClass.cpp b/libraries/AP_Common/AP_MetaClass.cpp deleted file mode 100644 index d47cbd4..0000000 --- a/libraries/AP_Common/AP_MetaClass.cpp +++ /dev/null @@ -1,36 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -// -// This is free software; you can redistribute it and/or modify it under -// the terms of the GNU Lesser General Public License as published by the -// Free Software Foundation; either version 2.1 of the License, or (at -// your option) any later version. -// - -/// @file AP_MetaClass.cpp -/// Abstract meta-class from which other AP classes may inherit. -/// Provides type introspection and some basic protocols that can -/// be implemented by subclasses. - -#include "AP_MetaClass.h" - -// Default ctor, currently does nothing -AP_Meta_class::AP_Meta_class(void) -{ -} - -// Default dtor, currently does nothing but must be defined in order to ensure that -// subclasses not overloading the default virtual dtor still have something in their -// vtable. -AP_Meta_class::~AP_Meta_class() -{ -} - -size_t AP_Meta_class::serialize(void *buf, size_t bufSize) const -{ - return 0; -} - -size_t AP_Meta_class::unserialize(void *buf, size_t bufSize) -{ - return 0; -} diff --git a/libraries/AP_Common/AP_MetaClass.h b/libraries/AP_Common/AP_MetaClass.h deleted file mode 100644 index 1025fad..0000000 --- a/libraries/AP_Common/AP_MetaClass.h +++ /dev/null @@ -1,259 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -// -// This is free software; you can redistribute it and/or modify it under -// the terms of the GNU Lesser General Public License as published by the -// Free Software Foundation; either version 2.1 of the License, or (at -// your option) any later version. -// - -/// @file AP_Meta_class.h -/// @brief An abstract base class from which other classes can inherit. -/// -/// This abstract base class declares and implements functions that are -/// useful to code that wants to know things about a class, or to operate -/// on the class without knowing precisely what it is. -/// -/// All classes that inherit from this class can be assumed to have these -/// basic functions. -/// - -#ifndef AP_META_CLASS_H -#define AP_META_CLASS_H - -#include // for size_t -#include - -#include // for RAMEND -/// -/// Basic meta-class from which other AP_* classes can derive. -/// -/// Functions that form the public API to the metaclass are prefixed meta_. -/// -/// Note that classes inheriting from AP_Meta_class *must* have a default -/// constructor and destructor in order for meta_cast to work. -/// -class AP_Meta_class -{ -public: - /// Default constructor does nothing. - AP_Meta_class(void); - - /// Default destructor is virtual, to ensure that all subclasses' - /// destructors are virtual. This guarantees that all destructors - /// in the inheritance chain are called at destruction time. - /// - virtual ~AP_Meta_class(); - - /// Typedef for the ID unique to all instances of a class. - /// - /// See ::meta_type_id for a discussion of class type IDs. - /// - typedef uint16_t Type_id; - - /// Obtain a value unique to all instances of a specific subclass. - /// - /// The value can be used to determine whether two class pointers - /// refer to the same exact class type. The value can also be cached - /// and then used to detect objects of a given type at a later point. - /// - /// This is similar to the basic functionality of the C++ typeid - /// keyword, but does not depend on std::type_info or any compiler- - /// generated RTTI. - /// - /// The value is derived from the vtable address, so it is guaranteed - /// to be unique but cannot be known until the program has been compiled - /// and linked. Thus, the only way to know the type ID of a given - /// type is to construct an object at runtime. To cache the type ID - /// of a class Foo, see the templated version below: - /// - /// @return A type-unique value for this. - /// - Type_id meta_type_id(void) const { - return *(Type_id *)this; - } - - /// Obtain a value unique to all instances of a named subclass. - /// - /// This is similar to ::meta_type_id, but is a template taking a class name. - /// Use this function to cache the Type_id for a class when you don't need - /// or cannot afford the constructor cost associated with meta_cast. - /// - /// @tparam T A subclass of AP_Meta_class. - /// @return The Type_id value for T. - /// - template - static Type_id meta_type_id(void) { - T tmp; - return tmp.meta_type_id(); - } - - /// External handle for an instance of an AP_Meta_class subclass, contains - /// enough information to construct and validate a pointer to the instance - /// when passed back from an untrusted source. - /// - /// Handles are useful when passing a reference to an object to a client outside - /// the system, as they can be validated by the system when the client hands - /// them back. - /// - typedef uint32_t Meta_handle; - - /// Return a value that can be used as an external pointer to an instance - /// of a subclass. - /// - /// The value can be passed to an untrusted agent, and validated on its return. - /// - /// The value contains the 16-bit type ID of the actual class and - /// a pointer to the class instance. - /// - /// @return An opaque handle - /// - Meta_handle meta_get_handle(void) const { - return ((Meta_handle)meta_type_id() << 16) | (uintptr_t)this; - } - - /// Validates an AP_Meta_class handle. - /// - /// The value of the handle is not required to be valid; in particular the - /// pointer encoded in the handle is validated before being dereferenced. - /// - /// The handle is considered good if the pointer is valid and the object - /// it points to has a type ID that matches the ID in the handle. - /// - /// @param handle A possible AP_Meta_class handle - /// @return The instance pointer if the handle is good, - /// or NULL if it is bad. - /// - static AP_Meta_class *meta_validate_handle(Meta_handle handle) { - AP_Meta_class *candidate = (AP_Meta_class *)(handle & 0xffff); // extract object pointer - uint16_t id = handle >> 16; // and claimed type - - // Sanity-check the pointer to ensure it lies within the device RAM, so that - // a bad handle won't cause ::meta_type_id to read outside of SRAM. - // Assume that RAM (or addressable storage of some sort, at least) starts at zero. - // - // Note that this implies that we cannot deal with objects in ROM or EEPROM, - // but the constructor wouldn't be able to populate a vtable pointer there anyway... - // - if ((uintptr_t)candidate >= (RAMEND - 2)) { // -2 to account for the type_id - return NULL; - } - - // Compare the typeid of the object that candidate points to with the typeid - // from the handle. Note that it's safe to call meta_type_id() off the untrusted - // candidate pointer because meta_type_id is non-virtual (and will in fact be - // inlined here). - // - if (candidate->meta_type_id() == id) { - return candidate; - } - - return NULL; - } - - /// Tests whether two objects are of precisely the same class. - /// - /// Note that in the case where p2 inherits from p1, or vice-versa, this will return - /// false as we cannot detect these inheritance relationships at runtime. - /// - /// In the caller's context, p1 and p2 may be pointers to any type, but we require - /// that they be passed as pointers to AP_Meta_class in order to make it clear that - /// they should be pointers to classes derived from AP_Meta_class. - /// - /// No attempt is made to validate whether p1 and p2 are actually derived from - /// AP_Meta_class. If p1 and p2 are equal, or if they point to non-class objects with - /// similar contents, or to non-AP_Meta_class derived classes with no virtual functions - /// this function may return true. - /// - /// @param p1 The first object to be compared. - /// @param p2 The second object to be compared. - /// @return True if the two objects are of the same class, false - /// if they are not. - /// - static bool meta_type_equivalent(AP_Meta_class *p1, AP_Meta_class *p2) { - return p1->meta_type_id() == p2->meta_type_id(); - } - - /// Cast a pointer to an expected class type. - /// - /// This function is used when a pointer is expected to be a pointer to a - /// subclass of AP_Meta_class, but the caller is not certain. It will return the pointer - /// if it is, or NULL if it is not a pointer to the expected class. - /// - /// This should be used with caution, as T's default constructor and - /// destructor will be run, possibly introducing undesired side-effects. - /// - /// @todo Consider whether we should make it difficult to have a default constructor - /// with appreciable side-effects. - /// - /// @param p An AP_Meta_class subclass that may be of type T. - /// @tparam T The name of a type to which p is to be cast. - /// @return NULL if p is not of precisely type T, otherwise p cast to T. - /// - template - static T *meta_cast(AP_Meta_class *p) { - T tmp; - if (meta_type_equivalent(p, &tmp)) { - return (T *)p; - } - return NULL; - } - - /// Cast this to an expected class type. - /// - /// This is equivalent to meta_cast(this) - /// - /// @tparam T The name of a type to which this is to be cast. - /// @return NULL if this is not of precisely type T, otherwise this cast to T. - /// - template - T *meta_cast(void) { - T tmp; - if (meta_type_equivalent(this, &tmp)) { - return (T*)this; - } - return NULL; - } - - /// Serialize the class. - /// - /// Serialization stores the state of the class in an external buffer in such a - /// fashion that it can later be restored by unserialization. - /// - /// AP_Meta_class subclasses should only implement these functions if saving and - /// restoring their state makes sense. - /// - /// Serialization provides a mechanism for exporting the state of the class to an - /// external consumer, either for external introspection or for subsequent restoration. - /// - /// Classes that wrap variables should define the format of their serialiaed data - /// so that external consumers can reliably interpret it. - /// - /// @param buf Buffer into which serialised data should be placed. - /// @param bufSize The size of the buffer provided. - /// @return The size of the serialised data, even if that data would - /// have overflowed the buffer. If the return value is zero, - /// the class does not support serialization. - /// - virtual size_t serialize(void *buf, size_t bufSize) const; - - /// Unserialize the class. - /// - /// Unserializing a class from a buffer into which the class previously serialized - /// itself restores the instance to an identical state, where "identical" is left - /// up to the class itself to define. - /// - /// Classes that wrap variables should define the format of their serialized data so - /// that external providers can reliably encode it. - /// - /// @param buf Buffer containing serialized data. - /// @param bufSize The size of the buffer. - /// @return The number of bytes from the buffer that would be consumed - /// unserializing the data. If the value is less than or equal - /// to bufSize, unserialization was successful. If the return - /// value is zero the class does not support unserialisation or - /// the data in the buffer is invalid. - /// - virtual size_t unserialize(void *buf, size_t bufSize); -}; - -#endif // AP_Meta_class_H diff --git a/libraries/AP_Common/AP_Param.cpp b/libraries/AP_Common/AP_Param.cpp deleted file mode 100644 index 95e81a0..0000000 --- a/libraries/AP_Common/AP_Param.cpp +++ /dev/null @@ -1,892 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -// -// This is free software; you can redistribute it and/or modify it under -// the terms of the GNU Lesser General Public License as published by the -// Free Software Foundation; either version 2.1 of the License, or (at -// your option) any later version. -// - -// total up and check overflow -// check size of group var_info - -/// @file AP_Param.cpp -/// @brief The AP variable store. - - -#include -#include -#include - -#include -#include - -// #define ENABLE_FASTSERIAL_DEBUG - -#ifdef ENABLE_FASTSERIAL_DEBUG -# define serialDebug(fmt, args...) if (FastSerial::getInitialized(0)) do {Serial.printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__ , ##args); delay(0); } while(0) -#else -# define serialDebug(fmt, args...) -#endif - -// some useful progmem macros -#define PGM_UINT8(addr) pgm_read_byte((const prog_char *)addr) -#define PGM_UINT16(addr) pgm_read_word((const uint16_t *)addr) -#define PGM_POINTER(addr) pgm_read_pointer((const void *)addr) - -// the 'GROUP_ID' of a element of a group is the 8 bit identifier used -// to distinguish between this element of the group and other elements -// of the same group. It is calculated using a bit shift per level of -// nesting, so the first level of nesting gets 4 bits, and the next -// level gets the next 4 bits. This limits groups to having at most 16 -// elements. -#define GROUP_ID(grpinfo, base, i, shift) ((base)+(((uint16_t)PGM_UINT8(&grpinfo[i].idx))<<(shift))) - -// Note about AP_Vector3f handling. -// The code has special cases for AP_Vector3f to allow it to be viewed -// as both a single 3 element vector and as a set of 3 AP_Float -// variables. This is done to make it possible for MAVLink to see -// vectors as parameters, which allows users to save their compass -// offsets in MAVLink parameter files. The code involves quite a few -// special cases which could be generalised to any vector/matrix type -// if we end up needing this behaviour for other than AP_Vector3f - - -// static member variables for AP_Param. -// - -// max EEPROM write size. This is usually less than the physical -// size as only part of the EEPROM is reserved for parameters -uint16_t AP_Param::_eeprom_size; - -// number of rows in the _var_info[] table -uint8_t AP_Param::_num_vars; - -// storage and naming information about all types that can be saved -const AP_Param::Info *AP_Param::_var_info; - -// write to EEPROM, checking each byte to avoid writing -// bytes that are already correct -void AP_Param::eeprom_write_check(const void *ptr, uint16_t ofs, uint8_t size) -{ - const uint8_t *b = (const uint8_t *)ptr; - while (size--) { - uint8_t v = eeprom_read_byte((const uint8_t *)ofs); - if (v != *b) { - eeprom_write_byte((uint8_t *)ofs, *b); - } - b++; - ofs++; - } -} - -// write a sentinal value at the given offset -void AP_Param::write_sentinal(uint16_t ofs) -{ - struct Param_header phdr; - phdr.type = _sentinal_type; - phdr.key = _sentinal_key; - phdr.group_element = _sentinal_group; - eeprom_write_check(&phdr, ofs, sizeof(phdr)); -} - -// erase all EEPROM variables by re-writing the header and adding -// a sentinal -void AP_Param::erase_all(void) -{ - struct EEPROM_header hdr; - - serialDebug("erase_all"); - - // write the header - hdr.magic[0] = k_EEPROM_magic0; - hdr.magic[1] = k_EEPROM_magic1; - hdr.revision = k_EEPROM_revision; - hdr.spare = 0; - eeprom_write_check(&hdr, 0, sizeof(hdr)); - - // add a sentinal directly after the header - write_sentinal(sizeof(struct EEPROM_header)); -} - -// validate a group info table -bool AP_Param::check_group_info(const struct AP_Param::GroupInfo *group_info, - uint16_t *total_size, - uint8_t group_shift) -{ - uint8_t type; - int8_t max_idx = -1; - for (uint8_t i=0; - (type=PGM_UINT8(&group_info[i].type)) != AP_PARAM_NONE; - i++) { -#ifdef AP_NESTED_GROUPS_ENABLED - if (type == AP_PARAM_GROUP) { - // a nested group - const struct GroupInfo *ginfo = (const struct GroupInfo *)PGM_POINTER(&group_info[i].group_info); - if (group_shift + _group_level_shift >= _group_bits) { - // double nesting of groups is not allowed - return false; - } - if (ginfo == NULL || - !check_group_info(ginfo, total_size, group_shift + _group_level_shift)) { - return false; - } - continue; - } -#endif // AP_NESTED_GROUPS_ENABLED - uint8_t idx = PGM_UINT8(&group_info[i].idx); - if (idx >= (1<<_group_level_shift)) { - // passed limit on table size - return false; - } - if ((int8_t)idx <= max_idx) { - // the indexes must be in increasing order - return false; - } - max_idx = (int8_t)idx; - uint8_t size = type_size((enum ap_var_type)type); - if (size == 0) { - // not a valid type - return false; - } - (*total_size) += size + sizeof(struct Param_header); - } - return true; -} - -// check for duplicate key values -bool AP_Param::duplicate_key(uint8_t vindex, uint8_t key) -{ - for (uint8_t i=vindex+1; i<_num_vars; i++) { - uint8_t key2 = PGM_UINT8(&_var_info[i].key); - if (key2 == key) { - // no duplicate keys allowed - return true; - } - } - return false; -} - -// validate the _var_info[] table -bool AP_Param::check_var_info(void) -{ - uint16_t total_size = sizeof(struct EEPROM_header); - - for (uint8_t i=0; i<_num_vars; i++) { - uint8_t type = PGM_UINT8(&_var_info[i].type); - uint8_t key = PGM_UINT8(&_var_info[i].key); - if (type == AP_PARAM_GROUP) { - if (i == 0) { - // first element can't be a group, for first() call - return false; - } - const struct GroupInfo *group_info = (const struct GroupInfo *)PGM_POINTER(&_var_info[i].group_info); - if (group_info == NULL || - !check_group_info(group_info, &total_size, 0)) { - return false; - } - } else { - uint8_t size = type_size((enum ap_var_type)type); - if (size == 0) { - // not a valid type - the top level list can't contain - // AP_PARAM_NONE - return false; - } - total_size += size + sizeof(struct Param_header); - } - if (duplicate_key(i, key)) { - return false; - } - } - if (total_size > _eeprom_size) { - serialDebug("total_size %u exceeds _eeprom_size %u", - total_size, _eeprom_size); - return false; - } - return true; -} - - -// setup the _var_info[] table -bool AP_Param::setup(const AP_Param::Info *info, uint8_t num_vars, uint16_t eeprom_size) -{ - struct EEPROM_header hdr; - - _eeprom_size = eeprom_size; - _var_info = info; - _num_vars = num_vars; - - if (!check_var_info()) { - return false; - } - - serialDebug("setup %u vars", (unsigned)num_vars); - - // check the header - eeprom_read_block(&hdr, 0, sizeof(hdr)); - if (hdr.magic[0] != k_EEPROM_magic0 || - hdr.magic[1] != k_EEPROM_magic1 || - hdr.revision != k_EEPROM_revision) { - // header doesn't match. We can't recover any variables. Wipe - // the header and setup the sentinal directly after the header - serialDebug("bad header in setup - erasing"); - erase_all(); - } - - return true; -} - -// check if AP_Param has been initialised -bool AP_Param::initialised(void) -{ - return _var_info != NULL; -} - -// find the info structure given a header and a group_info table -// return the Info structure and a pointer to the variables storage -const struct AP_Param::Info *AP_Param::find_by_header_group(struct Param_header phdr, void **ptr, - uint8_t vindex, - const struct GroupInfo *group_info, - uint8_t group_base, - uint8_t group_shift) -{ - uint8_t type; - for (uint8_t i=0; - (type=PGM_UINT8(&group_info[i].type)) != AP_PARAM_NONE; - i++) { -#ifdef AP_NESTED_GROUPS_ENABLED - if (type == AP_PARAM_GROUP) { - // a nested group - if (group_shift + _group_level_shift >= _group_bits) { - // too deeply nested - this should have been caught by - // setup() ! - return NULL; - } - const struct GroupInfo *ginfo = (const struct GroupInfo *)PGM_POINTER(&group_info[i].group_info); - const struct AP_Param::Info *ret = find_by_header_group(phdr, ptr, vindex, ginfo, - GROUP_ID(group_info, group_base, i, group_shift), - group_shift + _group_level_shift); - if (ret != NULL) { - return ret; - } - continue; - } -#endif // AP_NESTED_GROUPS_ENABLED - if (GROUP_ID(group_info, group_base, i, group_shift) == phdr.group_element) { - // found a group element - *ptr = (void*)(PGM_POINTER(&_var_info[vindex].ptr) + PGM_UINT16(&group_info[i].offset)); - return &_var_info[vindex]; - } - } - return NULL; -} - -// find the info structure given a header -// return the Info structure and a pointer to the variables storage -const struct AP_Param::Info *AP_Param::find_by_header(struct Param_header phdr, void **ptr) -{ - // loop over all named variables - for (uint8_t i=0; i<_num_vars; i++) { - uint8_t type = PGM_UINT8(&_var_info[i].type); - uint8_t key = PGM_UINT8(&_var_info[i].key); - if (key != phdr.key) { - // not the right key - continue; - } - if (type != AP_PARAM_GROUP) { - // if its not a group then we are done - *ptr = (void*)PGM_POINTER(&_var_info[i].ptr); - return &_var_info[i]; - } - - const struct GroupInfo *group_info = (const struct GroupInfo *)PGM_POINTER(&_var_info[i].group_info); - return find_by_header_group(phdr, ptr, i, group_info, 0, 0); - } - return NULL; -} - -// find the info structure for a variable in a group -const struct AP_Param::Info *AP_Param::find_var_info_group(const struct GroupInfo *group_info, - uint8_t vindex, - uint8_t group_base, - uint8_t group_shift, - uint8_t *group_element, - const struct GroupInfo **group_ret, - uint8_t *idx) -{ - uintptr_t base = PGM_POINTER(&_var_info[vindex].ptr); - uint8_t type; - for (uint8_t i=0; - (type=PGM_UINT8(&group_info[i].type)) != AP_PARAM_NONE; - i++) { - uintptr_t ofs = PGM_POINTER(&group_info[i].offset); -#ifdef AP_NESTED_GROUPS_ENABLED - if (type == AP_PARAM_GROUP) { - const struct GroupInfo *ginfo = (const struct GroupInfo *)PGM_POINTER(&group_info[i].group_info); - // a nested group - if (group_shift + _group_level_shift >= _group_bits) { - // too deeply nested - this should have been caught by - // setup() ! - return NULL; - } - const struct AP_Param::Info *info; - info = find_var_info_group(ginfo, vindex, - GROUP_ID(group_info, group_base, i, group_shift), - group_shift + _group_level_shift, - group_element, - group_ret, - idx); - if (info != NULL) { - return info; - } - } else // Forgive the poor formatting - if continues below. -#endif // AP_NESTED_GROUPS_ENABLED - if ((uintptr_t)this == base + ofs) { - *group_element = GROUP_ID(group_info, group_base, i, group_shift); - *group_ret = &group_info[i]; - *idx = 0; - return &_var_info[vindex]; - } else if (type == AP_PARAM_VECTOR3F && - (base+ofs+sizeof(float) == (uintptr_t)this || - base+ofs+2*sizeof(float) == (uintptr_t)this)) { - // we are inside a Vector3f. We need to work out which - // element of the vector the current object refers to. - *idx = (((uintptr_t)this) - (base+ofs))/sizeof(float); - *group_element = GROUP_ID(group_info, group_base, i, group_shift); - *group_ret = &group_info[i]; - return &_var_info[vindex]; - } - } - return NULL; -} - -// find the info structure for a variable -const struct AP_Param::Info *AP_Param::find_var_info(uint8_t *group_element, - const struct GroupInfo **group_ret, - uint8_t *idx) -{ - for (uint8_t i=0; i<_num_vars; i++) { - uint8_t type = PGM_UINT8(&_var_info[i].type); - uintptr_t base = PGM_POINTER(&_var_info[i].ptr); - if (type == AP_PARAM_GROUP) { - const struct GroupInfo *group_info = (const struct GroupInfo *)PGM_POINTER(&_var_info[i].group_info); - const struct AP_Param::Info *info; - info = find_var_info_group(group_info, i, 0, 0, group_element, group_ret, idx); - if (info != NULL) { - return info; - } - } else if (base == (uintptr_t)this) { - *group_element = 0; - *group_ret = NULL; - *idx = 0; - return &_var_info[i]; - } else if (type == AP_PARAM_VECTOR3F && - (base+sizeof(float) == (uintptr_t)this || - base+2*sizeof(float) == (uintptr_t)this)) { - // we are inside a Vector3f. Work out which element we are - // referring to. - *idx = (((uintptr_t)this) - base)/sizeof(float); - *group_element = 0; - *group_ret = NULL; - return &_var_info[i]; - } - } - return NULL; -} - -// return the storage size for a AP_PARAM_* type -const uint8_t AP_Param::type_size(enum ap_var_type type) -{ - switch (type) { - case AP_PARAM_NONE: - case AP_PARAM_GROUP: - return 0; - case AP_PARAM_INT8: - return 1; - case AP_PARAM_INT16: - return 2; - case AP_PARAM_INT32: - return 4; - case AP_PARAM_FLOAT: - return 4; - case AP_PARAM_VECTOR3F: - return 3*4; - case AP_PARAM_VECTOR6F: - return 6*4; - case AP_PARAM_MATRIX3F: - return 3*3*4; - } - serialDebug("unknown type %u\n", type); - return 0; -} - -// scan the EEPROM looking for a given variable by header content -// return true if found, along with the offset in the EEPROM where -// the variable is stored -// if not found return the offset of the sentinal, or -bool AP_Param::scan(const AP_Param::Param_header *target, uint16_t *pofs) -{ - struct Param_header phdr; - uint16_t ofs = sizeof(AP_Param::EEPROM_header); - while (ofs < _eeprom_size) { - eeprom_read_block(&phdr, (void *)ofs, sizeof(phdr)); - if (phdr.type == target->type && - phdr.key == target->key && - phdr.group_element == target->group_element) { - // found it - *pofs = ofs; - return true; - } - // note that this is an ||, not an &&, as this makes us more - // robust to power off while adding a variable to EEPROM - if (phdr.type == _sentinal_type || - phdr.key == _sentinal_key || - phdr.group_element == _sentinal_group) { - // we've reached the sentinal - *pofs = ofs; - return false; - } - ofs += type_size((enum ap_var_type)phdr.type) + sizeof(phdr); - } - *pofs = ~0; - serialDebug("scan past end of eeprom"); - return false; -} - -// add a X,Y,Z suffix to the name of a Vector3f element -void AP_Param::add_vector3f_suffix(char *buffer, size_t buffer_size, uint8_t idx) -{ - uint8_t len = strnlen(buffer, buffer_size); - if ((size_t)(len+3) >= buffer_size) { - // the suffix doesn't fit - return; - } - buffer[len] = '_'; - if (idx == 0) { - buffer[len+1] = 'X'; - } else if (idx == 1) { - buffer[len+1] = 'Y'; - } else if (idx == 2) { - buffer[len+1] = 'Z'; - } - buffer[len+2] = 0; -} - -// Copy the variable's whole name to the supplied buffer. -// -// If the variable is a group member, prepend the group name. -// -void AP_Param::copy_name(char *buffer, size_t buffer_size, bool force_scalar) -{ - uint8_t group_element; - const struct GroupInfo *ginfo; - uint8_t idx; - const struct AP_Param::Info *info = find_var_info(&group_element, &ginfo, &idx); - if (info == NULL) { - *buffer = 0; - serialDebug("no info found"); - return; - } - strncpy_P(buffer, info->name, buffer_size); - if (ginfo != NULL) { - uint8_t len = strnlen(buffer, buffer_size); - if (len < buffer_size) { - strncpy_P(&buffer[len], ginfo->name, buffer_size-len); - } - if ((force_scalar || idx != 0) && AP_PARAM_VECTOR3F == PGM_UINT8(&ginfo->type)) { - // the caller wants a specific element in a Vector3f - add_vector3f_suffix(buffer, buffer_size, idx); - } - } else if ((force_scalar || idx != 0) && AP_PARAM_VECTOR3F == PGM_UINT8(&info->type)) { - add_vector3f_suffix(buffer, buffer_size, idx); - } -} - -// Find a variable by name in a group -AP_Param * -AP_Param::find_group(const char *name, uint8_t vindex, const struct GroupInfo *group_info, enum ap_var_type *ptype) -{ - uint8_t type; - for (uint8_t i=0; - (type=PGM_UINT8(&group_info[i].type)) != AP_PARAM_NONE; - i++) { -#ifdef AP_NESTED_GROUPS_ENABLED - if (type == AP_PARAM_GROUP) { - const struct GroupInfo *ginfo = (const struct GroupInfo *)PGM_POINTER(&group_info[i].group_info); - AP_Param *ap = find_group(name, vindex, ginfo, ptype); - if (ap != NULL) { - return ap; - } - } else -#endif // AP_NESTED_GROUPS_ENABLED - if (strcasecmp_P(name, group_info[i].name) == 0) { - uintptr_t p = PGM_POINTER(&_var_info[vindex].ptr); - *ptype = (enum ap_var_type)type; - return (AP_Param *)(p + PGM_POINTER(&group_info[i].offset)); - } else if (type == AP_PARAM_VECTOR3F) { - // special case for finding Vector3f elements - uint8_t suffix_len = strlen_P(group_info[i].name); - if (strncmp_P(name, group_info[i].name, suffix_len) == 0 && - name[suffix_len] == '_' && - name[suffix_len+1] != 0 && - name[suffix_len+2] == 0) { - uintptr_t p = PGM_POINTER(&_var_info[vindex].ptr); - AP_Float *v = (AP_Float *)(p + PGM_POINTER(&group_info[i].offset)); - *ptype = AP_PARAM_FLOAT; - switch (name[suffix_len+1]) { - case 'X': - return (AP_Float *)&v[0]; - case 'Y': - return (AP_Float *)&v[1]; - case 'Z': - return (AP_Float *)&v[2]; - } - } - } - } - return NULL; -} - - -// Find a variable by name. -// -AP_Param * -AP_Param::find(const char *name, enum ap_var_type *ptype) -{ - for (uint8_t i=0; i<_num_vars; i++) { - uint8_t type = PGM_UINT8(&_var_info[i].type); - if (type == AP_PARAM_GROUP) { - uint8_t len = strnlen_P(_var_info[i].name, AP_MAX_NAME_SIZE); - if (strncmp_P(name, _var_info[i].name, len) != 0) { - continue; - } - const struct GroupInfo *group_info = (const struct GroupInfo *)PGM_POINTER(&_var_info[i].group_info); - AP_Param *ap = find_group(name + len, i, group_info, ptype); - if (ap != NULL) { - return ap; - } - // we continue looking as we want to allow top level - // parameter to have the same prefix name as group - // parameters, for example CAM_P_G - } else if (strcasecmp_P(name, _var_info[i].name) == 0) { - *ptype = (enum ap_var_type)type; - return (AP_Param *)PGM_POINTER(&_var_info[i].ptr); - } - } - return NULL; -} - -// Save the variable to EEPROM, if supported -// -bool AP_Param::save(void) -{ - uint8_t group_element = 0; - const struct GroupInfo *ginfo; - uint8_t idx; - const struct AP_Param::Info *info = find_var_info(&group_element, &ginfo, &idx); - const AP_Param *ap; - - if (info == NULL) { - // we don't have any info on how to store it - return false; - } - - struct Param_header phdr; - - // create the header we will use to store the variable - if (ginfo != NULL) { - phdr.type = PGM_UINT8(&ginfo->type); - } else { - phdr.type = PGM_UINT8(&info->type); - } - phdr.key = PGM_UINT8(&info->key); - phdr.group_element = group_element; - - ap = this; - if (phdr.type != AP_PARAM_VECTOR3F && idx != 0) { - // only vector3f can have non-zero idx for now - return false; - } - if (idx != 0) { - ap = (const AP_Param *)((uintptr_t)ap) - (idx*sizeof(float)); - } - - // scan EEPROM to find the right location - uint16_t ofs; - if (scan(&phdr, &ofs)) { - // found an existing copy of the variable - eeprom_write_check(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type)); - return true; - } - if (ofs == (uint16_t)~0) { - return false; - } - - // write a new sentinal, then the data, then the header - write_sentinal(ofs + sizeof(phdr) + type_size((enum ap_var_type)phdr.type)); - eeprom_write_check(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type)); - eeprom_write_check(&phdr, ofs, sizeof(phdr)); - return true; -} - -// Load the variable from EEPROM, if supported -// -bool AP_Param::load(void) -{ - uint8_t group_element = 0; - const struct GroupInfo *ginfo; - uint8_t idx; - const struct AP_Param::Info *info = find_var_info(&group_element, &ginfo, &idx); - if (info == NULL) { - // we don't have any info on how to load it - return false; - } - - struct Param_header phdr; - - // create the header we will use to match the variable - if (ginfo != NULL) { - phdr.type = PGM_UINT8(&ginfo->type); - } else { - phdr.type = PGM_UINT8(&info->type); - } - phdr.key = PGM_UINT8(&info->key); - phdr.group_element = group_element; - - // scan EEPROM to find the right location - uint16_t ofs; - if (!scan(&phdr, &ofs)) { - return false; - } - - if (phdr.type != AP_PARAM_VECTOR3F && idx != 0) { - // only vector3f can have non-zero idx for now - return false; - } - - AP_Param *ap; - ap = this; - if (idx != 0) { - ap = (AP_Param *)((uintptr_t)ap) - (idx*sizeof(float)); - } - - // found it - eeprom_read_block(ap, (void*)(ofs+sizeof(phdr)), type_size((enum ap_var_type)phdr.type)); - return true; -} - -// Load all variables from EEPROM -// -bool AP_Param::load_all(void) -{ - struct Param_header phdr; - uint16_t ofs = sizeof(AP_Param::EEPROM_header); - while (ofs < _eeprom_size) { - eeprom_read_block(&phdr, (void *)ofs, sizeof(phdr)); - // note that this is an || not an && for robustness - // against power off while adding a variable - if (phdr.type == _sentinal_type || - phdr.key == _sentinal_key || - phdr.group_element == _sentinal_group) { - // we've reached the sentinal - return true; - } - - const struct AP_Param::Info *info; - void *ptr; - - info = find_by_header(phdr, &ptr); - if (info != NULL) { - eeprom_read_block(ptr, (void*)(ofs+sizeof(phdr)), type_size((enum ap_var_type)phdr.type)); - } - - ofs += type_size((enum ap_var_type)phdr.type) + sizeof(phdr); - } - - // we didn't find the sentinal - serialDebug("no sentinal in load_all"); - return false; -} - - -// return the first variable in _var_info -AP_Param *AP_Param::first(ParamToken *token, enum ap_var_type *ptype) -{ - token->key = 0; - token->group_element = 0; - token->idx = 0; - if (_num_vars == 0) { - return NULL; - } - if (ptype != NULL) { - *ptype = (enum ap_var_type)PGM_UINT8(&_var_info[0].type); - } - return (AP_Param *)(PGM_POINTER(&_var_info[0].ptr)); -} - -/// Returns the next variable in a group, recursing into groups -/// as needed -AP_Param *AP_Param::next_group(uint8_t vindex, const struct GroupInfo *group_info, - bool *found_current, - uint8_t group_base, - uint8_t group_shift, - ParamToken *token, - enum ap_var_type *ptype) -{ - enum ap_var_type type; - for (uint8_t i=0; - (type=(enum ap_var_type)PGM_UINT8(&group_info[i].type)) != AP_PARAM_NONE; - i++) { -#ifdef AP_NESTED_GROUPS_ENABLED - if (type == AP_PARAM_GROUP) { - // a nested group - const struct GroupInfo *ginfo = (const struct GroupInfo *)PGM_POINTER(&group_info[i].group_info); - AP_Param *ap; - ap = next_group(vindex, ginfo, found_current, GROUP_ID(group_info, group_base, i, group_shift), - group_shift + _group_level_shift, token, ptype); - if (ap != NULL) { - return ap; - } - } else -#endif // AP_NESTED_GROUPS_ENABLED - { - if (*found_current) { - // got a new one - token->key = vindex; - token->group_element = GROUP_ID(group_info, group_base, i, group_shift); - token->idx = 0; - if (ptype != NULL) { - *ptype = type; - } - return (AP_Param*)(PGM_POINTER(&_var_info[vindex].ptr) + PGM_UINT16(&group_info[i].offset)); - } - if (GROUP_ID(group_info, group_base, i, group_shift) == token->group_element) { - *found_current = true; - if (type == AP_PARAM_VECTOR3F && token->idx < 3) { - // return the next element of the vector as a - // float - token->idx++; - if (ptype != NULL) { - *ptype = AP_PARAM_FLOAT; - } - uintptr_t ofs = (uintptr_t)PGM_POINTER(&_var_info[vindex].ptr) + PGM_UINT16(&group_info[i].offset); - ofs += sizeof(float)*(token->idx-1); - return (AP_Param *)ofs; - } - } - } - } - return NULL; -} - -/// Returns the next variable in _var_info, recursing into groups -/// as needed -AP_Param *AP_Param::next(ParamToken *token, enum ap_var_type *ptype) -{ - uint8_t i = token->key; - bool found_current = false; - if (i >= _num_vars) { - // illegal token - return NULL; - } - enum ap_var_type type = (enum ap_var_type)PGM_UINT8(&_var_info[i].type); - - // allow Vector3f to be seen as 3 variables. First as a vector, - // then as 3 separate floats - if (type == AP_PARAM_VECTOR3F && token->idx < 3) { - token->idx++; - if (ptype != NULL) { - *ptype = AP_PARAM_FLOAT; - } - return (AP_Param *)(((token->idx-1)*sizeof(float))+(uintptr_t)PGM_POINTER(&_var_info[i].ptr)); - } - - if (type != AP_PARAM_GROUP) { - i++; - found_current = true; - } - for (; i<_num_vars; i++) { - type = (enum ap_var_type)PGM_UINT8(&_var_info[i].type); - if (type == AP_PARAM_GROUP) { - const struct GroupInfo *group_info = (const struct GroupInfo *)PGM_POINTER(&_var_info[i].group_info); - AP_Param *ap = next_group(i, group_info, &found_current, 0, 0, token, ptype); - if (ap != NULL) { - return ap; - } - } else { - // found the next one - token->key = i; - token->group_element = 0; - token->idx = 0; - if (ptype != NULL) { - *ptype = type; - } - return (AP_Param *)(PGM_POINTER(&_var_info[i].ptr)); - } - } - return NULL; -} - -/// Returns the next scalar in _var_info, recursing into groups -/// as needed -AP_Param *AP_Param::next_scalar(ParamToken *token, enum ap_var_type *ptype) -{ - AP_Param *ap; - enum ap_var_type type; - while ((ap = next(token, &type)) != NULL && type > AP_PARAM_FLOAT) ; - if (ap != NULL && ptype != NULL) { - *ptype = type; - } - return ap; -} - - -/// cast a variable to a float given its type -float AP_Param::cast_to_float(enum ap_var_type type) -{ - switch (type) { - case AP_PARAM_INT8: - return ((AP_Int8 *)this)->cast_to_float(); - case AP_PARAM_INT16: - return ((AP_Int16 *)this)->cast_to_float(); - case AP_PARAM_INT32: - return ((AP_Int32 *)this)->cast_to_float(); - case AP_PARAM_FLOAT: - return ((AP_Float *)this)->cast_to_float(); - default: - return NAN; - } -} - - -// print the value of all variables -void AP_Param::show_all(void) -{ - ParamToken token; - AP_Param *ap; - enum ap_var_type type; - - for (ap=AP_Param::first(&token, &type); - ap; - ap=AP_Param::next_scalar(&token, &type)) { - char s[AP_MAX_NAME_SIZE+1]; - ap->copy_name(s, sizeof(s), true); - s[AP_MAX_NAME_SIZE] = 0; - - switch (type) { - case AP_PARAM_INT8: - Serial.printf_P(PSTR("%s: %d\n"), s, (int)((AP_Int8 *)ap)->get()); - break; - case AP_PARAM_INT16: - Serial.printf_P(PSTR("%s: %d\n"), s, (int)((AP_Int16 *)ap)->get()); - break; - case AP_PARAM_INT32: - Serial.printf_P(PSTR("%s: %ld\n"), s, (long)((AP_Int32 *)ap)->get()); - break; - case AP_PARAM_FLOAT: - Serial.printf_P(PSTR("%s: %f\n"), s, ((AP_Float *)ap)->get()); - break; - default: - break; - } - } -} diff --git a/libraries/AP_Common/AP_Param.h b/libraries/AP_Common/AP_Param.h deleted file mode 100644 index 68564fa..0000000 --- a/libraries/AP_Common/AP_Param.h +++ /dev/null @@ -1,473 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -// -// This is free software; you can redistribute it and/or modify it under -// the terms of the GNU Lesser General Public License as published by the -// Free Software Foundation; either version 2.1 of the License, or (at -// your option) any later version. -// - -/// @file AP_Param.h -/// @brief A system for managing and storing variables that are of -/// general interest to the system. - -#ifndef AP_PARAM_H -#define AP_PARAM_H -#include -#include -#include - -#include -#include - -#define AP_MAX_NAME_SIZE 15 -#define AP_NESTED_GROUPS_ENABLED - -// a variant of offsetof() to work around C++ restrictions. -// this can only be used when the offset of a variable in a object -// is constant and known at compile time -#define AP_VAROFFSET(type, element) (((uintptr_t)(&((const type *)1)->element))-1) - -// find the type of a variable given the class and element -#define AP_CLASSTYPE(class, element) (((const class *)1)->element.vtype) - -// declare a group var_info line -#define AP_GROUPINFO(name, idx, class, element) { AP_CLASSTYPE(class, element), idx, name, AP_VAROFFSET(class, element) } - -// declare a nested group entry in a group var_info -#ifdef AP_NESTED_GROUPS_ENABLED -#define AP_NESTEDGROUPINFO(class, idx) { AP_PARAM_GROUP, idx, "", 0, class::var_info } -#endif - -#define AP_GROUPEND { AP_PARAM_NONE, 0xFF, "" } - -enum ap_var_type { - AP_PARAM_NONE = 0, - AP_PARAM_INT8, - AP_PARAM_INT16, - AP_PARAM_INT32, - AP_PARAM_FLOAT, - AP_PARAM_VECTOR3F, - AP_PARAM_VECTOR6F, - AP_PARAM_MATRIX3F, - AP_PARAM_GROUP -}; - -/// Base class for variables. -/// -/// Provides naming and lookup services for variables. -/// -class AP_Param -{ -public: - // the Info and GroupInfo structures are passed by the main - // program in setup() to give information on how variables are - // named and their location in memory - struct GroupInfo { - uint8_t type; // AP_PARAM_* - uint8_t idx; // identifier within the group - const char name[AP_MAX_NAME_SIZE]; - uintptr_t offset; // offset within the object - const struct GroupInfo *group_info; - }; - struct Info { - uint8_t type; // AP_PARAM_* - const char name[AP_MAX_NAME_SIZE]; - uint8_t key; // k_param_* - void *ptr; // pointer to the variable in memory - const struct GroupInfo *group_info; - }; - - // a token used for first()/next() state - typedef struct { - uint8_t key; - uint8_t group_element; - uint8_t idx; // offset into array types - } ParamToken; - - // called once at startup to setup the _var_info[] table. This - // will also check the EEPROM header and re-initialise it if the - // wrong version is found - static bool setup(const struct Info *info, uint8_t num_vars, uint16_t eeprom_size); - - // return true if AP_Param has been initialised via setup() - static bool initialised(void); - - /// Copy the variable's name, prefixed by any containing group name, to a buffer. - /// - /// If the variable has no name, the buffer will contain an empty string. - /// - /// Note that if the combination of names is larger than the buffer, the - /// result in the buffer will be truncated. - /// - /// @param buffer The destination buffer - /// @param bufferSize Total size of the destination buffer. - /// - void copy_name(char *buffer, size_t bufferSize, bool force_scalar=false); - - /// Find a variable by name. - /// - /// If the variable has no name, it cannot be found by this interface. - /// - /// @param name The full name of the variable to be found. - /// @return A pointer to the variable, or NULL if - /// it does not exist. - /// - static AP_Param *find(const char *name, enum ap_var_type *ptype); - - /// Save the current value of the variable to EEPROM. - /// - /// @return True if the variable was saved successfully. - /// - bool save(void); - - /// Load the variable from EEPROM. - /// - /// @return True if the variable was loaded successfully. - /// - bool load(void); - - /// Load all variables from EEPROM - /// - /// This function performs a best-efforts attempt to load all - /// of the variables from EEPROM. If some fail to load, their - /// values will remain as they are. - /// - /// @return False if any variable failed to load - /// - static bool load_all(void); - - /// Erase all variables in EEPROM. - /// - static void erase_all(void); - - /// print the value of all variables - static void show_all(void); - - /// Returns the first variable - /// - /// @return The first variable in _var_info, or NULL if - /// there are none. - /// - static AP_Param *first(ParamToken *token, enum ap_var_type *ptype); - - /// Returns the next variable in _var_info, recursing into groups - /// as needed - static AP_Param *next(ParamToken *token, enum ap_var_type *ptype); - - /// Returns the next scalar variable in _var_info, recursing into groups - /// as needed - static AP_Param *next_scalar(ParamToken *token, enum ap_var_type *ptype); - - /// cast a variable to a float given its type - float cast_to_float(enum ap_var_type type); - -private: - /// EEPROM header - /// - /// This structure is placed at the head of the EEPROM to indicate - /// that the ROM is formatted for AP_Param. - /// - struct EEPROM_header { - uint8_t magic[2]; - uint8_t revision; - uint8_t spare; - }; - - // This header is prepended to a variable stored in EEPROM. - struct Param_header { - uint8_t key; - uint8_t group_element; - uint8_t type; - }; - - // number of bits in each level of nesting of groups - static const uint8_t _group_level_shift = 4; - static const uint8_t _group_bits = 8; - - static const uint8_t _sentinal_key = 0xFF; - static const uint8_t _sentinal_type = 0xFF; - static const uint8_t _sentinal_group = 0xFF; - - static bool check_group_info(const struct GroupInfo *group_info, uint16_t *total_size, uint8_t max_bits); - static bool duplicate_key(uint8_t vindex, uint8_t key); - static bool check_var_info(void); - const struct Info *find_var_info_group(const struct GroupInfo *group_info, - uint8_t vindex, - uint8_t group_base, - uint8_t group_shift, - uint8_t *group_element, - const struct GroupInfo **group_ret, - uint8_t *idx); - const struct Info *find_var_info(uint8_t *group_element, - const struct GroupInfo **group_ret, - uint8_t *idx); - static const struct Info *find_by_header_group(struct Param_header phdr, void **ptr, - uint8_t vindex, - const struct GroupInfo *group_info, - uint8_t group_base, - uint8_t group_shift); - static const struct Info *find_by_header(struct Param_header phdr, void **ptr); - void add_vector3f_suffix(char *buffer, size_t buffer_size, uint8_t idx); - static AP_Param *find_group(const char *name, uint8_t vindex, const struct GroupInfo *group_info, enum ap_var_type *ptype); - static void write_sentinal(uint16_t ofs); - bool scan(const struct Param_header *phdr, uint16_t *pofs); - static const uint8_t type_size(enum ap_var_type type); - static void eeprom_write_check(const void *ptr, uint16_t ofs, uint8_t size); - static AP_Param *next_group(uint8_t vindex, const struct GroupInfo *group_info, - bool *found_current, - uint8_t group_base, - uint8_t group_shift, - ParamToken *token, - enum ap_var_type *ptype); - - static uint16_t _eeprom_size; - static uint8_t _num_vars; - static const struct Info *_var_info; - - // values filled into the EEPROM header - static const uint8_t k_EEPROM_magic0 = 0x50; - static const uint8_t k_EEPROM_magic1 = 0x41; ///< "AP" - static const uint8_t k_EEPROM_revision = 5; ///< current format revision -}; - -/// Template class for scalar variables. -/// -/// Objects of this type have a value, and can be treated in many ways as though they -/// were the value. -/// -/// @tparam T The scalar type of the variable -/// @tparam PT The AP_PARAM_* type -/// -template -class AP_ParamT : public AP_Param -{ -public: - /// Constructor for scalar variable. - /// - /// Initialises a stand-alone variable with optional initial value. - /// - /// @param default_value Value the variable should have at startup. - /// - AP_ParamT (const T initial_value = 0) : - AP_Param(), - _value(initial_value) - { - } - - static const ap_var_type vtype = PT; - - /// Value getter - /// - T get(void) const { - return _value; - } - - /// Value setter - /// - void set(T v) { - _value = v; - } - - /// Combined set and save - /// - bool set_and_save(T v) { - set(v); - return save(); - } - - /// Combined set and save, but only does the save if the value if - /// different from the current ram value, thus saving us a - /// scan(). This should only be used where we have not set() the - /// value separately, as otherwise the value in EEPROM won't be - /// updated correctly. - bool set_and_save_ifchanged(T v) { - if (v == _value) { - return true; - } - set(v); - return save(); - } - - /// Conversion to T returns a reference to the value. - /// - /// This allows the class to be used in many situations where the value would be legal. - /// - operator T &() { - return _value; - } - - /// Copy assignment from self does nothing. - /// - AP_ParamT& operator=(AP_ParamT& v) { - return v; - } - - /// Copy assignment from T is equivalent to ::set. - /// - AP_ParamT& operator=(T v) { - _value = v; - return *this; - } - - /// AP_ParamT types can implement AP_Param::cast_to_float - /// - float cast_to_float(void) { - return (float)_value; - } - -protected: - T _value; -}; - - -/// Template class for non-scalar variables. -/// -/// Objects of this type have a value, and can be treated in many ways as though they -/// were the value. -/// -/// @tparam T The scalar type of the variable -/// @tparam PT AP_PARAM_* type -/// -template -class AP_ParamV : public AP_Param -{ -public: - static const ap_var_type vtype = PT; - - /// Value getter - /// - T get(void) const { - return _value; - } - - /// Value setter - /// - void set(T v) { - _value = v; - } - - /// Combined set and save - /// - bool set_and_save(T v) { - set(v); - return save(); - } - - /// Conversion to T returns a reference to the value. - /// - /// This allows the class to be used in many situations where the value would be legal. - /// - operator T &() { - return _value; - } - - /// Copy assignment from self does nothing. - /// - AP_ParamV& operator=(AP_ParamV& v) { - return v; - } - - /// Copy assignment from T is equivalent to ::set. - /// - AP_ParamV& operator=(T v) { - _value = v; - return *this; - } - -protected: - T _value; -}; - - -/// Template class for array variables. -/// -/// Objects created using this template behave like arrays of the type T, -/// but are stored like single variables. -/// -/// @tparam T The scalar type of the variable -/// @tparam N number of elements -/// @tparam PT the AP_PARAM_* type -/// -template -class AP_ParamA : public AP_Param -{ -public: - static const ap_var_type vtype = PT; - - /// Array operator accesses members. - /// - /// @note It would be nice to range-check i here, but then what would we return? - /// - T &operator [](uint8_t i) { - return _value[i]; - } - - /// Value getter - /// - /// @note Returns zero for index values out of range. - /// - T get(uint8_t i) const { - if (i < N) { - return _value[i]; - } else { - return (T)0; - } - } - - /// Value setter - /// - /// @note Attempts to set an index out of range are discarded. - /// - void set(uint8_t i, T v) { - if (i < N) { - _value[i] = v; - } - } - - /// Copy assignment from self does nothing. - /// - AP_ParamA& operator=(AP_ParamA& v) { - return v; - } - -protected: - T _value[N]; -}; - - - -/// Convenience macro for defining instances of the AP_ParamT template. -/// -// declare a scalar type -// _t is the base type -// _suffix is the suffix on the AP_* type name -// _pt is the enum ap_var_type type -#define AP_PARAMDEF(_t, _suffix, _pt) typedef AP_ParamT<_t, _pt> AP_##_suffix; -AP_PARAMDEF(float, Float, AP_PARAM_FLOAT); // defines AP_Float -AP_PARAMDEF(int8_t, Int8, AP_PARAM_INT8); // defines AP_Int8 -AP_PARAMDEF(int16_t, Int16, AP_PARAM_INT16); // defines AP_Int16 -AP_PARAMDEF(int32_t, Int32, AP_PARAM_INT32); // defines AP_Int32 - -// declare an array type -// _t is the base type -// _suffix is the suffix on the AP_* type name -// _size is the size of the array -// _pt is the enum ap_var_type type -#define AP_PARAMDEFA(_t, _suffix, _size, _pt) typedef AP_ParamA<_t, _size, _pt> AP_##_suffix; -AP_PARAMDEFA(float, Vector6f, 6, AP_PARAM_VECTOR6F); - -// declare a non-scalar type -// this is used in AP_Math.h -// _t is the base type -// _suffix is the suffix on the AP_* type name -// _pt is the enum ap_var_type type -#define AP_PARAMDEFV(_t, _suffix, _pt) typedef AP_ParamV<_t, _pt> AP_##_suffix; - -/// Rely on built in casting for other variable types -/// to minimize template creation and save memory -#define AP_Uint8 AP_Int8 -#define AP_Uint16 AP_Int16 -#define AP_Uint32 AP_Int32 -#define AP_Bool AP_Int8 - -#endif // AP_PARAM_H diff --git a/libraries/AP_Common/AP_Test.h b/libraries/AP_Common/AP_Test.h deleted file mode 100644 index 5e705d2..0000000 --- a/libraries/AP_Common/AP_Test.h +++ /dev/null @@ -1,127 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -// -// This is free software; you can redistribute it and/or modify it under -// the terms of the GNU Lesser General Public License as published by the -// Free Software Foundation; either version 2.1 of the License, or (at -// your option) any later version. -// - -/// @file AP_Test.h -/// @brief A simple unit test framework. -/// -/// AP_Test provides the usual test start, condition validation and reporting -/// functions in a compact form. -/// -/// Each test must be contained within a block; either a standalone function or -/// a block within a function. The TEST macro is used to start a test; it creates -/// the local object which will track the results of the test and saves the name -/// for later reporting. Only one test may be performed within each block. -/// -/// Within the test, use the REQUIRE macro to describe a condition that must be -/// met for the test to pass. If the condition within the macro is not met, -/// the condition will be output as a diagnostic and the test will be considered -/// to have failed. -/// -/// The test ends at the end of the block, and the result of the test will be -/// output as a diagnostic. -/// -/// Optionally at the end of the test suite, the Test::report method may be used -/// to summarize the results of all of the tests that were performed. -/// - -/// Unit test state and methods. -/// -class Test -{ -public: - /// Constructor - creates a new test. - /// - /// Normally called by the TEST macro. - /// - /// @param name The name of the test being started. - /// - Test(const char *name); - - /// Destructor - ends the test. - /// - ~Test(); - - /// Perform a success check. - /// - /// @param expr If false, the test has failed. - /// @param source The expression source; emitted in the diagnostic - /// indicating test failure. - /// - void require(bool expr, const char *source); - - /// Report the overall number of tests/pass/fails. - /// - static void report(); - -private: - const char *_name; ///< name of the current test - bool _fail; ///< set if any ::require calls indicate the test failed - static int _passed; ///< global pass count - static int _failed; ///< global fail count -}; - -/// Constructor -/// -Test::Test(const char *name) : - _name(name), - _fail(false) -{ -} - -/// Destructor -/// -Test::~Test() -{ - Serial.printf("%s: %s\n", _fail ? "FAILED" : "passed", _name); - if (_fail) { - _failed++; - } else { - _passed++; - } -} - -/// Success check -/// -void -Test::require(bool expr, const char *source) -{ - if (!expr) { - _fail = true; - Serial.printf("%s: fail: %s\n", _name, source); - } -} - -/// Summary report -/// -void -Test::report() -{ - Serial.printf("\n%d passed %d failed\n", _passed, _failed); -} - -int Test::_passed = 0; -int Test::_failed = 0; - -/// Start a new test. -/// -/// This should be invoked at the beginning of a block, before any REQUIRE -/// statements. A new test called name is started, and subsequent REQUIRE -/// statements will be applied to the test. The test will continue until -/// the end of the block (or until the _test object that is created otherwise -/// goes out of scope). -/// -#define TEST(name) Test _test(#name) - -/// Attach an expression to the test's success criteria. -/// -/// The expression expr must evaluate true for the test to pass. If -/// it does not, the text of the expression is output as a diagnostic -/// and the test is marked as a failure. -/// -#define REQUIRE(expr) _test.require(expr, #expr) - diff --git a/libraries/AP_Common/AP_Var_menufuncs.cpp b/libraries/AP_Common/AP_Var_menufuncs.cpp deleted file mode 100644 index ed7021d..0000000 --- a/libraries/AP_Common/AP_Var_menufuncs.cpp +++ /dev/null @@ -1,15 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -// -// This is free software; you can redistribute it and/or modify it under -// the terms of the GNU Lesser General Public License as published by the -// Free Software Foundation; either version 2.1 of the License, or (at -// your option) any later version. -// - -/// @file AP_Var_menufuncs.cpp -/// @brief Useful functions compatible with the menu system for -/// managing AP_Var variables. - -#include -#include - diff --git a/libraries/AP_Common/AP_Vector.h b/libraries/AP_Common/AP_Vector.h deleted file mode 100644 index bcd081a..0000000 --- a/libraries/AP_Common/AP_Vector.h +++ /dev/null @@ -1,382 +0,0 @@ -/* - * Vector.h - * Copyright (C) James Goppert 2010 - * - * This file 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 3 of the License, or - * (at your option) any later version. - * - * This file 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, see . - */ - -#ifndef Vector_H -#define Vector_H - -#include "../FastSerial/BetterStream.h" -#include -#include -#if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" -#else - #include -#endif - -#ifdef ASSERT -const static char vectorSource[] ="Vector.hpp"; -#endif - -// vector -template -class Vector -{ -private: - size_t size; - size_t extraAllocationSize; // extra space to add after each allocation - size_t sizeAllocated; // total allocated size - dataType* data; -public: - // default constructor - Vector(const size_t & _size=0, const size_t & _extraAllocationSize=0) : size(0), extraAllocationSize(_extraAllocationSize), sizeAllocated(0), data(NULL) { - setSize(_size); - } - // 3 vector constructor - Vector(const dataType & a, const dataType & b, const dataType & c) : size(3), extraAllocationSize(extraAllocationSize), sizeAllocated(0), data(NULL) { - setSize(size); - (*this)[0]=a; - (*this)[1]=b; - (*this)[2]=c; - } - - // construct from array - Vector(const dataType* array, const size_t & _size, const size_t & _extraAllocationSize=0) : size(0), extraAllocationSize(_extraAllocationSize), sizeAllocated(0), data(NULL) { - setSize(_size); - for (size_t i=0; i toFloat() const { - Vector v(getSize()); - for (size_t i=0; isizeAllocated) { - dataType * newData = new dataType[n+extraAllocationSize]; - memcpy(newData,data,sizeof(dataType)/sizeof(char)*getSize()); - memset(newData+size,0,sizeof(dataType)/sizeof(char)*(n-getSize())); - delete[] data; - data = newData; - sizeAllocated=n+extraAllocationSize; - } - size=n; - } - // return size - const size_t & getSize() const { - return size; - } - // insert - void insert(const size_t index, const dataType value) { - //Serial.println("insert called"); -#ifdef ASSERT - assert(index setting SRCROOT to $(SRCROOT)) - endif - # Correct the directory backslashes on cygwin - ARDUINO := $(subst \,/,$(ARDUINO)) -endif - -# -# We need to know the location of the sketchbook. If it hasn't been overridden, -# try the parent of the current directory. If there is no libraries directory -# there, assume that we are in a library's examples directory and try backing up -# further. -# -ifeq ($(SKETCHBOOK),) - SKETCHBOOK := $(shell cd $(SRCROOT)/.. && pwd) - ifeq ($(wildcard $(SKETCHBOOK)/libraries),) - SKETCHBOOK := $(shell cd $(SRCROOT)/../.. && pwd) - endif - ifeq ($(wildcard $(SKETCHBOOK)/libraries),) - SKETCHBOOK := $(shell cd $(SRCROOT)/../../.. && pwd) - endif - ifeq ($(wildcard $(SKETCHBOOK)/libraries),) - SKETCHBOOK := $(shell cd $(SRCROOT)/../../../.. && pwd) - endif - ifeq ($(wildcard $(SKETCHBOOK)/libraries),) - $(error ERROR: cannot determine sketchbook location - please specify on the commandline with SKETCHBOOK=) - endif -else - ifeq ($(wildcard $(SKETCHBOOK)/libraries),) - $(warning WARNING: sketchbook directory $(SKETCHBOOK) contains no libraries) - endif -endif -ifneq ($(findstring CYGWIN, $(SYSTYPE)),) - # Convert cygwin path into a windows normal path - SKETCHBOOK := $(shell cygpath -d ${SKETCHBOOK}) - SKETCHBOOK := $(subst \,/,$(SKETCHBOOK)) -endif - -# -# Work out the sketch name from the name of the source directory. -# -SKETCH := $(lastword $(subst /, ,$(SRCROOT))) -# Workaround a $(lastword ) bug on cygwin -ifeq ($(SKETCH),) - WORDLIST := $(subst /, ,$(SRCROOT)) - SKETCH := $(word $(words $(WORDLIST)),$(WORDLIST)) -endif - -# -# Work out where we are going to be building things -# -TMPDIR ?= /tmp -BUILDROOT := $(abspath $(TMPDIR)/$(SKETCH).build) -ifneq ($(findstring CYGWIN, $(SYSTYPE)),) - # Workaround a $(abspath ) bug on cygwin - ifeq ($(BUILDROOT),) - BUILDROOT := C:$(TMPDIR)/$(SKETCH).build - $(warning your abspath function is not working) - $(warning > setting BUILDROOT to $(BUILDROOT)) - endif -endif - -# Jump over the next makefile sections when runing a "make configure" -ifneq ($(MAKECMDGOALS),configure) - -################################################################################ -# Config options -# -# The Makefile calling us must specify BOARD -# -include $(SKETCHBOOK)/config.mk -ifeq ($(PORT),) -$(error ERROR: could not locate $(SKETCHBOOK)/config.mk, please run 'make configure' first) -endif - -HARDWARE ?= arduino -ifeq ($(BOARD),) -$(error ERROR: must set BOARD before including this file) -endif - -# -# Find Arduino, if not explicitly specified -# -ifeq ($(ARDUINO),) - - # - # List locations that might be valid ARDUINO settings - # - ifeq ($(SYSTYPE),Darwin) - # use Spotlight to find Arduino.app - ARDUINO_QUERY = 'kMDItemKind == Application && kMDItemFSName == Arduino.app' - ARDUINOS := $(addsuffix /Contents/Resources/Java,$(shell mdfind -literal $(ARDUINO_QUERY))) - ifeq ($(ARDUINOS),) - $(error ERROR: Spotlight cannot find Arduino on your system.) - endif - endif - - ifeq ($(SYSTYPE),Linux) - ARDUINO_SEARCHPATH = /usr/share/arduino* /usr/local/share/arduino* - ARDUINOS := $(wildcard $(ARDUINO_SEARCHPATH)) - endif - - ifneq ($(findstring CYGWIN, $(SYSTYPE)),) - # Most of the following commands are simply to deal with whitespaces in the path - # Read the "Program Files" system directory from the windows registry - PROGRAM_FILES := $(shell cat /proc/registry/HKEY_LOCAL_MACHINE/SOFTWARE/Microsoft/Windows/CurrentVersion/ProgramFilesDir) - # Convert the path delimiters to / - PROGRAM_FILES := $(shell cygpath -m ${PROGRAM_FILES}) - # Escape the space with a backslash - PROGRAM_FILES := $(shell echo $(PROGRAM_FILES) | sed s/\ /\\\\\ / ) - # Use DOS paths because they do not contain spaces - PROGRAM_FILES := $(shell cygpath -d ${PROGRAM_FILES}) - # Convert the path delimiters to / - PROGRAM_FILES := $(subst \,/,$(PROGRAM_FILES)) - # Search for an Arduino instalation in a couple of paths - ARDUINO_SEARCHPATH := c:/arduino* $(PROGRAM_FILES)/arduino* - ARDUINOS := $(wildcard $(ARDUINO_SEARCHPATH)) - endif - - # - # Pick the first option if more than one candidate is found. - # - ARDUINO := $(firstword $(ARDUINOS)) - ifeq ($(ARDUINO),) - $(error ERROR: Cannot find Arduino on this system, please specify on the commandline with ARDUINO= or on the config.mk file) - endif - - ifneq ($(words $(ARDUINOS)),1) - $(warning WARNING: More than one copy of Arduino was found, using $(ARDUINO)) - endif - -endif - -################################################################################ -# Tools -# - -# -# Decide where we are going to look for tools -# -ifeq ($(SYSTYPE),Darwin) - # use the tools that come with Arduino - TOOLPATH := $(ARDUINOS)/hardware/tools/avr/bin - # use BWK awk - AWK = awk -endif -ifeq ($(SYSTYPE),Linux) - # expect that tools are on the path - TOOLPATH := $(subst :, ,$(PATH)) -endif -ifeq ($(findstring CYGWIN, $(SYSTYPE)),CYGWIN) - TOOLPATH := $(ARDUINO)/hardware/tools/avr/bin -endif - -ifeq ($(findstring CYGWIN, $(SYSTYPE)),) -FIND_TOOL = $(firstword $(wildcard $(addsuffix /$(1),$(TOOLPATH)))) -else -FIND_TOOL = $(firstword $(wildcard $(addsuffix /$(1).exe,$(TOOLPATH)))) -endif -CXX := $(call FIND_TOOL,avr-g++) -CC := $(call FIND_TOOL,avr-gcc) -AS := $(call FIND_TOOL,avr-gcc) -AR := $(call FIND_TOOL,avr-ar) -LD := $(call FIND_TOOL,avr-gcc) -GDB := $(call FIND_TOOL,avr-gdb) -AVRDUDE := $(call FIND_TOOL,avrdude) -AVARICE := $(call FIND_TOOL,avarice) -OBJCOPY := $(call FIND_TOOL,avr-objcopy) -ifeq ($(CXX),) -$(error ERROR: cannot find the compiler tools anywhere on the path $(TOOLPATH)) -endif - -# Find awk -AWK ?= gawk -ifeq ($(shell which $(AWK)),) -$(error ERROR: cannot find $(AWK) - you may need to install GNU awk) -endif - -# -# Tool options -# -DEFINES = -DF_CPU=$(F_CPU) -DARDUINO=$(ARDUINO_VERS) $(EXTRAFLAGS) -OPTFLAGS = -Os -Wformat -Wall -Wshadow -Wpointer-arith -Wcast-align -Wwrite-strings -Wformat=2 -Wno-reorder -DEPFLAGS = -MD -MT $@ - -# XXX warning options TBD -CXXOPTS = -mcall-prologues -ffunction-sections -fdata-sections -fno-exceptions -COPTS = -mcall-prologues -ffunction-sections -fdata-sections -ASOPTS = -assembler-with-cpp -LISTOPTS = -adhlns=$(@:.o=.lst) - -CXXFLAGS = -g -mmcu=$(MCU) $(DEFINES) -Wa,$(LISTOPTS) $(OPTFLAGS) $(DEPFLAGS) $(CXXOPTS) -CFLAGS = -g -mmcu=$(MCU) $(DEFINES) -Wa,$(LISTOPTS) $(OPTFLAGS) $(DEPFLAGS) $(COPTS) -ASFLAGS = -g -mmcu=$(MCU) $(DEFINES) $(LISTOPTS) $(DEPFLAGS) $(ASOPTS) -LDFLAGS = -g -mmcu=$(MCU) $(OPTFLAGS) -Wl,--relax,--gc-sections -Wl,-Map -Wl,$(SKETCHMAP) - -ifeq ($(BOARD),mega) - LDFLAGS = -g -mmcu=$(MCU) $(OPTFLAGS) -Wl,--gc-sections -Wl,-Map -Wl,$(SKETCHMAP) -endif - - - -LIBS = -lm - -SRCSUFFIXES = *.cpp *.c *.S - -ifeq ($(VERBOSE),) -v = @ -else -v = -endif - - -################################################################################ -# Sketch -# - -# Sketch source files -SKETCHPDESRCS := $(wildcard $(SRCROOT)/*.pde $(SRCROOT)/*.ino) -SKETCHSRCS := $(wildcard $(addprefix $(SRCROOT)/,$(SRCSUFFIXES))) -SKETCHPDE := $(wildcard $(SRCROOT)/$(SKETCH).pde $(SRCROOT)/$(SKETCH).ino) -SKETCHCPP := $(BUILDROOT)/$(SKETCH).cpp -ifneq ($(words $(SKETCHPDE)),1) -$(error ERROR: sketch $(SKETCH) must contain exactly one of $(SKETCH).pde or $(SKETCH).ino) -endif - -# Sketch object files -SKETCHOBJS := $(subst $(SRCROOT),$(BUILDROOT),$(SKETCHSRCS)) $(SKETCHCPP) -SKETCHOBJS := $(addsuffix .o,$(basename $(SKETCHOBJS))) - -# List of input files to the sketch.cpp file in the order they should -# be appended to create it -SKETCHCPP_SRC := $(SKETCHPDE) $(sort $(filter-out $(SKETCHPDE),$(SKETCHPDESRCS))) - -################################################################################ -# Libraries -# -# Pick libraries to add to the include path and to link with based on -# #include directives in the sketchfiles. -# -# For example: -# -# #include -# -# implies that there might be a Foo library. -# -# Note that the # and $ require special treatment to avoid upsetting -# make. -# -SEXPR = 's/^[[:space:]]*\#include[[:space:]][<\"]([^>\"./]+).*$$/\1/p' -ifeq ($(SYSTYPE),Darwin) - LIBTOKENS := $(sort $(shell cat $(SKETCHPDESRCS) $(SKETCHSRCS) | sed -nEe $(SEXPR))) -else - LIBTOKENS := $(sort $(shell cat $(SKETCHPDESRCS) $(SKETCHSRCS) | sed -nre $(SEXPR))) -endif - -# -# Find sketchbook libraries referenced by the sketch. -# -# Include paths for sketch libraries -# -SKETCHLIBS := $(wildcard $(addprefix $(SKETCHBOOK)/libraries/,$(LIBTOKENS))) -SKETCHLIBNAMES := $(notdir $(SKETCHLIBS)) -SKETCHLIBSRCDIRS := $(SKETCHLIBS) $(addsuffix /utility,$(SKETCHLIBS)) -SKETCHLIBSRCS := $(wildcard $(foreach suffix,$(SRCSUFFIXES),$(addsuffix /$(suffix),$(SKETCHLIBSRCDIRS)))) -SKETCHLIBOBJS := $(addsuffix .o,$(basename $(subst $(SKETCHBOOK),$(BUILDROOT),$(SKETCHLIBSRCS)))) -SKETCHLIBINCLUDES := $(addprefix -I,$(SKETCHLIBS)) - -# -# Find Arduino libraries referenced by the sketch. Exclude any that -# are overloaded by versions in the sketchbook. -# -ARDUINOLIBS := $(wildcard $(addprefix $(ARDUINO)/libraries/,$(filter-out $(SKETCHLIBNAMES),$(LIBTOKENS)))) -ARDUINOLIBNAMES := $(notdir $(ARDUINOLIBS)) -ARDUINOLIBSRCDIRS := $(ARDUINOLIBS) $(addsuffix /utility,$(ARDUINOLIBS)) -ARDUINOLIBSRCS := $(wildcard $(foreach suffix,$(SRCSUFFIXES),$(addsuffix /$(suffix),$(ARDUINOLIBSRCDIRS)))) -ARDUINOLIBOBJS := $(addsuffix .o,$(basename $(subst $(ARDUINO),$(BUILDROOT),$(ARDUINOLIBSRCS)))) -ARDUINOLIBINCLUDES := $(addprefix -I,$(ARDUINOLIBS)) - -# Library object files -LIBOBJS := $(SKETCHLIBOBJS) $(ARDUINOLIBOBJS) - -################################################################################ -# *duino core -# - -# Pull the Arduino version -ARDUINO_VERS := $(shell $(SKETCHBOOK)/Tools/scripts/arduino_version.sh $(ARDUINO)) - -# Find the hardware directory to use -HARDWARE_DIR := $(firstword $(wildcard $(SKETCHBOOK)/hardware/$(HARDWARE) \ - $(ARDUINO)/hardware/$(HARDWARE))) -ifeq ($(HARDWARE_DIR),) -$(error ERROR: hardware directory for $(HARDWARE) not found) -endif - -# Find the boards.txt that we are using -BOARDFILE := $(wildcard $(HARDWARE_DIR)/boards.txt) -ifeq ($(BOARDFILE),) -$(error ERROR: could not locate boards.txt for hardware $(HARDWARE)) -endif - -# Extract needed build parameters from the boardfile -MCU := $(shell grep $(BOARD).build.mcu $(BOARDFILE) | cut -d = -f 2) -F_CPU := $(shell grep $(BOARD).build.f_cpu $(BOARDFILE) | cut -d = -f 2) -HARDWARE_CORE := $(shell grep $(BOARD).build.core $(BOARDFILE) | cut -d = -f 2) -UPLOAD_SPEED := $(shell grep $(BOARD).upload.speed $(BOARDFILE) | cut -d = -f 2) - -ifeq ($(UPLOAD_PROTOCOL),) - UPLOAD_PROTOCOL := $(shell grep $(BOARD).upload.protocol $(BOARDFILE) | cut -d = -f 2) -endif - -# Adding override for mega since boards.txt uses stk500 instead of -# arduino on 22 release -ifeq ($(BOARD),mega) - UPLOAD_PROTOCOL := arduino -endif - -ifeq ($(MCU),) -$(error ERROR: Could not locate board $(BOARD) in $(BOARDFILE)) -endif - -# Hardware source files -CORESRC_DIR = $(HARDWARE_DIR)/cores/$(HARDWARE_CORE) -CORESRC_PATTERNS = $(foreach suffix,/*.cpp /*.c /*.S,$(addsuffix $(suffix),$(CORESRC_DIR))) -CORESRCS := $(wildcard $(CORESRC_PATTERNS)) - -# Include spec for core includes -COREINCLUDES = -I$(CORESRC_DIR) -I$(HARDWARE_DIR)/variants/mega - -# Hardware object files -CORELIBOBJS := $(subst $(CORESRC_DIR),$(BUILDROOT)/$(HARDWARE),$(CORESRCS)) -CORELIBOBJS := $(addsuffix .o,$(basename $(CORELIBOBJS))) - -################################################################################ -# Built products -# - -# The ELF file -SKETCHELF = $(BUILDROOT)/$(SKETCH).elf - -# HEX file -SKETCHHEX = $(BUILDROOT)/$(SKETCH).hex - -# EEP file -SKETCHEEP = $(BUILDROOT)/$(SKETCH).eep - -# Map file -SKETCHMAP = $(BUILDROOT)/$(SKETCH).map - -# The core library -CORELIB = $(BUILDROOT)/$(HARDWARE)/core.a - -# All of the objects that may be built -ALLOBJS = $(SKETCHOBJS) $(LIBOBJS) $(CORELIBOBJS) - -# All of the dependency files that may be generated -ALLDEPS = $(ALLOBJS:%.o=%.d) -endif - -################################################################################ -# Targets -# - -all: $(SKETCHELF) $(SKETCHEEP) $(SKETCHHEX) - -.PHONY: upload -upload: $(SKETCHHEX) - $(AVRDUDE) -c $(UPLOAD_PROTOCOL) -p $(MCU) -P $(PORT) -b$(UPLOAD_SPEED) -U flash:w:$(SKETCHHEX):i - -configure: - $(warning WARNING - A $(SKETCHBOOK)/config.mk file has been written) - $(warning Please edit the file to match your system configuration, if you use a different board or port) - @echo \# Select \'mega\' for the original APM, or \'mega2560\' for the V2 APM. > $(SKETCHBOOK)/config.mk - @echo BOARD=mega2560 >> $(SKETCHBOOK)/config.mk - @echo \# The communication port used to communicate with the APM. >> $(SKETCHBOOK)/config.mk -ifneq ($(findstring CYGWIN, $(SYSTYPE)),) - @echo PORT=com3 >> $(SKETCHBOOK)/config.mk -else - @echo PORT=/dev/ttyUSB0 >> $(SKETCHBOOK)/config.mk -endif - -debug: - $(AVARICE) --mkII --capture --jtag usb :4242 & \ - gnome-terminal -x $(GDB) $(SKETCHELF) & \ - echo -e '\n\nat the gdb prompt type "target remote localhost:4242"' - -# this allows you to flash your image via JTAG for when you -# have completely broken your USB -jtag-program: - $(AVARICE) --mkII --jtag usb --erase --program --file $(SKETCHELF) - -clean: -ifneq ($(findstring CYGWIN, $(SYSTYPE)),) - @del /S $(BUILDROOT) -else - @rm -fr $(BUILDROOT) -endif - -################################################################################ -# Rules -# - -# fetch dependency info from a previous build if any of it exists --include $(ALLDEPS) - -# common header for rules, prints what is being built -define RULEHDR - @echo %% $(subst $(BUILDROOT)/,,$@) - @mkdir -p $(dir $@) -endef - -# Link the final object -$(SKETCHELF): $(SKETCHOBJS) $(LIBOBJS) $(CORELIB) - $(RULEHDR) - $(v)$(LD) $(LDFLAGS) -o $@ $^ $(LIBS) - -# Create the hex file -$(SKETCHHEX): $(SKETCHELF) - $(RULEHDR) - $(v)$(OBJCOPY) -O ihex -R .eeprom $< $@ - -# Create the eep file -$(SKETCHEEP): $(SKETCHELF) - $(RULEHDR) - $(v)$(OBJCOPY) -O ihex -j.eeprom --set-section-flags=.eeprom=alloc,load --no-change-warnings --change-section-lma .eeprom=0 $< $@ - -# -# Build sketch objects -# -SKETCH_INCLUDES = $(SKETCHLIBINCLUDES) $(ARDUINOLIBINCLUDES) $(COREINCLUDES) - -$(BUILDROOT)/%.o: $(BUILDROOT)/%.cpp - $(RULEHDR) - $(v)$(CXX) $(CXXFLAGS) -c -o $@ $< -I$(SRCROOT) $(SKETCH_INCLUDES) - -$(BUILDROOT)/%.o: $(SRCROOT)/%.cpp - $(RULEHDR) - $(v)$(CXX) $(CXXFLAGS) -c -o $@ $< $(SKETCH_INCLUDES) - -$(BUILDROOT)/%.o: $(SRCROOT)/%.c - $(RULEHDR) - $(v)$(CC) $(CFLAGS) -c -o $@ $< $(SKETCH_INCLUDES) - -$(BUILDROOT)/%.o: $(SRCROOT)/%.S - $(RULEHDR) - $(v)$(AS) $(ASFLAGS) -c -o $@ $< $(SKETCH_INCLUDES) - -# -# Build library objects from sources in the sketchbook -# -SLIB_INCLUDES = -I$(dir $<)/utility $(SKETCHLIBINCLUDES) $(ARDUINOLIBINCLUDES) $(COREINCLUDES) - -$(BUILDROOT)/libraries/%.o: $(SKETCHBOOK)/libraries/%.cpp - $(RULEHDR) - $(v)$(CXX) $(CXXFLAGS) -c -o $@ $< $(SLIB_INCLUDES) - -$(BUILDROOT)/libraries/%.o: $(SKETCHBOOK)/libraries/%.c - $(RULEHDR) - $(v)$(CC) $(CFLAGS) -c -o $@ $< $(SLIB_INCLUDES) - -$(BUILDROOT)/libraries/%.o: $(SKETCHBOOK)/libraries/%.S - $(RULEHDR) - $(v)$(AS) $(ASFLAGS) -c -o $@ $< $(SLIB_INCLUDES) - -# -# Build library objects from Ardiuno library sources -# -ALIB_INCLUDES = -I$(dir $<)/utility $(ARDUINOLIBINCLUDES) $(COREINCLUDES) - -$(BUILDROOT)/libraries/%.o: $(ARDUINO)/libraries/%.cpp - $(RULEHDR) - $(v)$(CXX) $(CXXFLAGS) -c -o $@ $< $(ALIB_INCLUDES) - -$(BUILDROOT)/libraries/%.o: $(ARDUINO)/libraries/%.c - $(RULEHDR) - $(v)$(CC) $(CFLAGS) -c -o $@ $< $(ALIB_INCLUDES) - -$(BUILDROOT)/libraries/%.o: $(ARDUINO)/libraries/%.S - $(RULEHDR) - $(v)$(AS) $(ASFLAGS) -c -o $@ $< $(ALIB_INCLUDES) - -# -# Build objects from the hardware core -# -$(BUILDROOT)/$(HARDWARE)/%.o: $(CORESRC_DIR)/%.cpp - $(RULEHDR) - $(v)$(CXX) $(filter-out -W%,$(CXXFLAGS)) -c -o $@ $< $(COREINCLUDES) - -$(BUILDROOT)/$(HARDWARE)/%.o: $(CORESRC_DIR)/%.c - @mkdir -p $(dir $@) - $(v)$(CC) $(filter-out -W%,$(CFLAGS)) -c -o $@ $< $(COREINCLUDES) - -$(BUILDROOT)/$(HARDWARE)/%.o: $(CORESRC_DIR)/%.S - $(RULEHDR) - $(v)$(AS) $(ASFLAGS) -c -o $@ $< $(COREINCLUDES) - -# -# Build the core library -# -$(CORELIB): $(CORELIBOBJS) - $(RULEHDR) - $(v)$(AR) -rcs $@ $^ - -# -# Build the sketch.cpp file -# -# This process strives to be as faithful to the Arduino implementation as -# possible. Conceptually, the process is as follows: -# -# * All of the .pde/.ino files are concatenated, starting with the file named -# for the sketch and followed by the others in alphabetical order. -# * An insertion point is created in the concatenated file at -# the first statement that isn't a preprocessor directive or comment. -# * An include of "WProgram.h" is inserted at the insertion point. -# * The file following the insertion point is scanned for function definitions -# and prototypes for these functions are inserted at the insertion point. -# -# In addition, we add #line directives wherever the originating file changes -# to help backtrack from compiler messages and in the debugger. -# -$(SKETCHCPP): $(SKETCHCPP_SRC) - $(RULEHDR) - $(v)$(AWK) -v mode=header '$(SKETCH_SPLITTER)' $(SKETCHCPP_SRC) > $@ - $(v)echo "#line 1 \"autogenerated\"" >> $@ - $(v)echo "#if defined(ARDUINO) && ARDUINO >= 100" >> $@ - $(v)echo "#include \"Arduino.h\"" >> $@ - $(v)echo "#else" >> $@ - $(v)echo "#include \"WProgram.h\"" >> $@ - $(v)echo "#endif" >> $@ - $(v)$(AWK) '$(SKETCH_PROTOTYPER)' $(SKETCHCPP_SRC) >> $@ - $(v)$(AWK) -v mode=body '$(SKETCH_SPLITTER)' $(SKETCHCPP_SRC) >> $@ - -# delete the sketch.cpp file if a processing error occurs -.DELETE_ON_ERROR: $(SKETCHCPP) - -# -# The sketch splitter is an awk script used to split off the -# header and body of the concatenated .pde/.ino files. It also -# inserts #line directives to help in backtracking from compiler -# and debugger messages to the original source file. -# -# Note that # and $ require special treatment here to avoid upsetting -# make. -# -# This script requires BWK or GNU awk. -# -define SKETCH_SPLITTER - BEGIN { \ - scanning = 1; \ - printing = (mode ~ "header") ? 1 : 0; \ - } \ - { toggles = 1 } \ - (FNR == 1) && printing { \ - printf "#line %d \"%s\"\n", FNR, FILENAME; \ - } \ - /^[[:space:]]*\/\*/,/\*\// { \ - toggles = 0; \ - } \ - /^[[:space:]]*$$/ || /^[[:space:]]*\/\/.*/ || /^\#.*$$/ { \ - toggles = 0; \ - } \ - scanning && toggles { \ - scanning = 0; \ - printing = !printing; \ - if (printing) { \ - printf "#line %d \"%s\"\n", FNR, FILENAME; \ - } \ - } \ - printing -endef - -# -# The prototype scanner is an awk script used to generate function -# prototypes from the concantenated .pde/.ino files. -# -# Function definitions are expected to follow the form -# -# [...]([]){ -# -# with whitespace permitted between the various elements. The pattern -# is assembled from separate subpatterns to make adjustments easier. -# -# Note that $ requires special treatment here to avoid upsetting make, -# and backslashes are doubled in the partial patterns to satisfy -# escaping rules. -# -# This script requires BWK or GNU awk. -# -define SKETCH_PROTOTYPER - BEGIN { \ - RS="{"; \ - type = "((\\n)|(^))[[:space:]]*[[:alnum:]_]+[[:space:]]+"; \ - qualifiers = "([[:alnum:]_\\*&]+[[:space:]]*)*"; \ - name = "[[:alnum:]_]+[[:space:]]*"; \ - args = "\\([[:space:][:alnum:]_,&\\*\\[\\]]*\\)"; \ - bodycuddle = "[[:space:]]*$$"; \ - pattern = type qualifiers name args bodycuddle; \ - } \ - match($$0, pattern) { \ - proto = substr($$0, RSTART, RLENGTH); \ - gsub("\n", " ", proto); \ - printf "%s;\n", proto; \ - } -endef diff --git a/libraries/AP_Common/c++.cpp b/libraries/AP_Common/c++.cpp deleted file mode 100644 index 8ea4bc4..0000000 --- a/libraries/AP_Common/c++.cpp +++ /dev/null @@ -1,91 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -// -// C++ runtime support not provided by Arduino -// -// Note: use new/delete with caution. The heap is small and -// easily fragmented. -// - -#include -#include "c++.h" -#if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" -#else - #include "WProgram.h" -#endif - -void * operator new(size_t size) -{ -#ifdef AP_DISPLAYMEM - displayMemory(); -#endif - return(calloc(size, 1)); -} - -void operator delete(void *p) -{ - if (p) free(p); -} - -extern "C" void __cxa_pure_virtual() -{ - while (1) - { - Serial.println("Error: pure virtual call"); - delay(1000); - } -} - -void * operator new[](size_t size) -{ -#ifdef AP_DISPLAYMEM - displayMemory(); -#endif - return(calloc(size, 1)); -} - -void operator delete[](void * ptr) -{ - if (ptr) free(ptr); -} - -__extension__ typedef int __guard __attribute__((mode (__DI__))); - -int __cxa_guard_acquire(__guard *g) -{ - return !*(char *)(g); -}; - -void __cxa_guard_release (__guard *g) -{ - *(char *)g = 1; -}; - -void __cxa_guard_abort (__guard *) {}; - -// free memory -extern unsigned int __bss_end; -extern void *__brkval; - -void displayMemory() -{ - static int minMemFree=0; - if (minMemFree<=0 || freeMemory() -#include - -FastSerialPort0(Serial); - -int8_t -menu_test(uint8_t argc, const Menu::arg *argv) -{ - int i; - - Serial.printf("This is a test with %d arguments\n", argc); - for (i = 1; i < argc; i++) { - Serial.printf("%d: int %ld float ", i, argv[i].i); - Serial.println(argv[i].f, 6); // gross - } -} - -int8_t -menu_auto(uint8_t argc, const Menu::arg *argv) -{ - Serial.println("auto text"); -} - -const struct Menu::command top_menu_commands[] PROGMEM = { - {"*", menu_auto}, - {"test", menu_test}, -}; - -MENU(top, "menu", top_menu_commands); - -void -setup(void) -{ - Serial.begin(38400); - top.run(); -} - -void -loop(void) -{ -} - diff --git a/libraries/AP_Common/include/menu.h b/libraries/AP_Common/include/menu.h deleted file mode 100644 index 2cff1b9..0000000 --- a/libraries/AP_Common/include/menu.h +++ /dev/null @@ -1,143 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -/// @file menu.h -/// @brief Simple commandline menu subsystem. -/// @discussion -/// The Menu class implements a simple CLI that accepts commands typed by -/// the user, and passes the arguments to those commands to a function -/// defined as handing the command. -/// -/// Commands are defined in an array of Menu::command structures passed -/// to the constructor. Each entry in the array defines one command. -/// -/// Arguments passed to the handler function are pre-converted to both -/// long and float for convenience. - -#ifndef __AP_COMMON_MENU_H -#define __AP_COMMON_MENU_H - -#include - -#define MENU_COMMANDLINE_MAX 32 ///< maximum input line length -#define MENU_ARGS_MAX 4 ///< maximum number of arguments -#define MENU_COMMAND_MAX 14 ///< maximum size of a command name - -/// Class defining and handling one menu tree -class Menu { -public: - /// argument passed to a menu function - /// - /// Space-delimited arguments are parsed from the commandline and - /// separated into these structures. - /// - /// If the argument cannot be parsed as a float or a long, the value - /// of f or i respectively is undefined. You should range-check - /// the inputs to your function. - /// - struct arg { - const char *str; ///< string form of the argument - long i; ///< integer form of the argument (if a number) - float f; ///< floating point form of the argument (if a number) - }; - - /// menu command function - /// - /// Functions called by menu array entries are expected to be of this - /// type. - /// - /// @param argc The number of valid arguments, including the - /// name of the command in argv[0]. Will never be - /// more than MENU_ARGS_MAX. - /// @param argv Pointer to an array of Menu::arg structures - /// detailing any optional arguments given to the - /// command. argv[0] is always the name of the - /// command, so that the same function can be used - /// to handle more than one command. - /// - typedef int8_t (*func)(uint8_t argc, const struct arg *argv); - - /// menu pre-prompt function - /// - /// Called immediately before waiting for the user to type a command; can be - /// used to display help text or status, for example. - /// - /// If this function returns false, the menu exits. - /// - typedef bool (*preprompt)(void); - - /// menu command description - /// - struct command { - /// Name of the command, as typed or received. - /// Command names are limited in size to keep this structure compact. - /// - const char command[MENU_COMMAND_MAX]; - - /// The function to call when the command is received. - /// - /// The argc argument will be at least 1, and no more than - /// MENU_ARGS_MAX. The argv array will be populated with - /// arguments typed/received up to MENU_ARGS_MAX. The command - /// name will always be in argv[0]. - /// - /// Commands may return -2 to cause the menu itself to exit. - /// The "?", "help" and "exit" commands are always defined, but - /// can be overridden by explicit entries in the command array. - /// - int8_t (*func)(uint8_t argc, const struct arg *argv); ///< callback function - }; - - /// constructor - /// - /// Note that you should normally not call the constructor directly. Use - /// the MENU and MENU2 macros defined below. - /// - /// @param prompt The prompt to be displayed with this menu. - /// @param commands An array of ::command structures in program memory (PROGMEM). - /// @param entries The number of entries in the menu. - /// - Menu(const char *prompt, const struct command *commands, uint8_t entries, preprompt ppfunc = 0); - - /// menu runner - void run(void); - -private: - /// Implements the default 'help' command. - /// - void _help(void); ///< implements the 'help' command - - /// calls the function for the n'th menu item - /// - /// @param n Index for the menu item to call - /// @param argc Number of arguments prepared for the menu item - /// - int8_t _call(uint8_t n, uint8_t argc); - - const char *_prompt; ///< prompt to display - const command *_commands; ///< array of commands - const uint8_t _entries; ///< size of the menu - const preprompt _ppfunc; ///< optional pre-prompt action - - static char _inbuf[MENU_COMMANDLINE_MAX]; ///< input buffer - static arg _argv[MENU_ARGS_MAX + 1]; ///< arguments -}; - -/// Macros used to define a menu. -/// -/// The commands argument should be an arary of Menu::command structures, one -/// per command name. The array does not need to be terminated with any special -/// record. -/// -/// Use name.run() to run the menu. -/// -/// The MENU2 macro supports the optional pre-prompt printing function. -/// -#define MENU(name, prompt, commands) \ - static const char __menu_name__ ##name[] PROGMEM = prompt; \ - static Menu name(__menu_name__ ##name, commands, sizeof(commands) / sizeof(commands[0])) - -#define MENU2(name, prompt, commands, preprompt) \ - static const char __menu_name__ ##name[] PROGMEM = prompt; \ - static Menu name(__menu_name__ ##name, commands, sizeof(commands) / sizeof(commands[0]), preprompt) - -#endif diff --git a/libraries/AP_Common/keywords.txt b/libraries/AP_Common/keywords.txt deleted file mode 100644 index 470e71f..0000000 --- a/libraries/AP_Common/keywords.txt +++ /dev/null @@ -1,4 +0,0 @@ -Menu KEYWORD1 -run KEYWORD2 -Location KEYWORD2 - diff --git a/libraries/AP_Common/menu.cpp b/libraries/AP_Common/menu.cpp deleted file mode 100644 index 3c29258..0000000 --- a/libraries/AP_Common/menu.cpp +++ /dev/null @@ -1,155 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -// -// Simple commandline menu system. -// - -#include -#include - -#include -#include -#include - -#include "include/menu.h" - -// statics -char Menu::_inbuf[MENU_COMMANDLINE_MAX]; -Menu::arg Menu::_argv[MENU_ARGS_MAX + 1]; - -// constructor -Menu::Menu(const prog_char *prompt, const Menu::command *commands, uint8_t entries, preprompt ppfunc) : - _prompt(prompt), - _commands(commands), - _entries(entries), - _ppfunc(ppfunc) -{ -} - -// run the menu -void -Menu::run(void) -{ - int8_t ret; - uint8_t len, i; - uint8_t argc; - int c; - char *s; - - // loop performing commands - for (;;) { - - // run the pre-prompt function, if one is defined - if ((NULL != _ppfunc) && !_ppfunc()) - return; - - // loop reading characters from the input - len = 0; - Serial.printf("%S] ", FPSTR(_prompt)); - for (;;) { - c = Serial.read(); - if (-1 == c) - continue; - // carriage return -> process command - if ('\r' == c) { - _inbuf[len] = '\0'; - Serial.write('\r'); - Serial.write('\n'); - break; - } - // backspace - if ('\b' == c) { - if (len > 0) { - len--; - Serial.write('\b'); - Serial.write(' '); - Serial.write('\b'); - continue; - } - } - // printable character - if (isprint(c) && (len < (MENU_COMMANDLINE_MAX - 1))) { - _inbuf[len++] = c; - Serial.write((char)c); - continue; - } - } - - // split the input line into tokens - argc = 0; - _argv[argc++].str = strtok_r(_inbuf, " ", &s); - // XXX should an empty line by itself back out of the current menu? - while (argc <= MENU_ARGS_MAX) { - _argv[argc].str = strtok_r(NULL, " ", &s); - if ('\0' == _argv[argc].str) - break; - _argv[argc].i = atol(_argv[argc].str); - _argv[argc].f = atof(_argv[argc].str); // calls strtod, > 700B ! - argc++; - } - - if (_argv[0].str == NULL) { - continue; - } - - // populate arguments that have not been specified with "" and 0 - // this is safer than NULL in the case where commands may look - // without testing argc - i = argc; - while (i <= MENU_ARGS_MAX) { - _argv[i].str = ""; - _argv[i].i = 0; - _argv[i].f = 0; - i++; - } - - bool cmd_found = false; - // look for a command matching the first word (note that it may be empty) - for (i = 0; i < _entries; i++) { - if (!strcasecmp_P(_argv[0].str, _commands[i].command)) { - ret = _call(i, argc); - cmd_found=true; - if (-2 == ret) - return; - break; - } - } - - // implicit commands - if (i == _entries) { - if (!strcmp(_argv[0].str, "?") || (!strcasecmp_P(_argv[0].str, PSTR("help")))) { - _help(); - cmd_found=true; - } else if (!strcasecmp_P(_argv[0].str, PSTR("exit"))) { - return; - } - } - - if (cmd_found==false) - { - Serial.println("Invalid command, type 'help'"); - } - - } -} - -// display the list of commands in response to the 'help' command -void -Menu::_help(void) -{ - int i; - - Serial.println("Commands:"); - for (i = 0; i < _entries; i++) - Serial.printf(" %S\n", FPSTR(_commands[i].command)); -} - -// run the n'th command in the menu -int8_t -Menu::_call(uint8_t n, uint8_t argc) -{ - func fn; - - fn = (func)pgm_read_pointer(&_commands[n].func); - return(fn(argc, &_argv[0])); -} diff --git a/libraries/AP_Common/tools/eedump.c b/libraries/AP_Common/tools/eedump.c deleted file mode 100644 index 638ff1a..0000000 --- a/libraries/AP_Common/tools/eedump.c +++ /dev/null @@ -1,84 +0,0 @@ -/* - * Simple tool to dump the AP_Var contents from an EEPROM dump - */ -#include -#include -#include - -uint8_t eeprom[0x1000]; - -#pragma pack(1) - -struct EEPROM_header { - uint16_t magic; - uint8_t revision; - uint8_t spare; -}; - -static const uint16_t k_EEPROM_magic = 0x5041; -static const uint16_t k_EEPROM_revision = 2; - -struct Var_header { - uint8_t size:6; - uint8_t spare:2; - uint8_t key; -}; - -static const uint8_t k_key_sentinel = 0xff; - -void -fail(const char *why) -{ - fprintf(stderr, "ERROR: %s\n", why); - exit(1); -} - -int -main(int argc, char *argv[]) -{ - FILE *fp; - struct EEPROM_header *header; - struct Var_header *var; - unsigned index; - unsigned i; - - if (argc != 2) { - fail("missing EEPROM file name"); - } - if (NULL == (fp = fopen(argv[1], "rb"))) { - fail("can't open EEPROM file"); - } - if (1 != fread(eeprom, sizeof(eeprom), 1, fp)) { - fail("can't read EEPROM file"); - } - fclose(fp); - - header = (struct EEPROM_header *)&eeprom[0]; - if (header->magic != k_EEPROM_magic) { - fail("bad magic in EEPROM file"); - } - if (header->revision != 2) { - fail("unsupported EEPROM format revision"); - } - printf("Header OK\n"); - - index = sizeof(*header); - for (;;) { - var = (struct Var_header *)&eeprom[index]; - if (var->key == k_key_sentinel) { - printf("end sentinel at %u\n", index); - break; - } - printf("%04x: key %u size %d\n ", index, var->key, var->size + 1); - index += sizeof(*var); - for (i = 0; i <= var->size; i++) { - printf(" %02x", eeprom[index + i]); - } - printf("\n"); - index += var->size + 1; - if (index >= sizeof(eeprom)) { - fflush(stdout); - fail("missing end sentinel"); - } - } -} diff --git a/libraries/AP_Common/tools/eedump.pl b/libraries/AP_Common/tools/eedump.pl deleted file mode 100644 index f9a9e3b..0000000 --- a/libraries/AP_Common/tools/eedump.pl +++ /dev/null @@ -1,47 +0,0 @@ -#!/usr/bin/perl - - -$file = $ARGV[0]; - - -open(IN,$file) || die print "Failed to open file: $file : $!"; - -read(IN,$buffer,1); -read(IN,$buffer2,1); -if (ord($buffer) != 0x41 && ord($buffer2) != 0x50) { - print "bad header ". $buffer ." ".$buffer2. "\n"; - exit; -} -read(IN,$buffer,1); -if (ord($buffer) != 2) { - print "bad version"; - exit; -} - -# spare -read(IN,$buffer,1); - -$a = 0; - -while (read(IN,$buffer,1)) { - $pos = (tell(IN) - 1); - - $size = ((ord($buffer) & 63)); - - read(IN,$buffer,1); - - if (ord($buffer) == 0xff) { - printf("end sentinel at %u\n", $pos); - last; - } - - printf("%04x: key %u size %d\n ", $pos, ord($buffer), $size + 1); - - for ($i = 0; $i <= ($size); $i++) { - read(IN,$buffer,1); - printf(" %02x", ord($buffer)); - } - print "\n"; -} - -close IN; \ No newline at end of file diff --git a/libraries/AP_Common/tools/eedump_apparam.c b/libraries/AP_Common/tools/eedump_apparam.c deleted file mode 100644 index 6f4f717..0000000 --- a/libraries/AP_Common/tools/eedump_apparam.c +++ /dev/null @@ -1,181 +0,0 @@ -/* - * Simple tool to dump the AP_Param contents from an EEPROM dump - * Andrew Tridgell February 2012 - */ -#include -#include -#include - -uint8_t eeprom[0x1000]; - -#pragma pack(1) - -struct EEPROM_header { - uint8_t magic[2]; - uint8_t revision; - uint8_t spare; -}; - -static const uint16_t k_EEPROM_magic0 = 0x50; -static const uint16_t k_EEPROM_magic1 = 0x41; -static const uint16_t k_EEPROM_revision = 5; - -enum ap_var_type { - AP_PARAM_NONE = 0, - AP_PARAM_INT8, - AP_PARAM_INT16, - AP_PARAM_INT32, - AP_PARAM_FLOAT, - AP_PARAM_VECTOR3F, - AP_PARAM_VECTOR6F, - AP_PARAM_MATRIX3F, - AP_PARAM_GROUP -}; - -static const char *type_names[8] = { - "NONE", "INT8", "INT16", "INT32", "FLOAT", "VECTOR3F", "MATRIX6F", "GROUP" -}; - -struct Param_header { - uint8_t key; - uint8_t group_element; - uint8_t type; -}; - - -static const uint8_t _sentinal_key = 0xFF; -static const uint8_t _sentinal_type = 0xFF; -static const uint8_t _sentinal_group = 0xFF; - -static uint8_t type_size(enum ap_var_type type) -{ - switch (type) { - case AP_PARAM_NONE: - case AP_PARAM_GROUP: - return 0; - case AP_PARAM_INT8: - return 1; - case AP_PARAM_INT16: - return 2; - case AP_PARAM_INT32: - return 4; - case AP_PARAM_FLOAT: - return 4; - case AP_PARAM_VECTOR3F: - return 3*4; - case AP_PARAM_VECTOR6F: - return 6*4; - case AP_PARAM_MATRIX3F: - return 3*3*4; - } - printf("unknown type %u\n", type); - return 0; -} - -static void -fail(const char *why) -{ - fprintf(stderr, "ERROR: %s\n", why); - exit(1); -} - -int -main(int argc, char *argv[]) -{ - FILE *fp; - struct EEPROM_header *header; - struct Param_header *var; - unsigned index; - unsigned i; - - if (argc != 2) { - fail("missing EEPROM file name"); - } - if (NULL == (fp = fopen(argv[1], "rb"))) { - fail("can't open EEPROM file"); - } - if (1 != fread(eeprom, sizeof(eeprom), 1, fp)) { - fail("can't read EEPROM file"); - } - fclose(fp); - - header = (struct EEPROM_header *)&eeprom[0]; - if (header->magic[0] != k_EEPROM_magic0 || - header->magic[1] != k_EEPROM_magic1) { - fail("bad magic in EEPROM file"); - } - if (header->revision != k_EEPROM_revision) { - fail("unsupported EEPROM format revision"); - } - printf("Header OK\n"); - - index = sizeof(*header); - for (;;) { - uint8_t size; - var = (struct Param_header *)&eeprom[index]; - if (var->key == _sentinal_key || - var->group_element == _sentinal_group || - var->type == _sentinal_type) { - printf("end sentinel at %u\n", index); - break; - } - size = type_size(var->type); - printf("%04x: type %u (%s) key %u group_element %u size %d value ", - index, var->type, type_names[var->type], var->key, var->group_element, size); - index += sizeof(*var); - switch (var->type) { - case AP_PARAM_INT8: - printf("%d\n", (int)*(int8_t *)&eeprom[index]); - break; - case AP_PARAM_INT16: - printf("%d\n", (int)*(int16_t *)&eeprom[index]); - break; - case AP_PARAM_INT32: - printf("%d\n", (int)*(int32_t *)&eeprom[index]); - break; - case AP_PARAM_FLOAT: - printf("%f\n", *(float *)&eeprom[index]); - break; - case AP_PARAM_VECTOR3F: - printf("%f %f %f\n", - *(float *)&eeprom[index], - *(float *)&eeprom[index+4], - *(float *)&eeprom[index+8]); - break; - case AP_PARAM_VECTOR6F: - printf("%f %f %f %f %f %f\n", - *(float *)&eeprom[index], - *(float *)&eeprom[index+4], - *(float *)&eeprom[index+8], - *(float *)&eeprom[index+12], - *(float *)&eeprom[index+16], - *(float *)&eeprom[index+20]); - break; - case AP_PARAM_MATRIX3F: - printf("%f %f %f %f %f %f %f %f %f\n", - *(float *)&eeprom[index], - *(float *)&eeprom[index+4], - *(float *)&eeprom[index+8], - *(float *)&eeprom[index+12], - *(float *)&eeprom[index+16], - *(float *)&eeprom[index+20], - *(float *)&eeprom[index+24], - *(float *)&eeprom[index+28], - *(float *)&eeprom[index+32]); - break; - default: - printf("NONE\n"); - break; - } - for (i = 0; i < size; i++) { - printf(" %02x", eeprom[index + i]); - } - printf("\n"); - index += size; - if (index >= sizeof(eeprom)) { - fflush(stdout); - fail("missing end sentinel"); - } - } - return 0; -} diff --git a/libraries/AP_Common/tools/eedump_apparam.pl b/libraries/AP_Common/tools/eedump_apparam.pl deleted file mode 100644 index d5e972a..0000000 --- a/libraries/AP_Common/tools/eedump_apparam.pl +++ /dev/null @@ -1,78 +0,0 @@ -#!/usr/bin/perl - - -$file = $ARGV[0]; - - -open(IN,$file) || die print "Failed to open file: $file : $!"; - -read(IN,$buffer,1); -read(IN,$buffer2,1); -if (ord($buffer2) != 0x41 && ord($buffer) != 0x50) { - print "bad header ". $buffer ." ".$buffer2. "\n"; - exit; -} -read(IN,$buffer,1); -if (ord($buffer) != 5) { - print "bad version"; - exit; -} - -# spare -read(IN,$buffer,1); - -$a = 0; - -while (read(IN,$buffer,1)) { - $pos = (tell(IN) - 1); - - if (ord($buffer) == 0xff) { - printf("end sentinel at %u\n", $pos); - last; - } - - read(IN,$buffer2,1); - read(IN,$buffer3,1); - - if (ord($buffer3) == 0) { #none - $size = 0; - $type = "NONE"; - } elsif (ord($buffer3) == 1) { #int8 - $size = 1; - $type = "INT8"; - } elsif (ord($buffer3) == 2) { #int16 - $size = 2; - $type = "INT16"; - } elsif (ord($buffer3) == 3) { #int32 - $size = 4; - $type = "INT32"; - } elsif (ord($buffer3) == 4) { #float - $size = 4; - $type = "FLOAT"; - } elsif (ord($buffer3) == 5) { #vector 3 - $size = 3*4; - $type = "VECTOR3F"; - } elsif (ord($buffer3) == 6) { #vector6 - $size = 6*4; - $type = "VECTOR6F"; - } elsif (ord($buffer3) == 7) { #matrix - $size = 3*3*4; - $type = "MATRIX6F"; - } elsif (ord($buffer3) == 8) { #group - $size = 0; - $type = "GROUP"; - } else { - print "Unknown type\n"; - $size = 0; - } - - printf("%04x: type %u ($type) key %u group_element %u size %d\n ", $pos, ord($buffer3),ord($buffer),ord($buffer2), $size); - - for ($i = 0; $i < ($size); $i++) { - read(IN,$buffer,1); - printf(" %02x", ord($buffer)); - } - print "\n"; -} - -close IN; diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp deleted file mode 100644 index abb8a36..0000000 --- a/libraries/AP_Math/AP_Math.cpp +++ /dev/null @@ -1,69 +0,0 @@ -#include "AP_Math.h" - -// a varient of asin() that checks the input ranges and ensures a -// valid angle as output. If nan is given as input then zero is -// returned. -float safe_asin(float v) -{ - if (isnan(v)) { - return 0.0; - } - if (v >= 1.0) { - return PI/2; - } - if (v <= -1.0) { - return -PI/2; - } - return asin(v); -} - -// a varient of sqrt() that checks the input ranges and ensures a -// valid value as output. If a negative number is given then 0 is -// returned. The reasoning is that a negative number for sqrt() in our -// code is usually caused by small numerical rounding errors, so the -// real input should have been zero -float safe_sqrt(float v) -{ - float ret = sqrt(v); - if (isnan(ret)) { - return 0; - } - return ret; -} - - -// find a rotation that is the combination of two other -// rotations. This is used to allow us to add an overall board -// rotation to an existing rotation of a sensor such as the compass -// Note that this relies the set of rotations being complete. The -// optional 'found' parameter is for the test suite to ensure that it is. -enum Rotation rotation_combination(enum Rotation r1, enum Rotation r2, bool *found) -{ - Vector3f tv1, tv2; - enum Rotation r; - tv1(1,2,3); - tv1.rotate(r1); - tv1.rotate(r2); - - for (r=ROTATION_NONE; r -#include -#include "rotations.h" -#include "vector2.h" -#include "vector3.h" -#include "matrix3.h" -#include "quaternion.h" -#include "polygon.h" - -// define AP_Param types AP_Vector3f and Ap_Matrix3f -AP_PARAMDEFV(Matrix3f, Matrix3f, AP_PARAM_MATRIX3F); -AP_PARAMDEFV(Vector3f, Vector3f, AP_PARAM_VECTOR3F); - -// a varient of asin() that always gives a valid answer. -float safe_asin(float v); - -// a varient of sqrt() that always gives a valid answer. -float safe_sqrt(float v); - -// find a rotation that is the combination of two other -// rotations. This is used to allow us to add an overall board -// rotation to an existing rotation of a sensor such as the compass -enum Rotation rotation_combination(enum Rotation r1, enum Rotation r2, bool *found = NULL); - -#endif diff --git a/libraries/AP_Math/examples/eulers/Makefile b/libraries/AP_Math/examples/eulers/Makefile deleted file mode 100644 index fcdc8ff..0000000 --- a/libraries/AP_Math/examples/eulers/Makefile +++ /dev/null @@ -1,4 +0,0 @@ -include ../../../AP_Common/Arduino.mk - -sitl: - make -f ../../../../libraries/Desktop/Desktop.mk diff --git a/libraries/AP_Math/examples/eulers/eulers.pde b/libraries/AP_Math/examples/eulers/eulers.pde deleted file mode 100644 index 2bbf236..0000000 --- a/libraries/AP_Math/examples/eulers/eulers.pde +++ /dev/null @@ -1,281 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -// -// Unit tests for the AP_Math euler code -// - -#include -#include -#include - -FastSerialPort(Serial, 0); - -#ifdef DESKTOP_BUILD -// all of this is needed to build with SITL -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -Arduino_Mega_ISR_Registry isr_registry; -AP_Baro_BMP085_HIL barometer; -AP_Compass_HIL compass; -#endif - -#include - - -static float rad_diff(float rad1, float rad2) -{ - float diff = rad1 - rad2; - if (diff > PI) { - diff -= 2*PI; - } - if (diff < -PI) { - diff += 2*PI; - } - return fabs(diff); -} - -static void check_result(float roll, float pitch, float yaw, - float roll2, float pitch2, float yaw2) -{ - if (isnan(roll2) || - isnan(pitch2) || - isnan(yaw2)) { - Serial.printf("NAN eulers roll=%f pitch=%f yaw=%f\n", - roll, pitch, yaw); - } - - if (rad_diff(roll2,roll) > ToRad(179)) { - // reverse all 3 - roll2 += fmod(roll2+PI, 2*PI); - pitch2 += fmod(pitch2+PI, 2*PI); - yaw2 += fmod(yaw2+PI, 2*PI); - } - - if (rad_diff(roll2,roll) > 0.01 || - rad_diff(pitch2, pitch) > 0.01 || - rad_diff(yaw2, yaw) > 0.01) { - if (pitch >= PI/2 || - pitch <= -PI/2 || - ToDeg(rad_diff(pitch, PI/2)) < 1 || - ToDeg(rad_diff(pitch, -PI/2)) < 1) { - // we expect breakdown at these poles - Serial.printf("breakdown eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n", - ToDeg(roll), ToDeg(roll2), ToDeg(pitch), ToDeg(pitch2), ToDeg(yaw), ToDeg(yaw2)); - } else { - Serial.printf("incorrect eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n", - ToDeg(roll), ToDeg(roll2), ToDeg(pitch), ToDeg(pitch2), ToDeg(yaw), ToDeg(yaw2)); - } - } -} - -static void test_euler(float roll, float pitch, float yaw) -{ - Matrix3f m; - float roll2, pitch2, yaw2; - - m.from_euler(roll, pitch, yaw); - m.to_euler(&roll2, &pitch2, &yaw2); - check_result(roll, pitch, yaw, roll2, pitch2, yaw2); -} - -#define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0])) - -static const float angles[] = { 0, PI/8, PI/4, PI/2, PI, - -PI/8, -PI/4, -PI/2, -PI}; - -void test_matrix_eulers(void) -{ - uint8_t i, j, k; - uint8_t N = ARRAY_LENGTH(angles); - - Serial.println("rotation matrix unit tests\n"); - - for (i=0; i 0) { - Serial.printf("ERROR: i=%u err=%f\n", (unsigned)i, err); - } - } -} - -/* - euler angle tests - */ -void setup(void) -{ - Serial.begin(115200); - Serial.println("euler unit tests\n"); - - test_conversion(0, PI, 0); - - test_frame_transforms(); - test_conversions(); - test_quaternion_eulers(); - test_matrix_eulers(); - test_matrix_rotate(); -} - -void -loop(void) -{ -} diff --git a/libraries/AP_Math/examples/polygon/Makefile b/libraries/AP_Math/examples/polygon/Makefile deleted file mode 100644 index d1f40fd..0000000 --- a/libraries/AP_Math/examples/polygon/Makefile +++ /dev/null @@ -1 +0,0 @@ -include ../../../AP_Common/Arduino.mk diff --git a/libraries/AP_Math/examples/polygon/polygon.pde b/libraries/AP_Math/examples/polygon/polygon.pde deleted file mode 100644 index 2533137..0000000 --- a/libraries/AP_Math/examples/polygon/polygon.pde +++ /dev/null @@ -1,115 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -// -// Unit tests for the AP_Math polygon code -// - -#include -#include -#include - -FastSerialPort(Serial, 0); - -/* - this is the boundary of the 2010 outback challenge - Note that the last point must be the same as the first for the - Polygon_outside() algorithm - */ -static const Vector2l OBC_boundary[] = { - Vector2l(-265695640, 1518373730), - Vector2l(-265699560, 1518394050), - Vector2l(-265768230, 1518411420), - Vector2l(-265773080, 1518403440), - Vector2l(-265815110, 1518419500), - Vector2l(-265784860, 1518474690), - Vector2l(-265994890, 1518528860), - Vector2l(-266092110, 1518747420), - Vector2l(-266454780, 1518820530), - Vector2l(-266435720, 1518303500), - Vector2l(-265875990, 1518344050), - Vector2l(-265695640, 1518373730) -}; - -static const struct { - Vector2l point; - bool outside; -} test_points[] = { - { Vector2l(-266398870, 1518220000), true }, - { Vector2l(-266418700, 1518709260), false }, - { Vector2l(-350000000, 1490000000), true }, - { Vector2l(0, 0), true }, - { Vector2l(-265768150, 1518408250), false }, - { Vector2l(-265774060, 1518405860), true }, - { Vector2l(-266435630, 1518303440), true }, - { Vector2l(-266435650, 1518313540), false }, - { Vector2l(-266435690, 1518303530), false }, - { Vector2l(-266435690, 1518303490), true }, - { Vector2l(-265875990, 1518344049), true }, - { Vector2l(-265875990, 1518344051), false }, - { Vector2l(-266454781, 1518820530), true }, - { Vector2l(-266454779, 1518820530), true }, - { Vector2l(-266092109, 1518747420), true }, - { Vector2l(-266092111, 1518747420), false }, - { Vector2l(-266092110, 1518747421), true }, - { Vector2l(-266092110, 1518747419), false }, - { Vector2l(-266092111, 1518747421), true }, - { Vector2l(-266092109, 1518747421), true }, - { Vector2l(-266092111, 1518747419), false }, -}; - -#define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0])) - -/* - polygon tests - */ -void setup(void) -{ - unsigned i, count; - bool all_passed = true; - uint32_t start_time; - - Serial.begin(115200); - Serial.println("polygon unit tests\n"); - - if (!Polygon_complete(OBC_boundary, ARRAY_LENGTH(OBC_boundary))) { - Serial.println("OBC boundary is not complete!"); - all_passed = false; - } - - if (Polygon_complete(OBC_boundary, ARRAY_LENGTH(OBC_boundary)-1)) { - Serial.println("Polygon_complete test failed"); - all_passed = false; - } - - for (i=0; i -#include -#include - -FastSerialPort(Serial, 0); - -#ifdef DESKTOP_BUILD -// all of this is needed to build with SITL -#include -#include -#include -#include -#include -#include -#include -#include -#include -Arduino_Mega_ISR_Registry isr_registry; -AP_Baro_BMP085_HIL barometer; -AP_Compass_HIL compass; -#endif - -#include // ArduPilot Mega Declination Helper Library - - -// standard rotation matrices (these are the originals from the old code) -#define MATRIX_ROTATION_NONE Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1) -#define MATRIX_ROTATION_YAW_45 Matrix3f(0.70710678, -0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, 1) -#define MATRIX_ROTATION_YAW_90 Matrix3f(0, -1, 0, 1, 0, 0, 0, 0, 1) -#define MATRIX_ROTATION_YAW_135 Matrix3f(-0.70710678, -0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, 1) -#define MATRIX_ROTATION_YAW_180 Matrix3f(-1, 0, 0, 0, -1, 0, 0, 0, 1) -#define MATRIX_ROTATION_YAW_225 Matrix3f(-0.70710678, 0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, 1) -#define MATRIX_ROTATION_YAW_270 Matrix3f(0, 1, 0, -1, 0, 0, 0, 0, 1) -#define MATRIX_ROTATION_YAW_315 Matrix3f(0.70710678, 0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, 1) -#define MATRIX_ROTATION_ROLL_180 Matrix3f(1, 0, 0, 0, -1, 0, 0, 0, -1) -#define MATRIX_ROTATION_ROLL_180_YAW_45 Matrix3f(0.70710678, 0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, -1) -#define MATRIX_ROTATION_ROLL_180_YAW_90 Matrix3f(0, 1, 0, 1, 0, 0, 0, 0, -1) -#define MATRIX_ROTATION_ROLL_180_YAW_135 Matrix3f(-0.70710678, 0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, -1) -#define MATRIX_ROTATION_PITCH_180 Matrix3f(-1, 0, 0, 0, 1, 0, 0, 0, -1) -#define MATRIX_ROTATION_ROLL_180_YAW_225 Matrix3f(-0.70710678, -0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, -1) -#define MATRIX_ROTATION_ROLL_180_YAW_270 Matrix3f(0, -1, 0, -1, 0, 0, 0, 0, -1) -#define MATRIX_ROTATION_ROLL_180_YAW_315 Matrix3f(0.70710678, -0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, -1) - -static void print_matrix(Matrix3f &m) -{ - Serial.printf("[%.2f %.2f %.2f] [%.2f %.2f %.2f] [%.2f %.2f %.2f]\n", - m.a.x, m.a.y, m.a.z, - m.b.x, m.b.y, m.b.z, - m.c.x, m.c.y, m.c.z); -} - -// test one matrix -static void test_matrix(enum Rotation rotation, Matrix3f m) -{ - Matrix3f m2, diff; - const float accuracy = 1.0e-6; - m2.rotation(rotation); - diff = (m - m2); - if (diff.a.length() > accuracy || - diff.b.length() > accuracy || - diff.c.length() > accuracy) { - Serial.printf("rotation matrix %u incorrect\n", (unsigned)rotation); - print_matrix(m); - print_matrix(m2); - } -} - -// test generation of rotation matrices -static void test_matrices(void) -{ - Serial.println("testing rotation matrices\n"); - test_matrix(ROTATION_NONE, MATRIX_ROTATION_NONE); - test_matrix(ROTATION_YAW_45, MATRIX_ROTATION_YAW_45); - test_matrix(ROTATION_YAW_90, MATRIX_ROTATION_YAW_90); - test_matrix(ROTATION_YAW_135, MATRIX_ROTATION_YAW_135); - test_matrix(ROTATION_YAW_180, MATRIX_ROTATION_YAW_180); - test_matrix(ROTATION_YAW_225, MATRIX_ROTATION_YAW_225); - test_matrix(ROTATION_YAW_270, MATRIX_ROTATION_YAW_270); - test_matrix(ROTATION_YAW_315, MATRIX_ROTATION_YAW_315); - test_matrix(ROTATION_ROLL_180, MATRIX_ROTATION_ROLL_180); - test_matrix(ROTATION_ROLL_180_YAW_45, MATRIX_ROTATION_ROLL_180_YAW_45); - test_matrix(ROTATION_ROLL_180_YAW_90, MATRIX_ROTATION_ROLL_180_YAW_90); - test_matrix(ROTATION_ROLL_180_YAW_135, MATRIX_ROTATION_ROLL_180_YAW_135); - test_matrix(ROTATION_PITCH_180, MATRIX_ROTATION_PITCH_180); - test_matrix(ROTATION_ROLL_180_YAW_225, MATRIX_ROTATION_ROLL_180_YAW_225); - test_matrix(ROTATION_ROLL_180_YAW_270, MATRIX_ROTATION_ROLL_180_YAW_270); - test_matrix(ROTATION_ROLL_180_YAW_315, MATRIX_ROTATION_ROLL_180_YAW_315); -} - -// test rotation of vectors -static void test_vector(enum Rotation rotation, Vector3f v1, bool show=true) -{ - Vector3f v2, diff; - Matrix3f m; - v2 = v1; - m.rotation(rotation); - v1.rotate(rotation); - v2 = m * v2; - diff = v1 - v2; - if (diff.length() > 1.0e-6) { - Serial.printf("rotation vector %u incorrect\n", (unsigned)rotation); - Serial.printf("%u %f %f %f\n", - (unsigned)rotation, - v2.x, v2.y, v2.z); - } - if (show) { - Serial.printf("%u %f %f %f\n", - (unsigned)rotation, - v1.x, v1.y, v1.z); - } -} - -// generate a random float between -1 and 1 -static float rand_num(void) -{ - float ret = ((unsigned)random()) % 2000000; - return (ret - 1.0e6) / 1.0e6; -} - -// test rotation of vectors -static void test_vector(enum Rotation rotation) -{ - uint8_t i; - - Vector3f v1; - v1.x = 1; - v1.y = 2; - v1.z = 3; - test_vector(rotation, v1); - - for (i=0; i<10; i++) { - v1.x = rand_num(); - v1.y = rand_num(); - v1.z = rand_num(); - test_vector(rotation, v1, false); - } -} - -// test rotation of vectors -static void test_vectors(void) -{ - Serial.println("testing rotation of vectors\n"); - test_vector(ROTATION_NONE); - test_vector(ROTATION_YAW_45); - test_vector(ROTATION_YAW_90); - test_vector(ROTATION_YAW_135); - test_vector(ROTATION_YAW_180); - test_vector(ROTATION_YAW_225); - test_vector(ROTATION_YAW_270); - test_vector(ROTATION_YAW_315); - test_vector(ROTATION_ROLL_180); - test_vector(ROTATION_ROLL_180_YAW_45); - test_vector(ROTATION_ROLL_180_YAW_90); - test_vector(ROTATION_ROLL_180_YAW_135); - test_vector(ROTATION_PITCH_180); - test_vector(ROTATION_ROLL_180_YAW_225); - test_vector(ROTATION_ROLL_180_YAW_270); - test_vector(ROTATION_ROLL_180_YAW_315); -} - - -// test combinations of rotations -static void test_combinations(void) -{ - enum Rotation r1, r2, r3; - bool found; - - for (r1=ROTATION_NONE; r1 %u\n", - (unsigned)r1, (unsigned)r2, (unsigned)r3); - } else { - Serial.printf("ERROR rotation: no combination for %u + %u\n", - (unsigned)r1, (unsigned)r2); - } - } - } -} - -/* - rotation tests - */ -void setup(void) -{ - Serial.begin(115200); - Serial.println("rotation unit tests\n"); - test_matrices(); - test_vectors(); - test_combinations(); - Serial.println("rotation unit tests done\n"); -} - -void -loop(void) -{ -} diff --git a/libraries/AP_Math/keywords.txt b/libraries/AP_Math/keywords.txt deleted file mode 100644 index a5b7938..0000000 --- a/libraries/AP_Math/keywords.txt +++ /dev/null @@ -1,29 +0,0 @@ -Vector2 KEYWORD1 -Vector2i KEYWORD1 -Vector2ui KEYWORD1 -Vector2l KEYWORD1 -Vector2ul KEYWORD1 -Vector2f KEYWORD1 -Vector3 KEYWORD1 -Vector3i KEYWORD1 -Vector3ui KEYWORD1 -Vector3l KEYWORD1 -Vector3ul KEYWORD1 -Vector3f KEYWORD1 -Matrix3 KEYWORD1 -Matrix3i KEYWORD1 -Matrix3ui KEYWORD1 -Matrix3l KEYWORD1 -Matrix3ul KEYWORD1 -Matrix3f KEYWORD1 -length_squared KEYWORD2 -length KEYWORD2 -normalize KEYWORD2 -normalized KEYWORD2 -reflect KEYWORD2 -project KEYWORD2 -projected KEYWORD2 -angle KEYWORD2 -angle_normalized KEYWORD2 -rotate KEYWORD2 -rotated KEYWORD2 diff --git a/libraries/AP_Math/matrix3.cpp b/libraries/AP_Math/matrix3.cpp deleted file mode 100644 index 8ad2ffd..0000000 --- a/libraries/AP_Math/matrix3.cpp +++ /dev/null @@ -1,199 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -/* - * matrix3.cpp - * Copyright (C) Andrew Tridgell 2012 - * - * This file 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 3 of the License, or - * (at your option) any later version. - * - * This file 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, see . - */ - -#include "AP_Math.h" - -#define HALF_SQRT_2 0.70710678118654757 - -#define MATRIX_ROTATION_NONE Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1) -#define MATRIX_ROTATION_YAW_45 Matrix3f(HALF_SQRT_2, -HALF_SQRT_2, 0, HALF_SQRT_2, HALF_SQRT_2, 0, 0, 0, 1) -#define MATRIX_ROTATION_YAW_90 Matrix3f(0, -1, 0, 1, 0, 0, 0, 0, 1) -#define MATRIX_ROTATION_YAW_135 Matrix3f(-HALF_SQRT_2, -HALF_SQRT_2, 0, HALF_SQRT_2, -HALF_SQRT_2, 0, 0, 0, 1) -#define MATRIX_ROTATION_YAW_180 Matrix3f(-1, 0, 0, 0, -1, 0, 0, 0, 1) -#define MATRIX_ROTATION_YAW_225 Matrix3f(-HALF_SQRT_2, HALF_SQRT_2, 0, -HALF_SQRT_2, -HALF_SQRT_2, 0, 0, 0, 1) -#define MATRIX_ROTATION_YAW_270 Matrix3f(0, 1, 0, -1, 0, 0, 0, 0, 1) -#define MATRIX_ROTATION_YAW_315 Matrix3f(HALF_SQRT_2, HALF_SQRT_2, 0, -HALF_SQRT_2, HALF_SQRT_2, 0, 0, 0, 1) -#define MATRIX_ROTATION_ROLL_180 Matrix3f(1, 0, 0, 0, -1, 0, 0, 0, -1) -#define MATRIX_ROTATION_ROLL_180_YAW_45 Matrix3f(HALF_SQRT_2, HALF_SQRT_2, 0, HALF_SQRT_2, -HALF_SQRT_2, 0, 0, 0, -1) -#define MATRIX_ROTATION_ROLL_180_YAW_90 Matrix3f(0, 1, 0, 1, 0, 0, 0, 0, -1) -#define MATRIX_ROTATION_ROLL_180_YAW_135 Matrix3f(-HALF_SQRT_2, HALF_SQRT_2, 0, HALF_SQRT_2, HALF_SQRT_2, 0, 0, 0, -1) -#define MATRIX_ROTATION_PITCH_180 Matrix3f(-1, 0, 0, 0, 1, 0, 0, 0, -1) -#define MATRIX_ROTATION_ROLL_180_YAW_225 Matrix3f(-HALF_SQRT_2, -HALF_SQRT_2, 0, -HALF_SQRT_2, HALF_SQRT_2, 0, 0, 0, -1) -#define MATRIX_ROTATION_ROLL_180_YAW_270 Matrix3f(0, -1, 0, -1, 0, 0, 0, 0, -1) -#define MATRIX_ROTATION_ROLL_180_YAW_315 Matrix3f(HALF_SQRT_2, -HALF_SQRT_2, 0, -HALF_SQRT_2, -HALF_SQRT_2, 0, 0, 0, -1) - -// fill in a matrix with a standard rotation -template -void Matrix3::rotation(enum Rotation r) -{ - switch (r) { - case ROTATION_NONE: - case ROTATION_MAX: - *this = MATRIX_ROTATION_NONE; - break; - case ROTATION_YAW_45: - *this = MATRIX_ROTATION_YAW_45; - break; - case ROTATION_YAW_90: - *this = MATRIX_ROTATION_YAW_90; - break; - case ROTATION_YAW_135: - *this = MATRIX_ROTATION_YAW_135; - break; - case ROTATION_YAW_180: - *this = MATRIX_ROTATION_YAW_180; - break; - case ROTATION_YAW_225: - *this = MATRIX_ROTATION_YAW_225; - break; - case ROTATION_YAW_270: - *this = MATRIX_ROTATION_YAW_270; - break; - case ROTATION_YAW_315: - *this = MATRIX_ROTATION_YAW_315; - break; - case ROTATION_ROLL_180: - *this = MATRIX_ROTATION_ROLL_180; - break; - case ROTATION_ROLL_180_YAW_45: - *this = MATRIX_ROTATION_ROLL_180_YAW_45; - break; - case ROTATION_ROLL_180_YAW_90: - *this = MATRIX_ROTATION_ROLL_180_YAW_90; - break; - case ROTATION_ROLL_180_YAW_135: - *this = MATRIX_ROTATION_ROLL_180_YAW_135; - break; - case ROTATION_PITCH_180: - *this = MATRIX_ROTATION_PITCH_180; - break; - case ROTATION_ROLL_180_YAW_225: - *this = MATRIX_ROTATION_ROLL_180_YAW_225; - break; - case ROTATION_ROLL_180_YAW_270: - *this = MATRIX_ROTATION_ROLL_180_YAW_270; - break; - case ROTATION_ROLL_180_YAW_315: - *this = MATRIX_ROTATION_ROLL_180_YAW_315; - break; - } -} - -// create a rotation matrix given some euler angles -// this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf -template -void Matrix3::from_euler(float roll, float pitch, float yaw) -{ - float cp = cos(pitch); - float sp = sin(pitch); - float sr = sin(roll); - float cr = cos(roll); - float sy = sin(yaw); - float cy = cos(yaw); - - a.x = cp * cy; - a.y = (sr * sp * cy) - (cr * sy); - a.z = (cr * sp * cy) + (sr * sy); - b.x = cp * sy; - b.y = (sr * sp * sy) + (cr * cy); - b.z = (cr * sp * sy) - (sr * cy); - c.x = -sp; - c.y = sr * cp; - c.z = cr * cp; -} - -// calculate euler angles from a rotation matrix -// this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf -template -void Matrix3::to_euler(float *roll, float *pitch, float *yaw) -{ - if (pitch != NULL) { - *pitch = -safe_asin(c.x); - } - if (roll != NULL) { - *roll = atan2(c.y, c.z); - } - if (yaw != NULL) { - *yaw = atan2(b.x, a.x); - } -} - -// apply an additional rotation from a body frame gyro vector -// to a rotation matrix. -template -void Matrix3::rotate(const Vector3 &g) -{ - Matrix3f temp_matrix; - temp_matrix.a.x = a.y * g.z - a.z * g.y; - temp_matrix.a.y = a.z * g.x - a.x * g.z; - temp_matrix.a.z = a.x * g.y - a.y * g.x; - temp_matrix.b.x = b.y * g.z - b.z * g.y; - temp_matrix.b.y = b.z * g.x - b.x * g.z; - temp_matrix.b.z = b.x * g.y - b.y * g.x; - temp_matrix.c.x = c.y * g.z - c.z * g.y; - temp_matrix.c.y = c.z * g.x - c.x * g.z; - temp_matrix.c.z = c.x * g.y - c.y * g.x; - - (*this) += temp_matrix; -} - - -// multiplication by a vector -template -Vector3 Matrix3::operator *(const Vector3 &v) const -{ - return Vector3(a.x * v.x + a.y * v.y + a.z * v.z, - b.x * v.x + b.y * v.y + b.z * v.z, - c.x * v.x + c.y * v.y + c.z * v.z); -} - -// multiplication of transpose by a vector -template -Vector3 Matrix3::mul_transpose(const Vector3 &v) const -{ - return Vector3(a.x * v.x + b.x * v.y + c.x * v.z, - a.y * v.x + b.y * v.y + c.y * v.z, - a.z * v.x + b.z * v.y + c.z * v.z); -} - -// multiplication by another Matrix3 -template -Matrix3 Matrix3::operator *(const Matrix3 &m) const -{ - Matrix3 temp (Vector3(a.x * m.a.x + a.y * m.b.x + a.z * m.c.x, - a.x * m.a.y + a.y * m.b.y + a.z * m.c.y, - a.x * m.a.z + a.y * m.b.z + a.z * m.c.z), - Vector3(b.x * m.a.x + b.y * m.b.x + b.z * m.c.x, - b.x * m.a.y + b.y * m.b.y + b.z * m.c.y, - b.x * m.a.z + b.y * m.b.z + b.z * m.c.z), - Vector3(c.x * m.a.x + c.y * m.b.x + c.z * m.c.x, - c.x * m.a.y + c.y * m.b.y + c.z * m.c.y, - c.x * m.a.z + c.y * m.b.z + c.z * m.c.z)); - return temp; -} - - -// only define for float -template void Matrix3::rotation(enum Rotation); -template void Matrix3::rotate(const Vector3 &g); -template void Matrix3::from_euler(float roll, float pitch, float yaw); -template void Matrix3::to_euler(float *roll, float *pitch, float *yaw); -template Vector3 Matrix3::operator *(const Vector3 &v) const; -template Vector3 Matrix3::mul_transpose(const Vector3 &v) const; -template Matrix3 Matrix3::operator *(const Matrix3 &m) const; diff --git a/libraries/AP_Math/matrix3.h b/libraries/AP_Math/matrix3.h deleted file mode 100644 index 202d294..0000000 --- a/libraries/AP_Math/matrix3.h +++ /dev/null @@ -1,153 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -// Copyright 2010 Michael Smith, all rights reserved. - -// This library is free software; you can redistribute it and / or -// modify it under the terms of the GNU Lesser General Public -// License as published by the Free Software Foundation; either -// version 2.1 of the License, or (at your option) any later version. - -// Inspired by: -/**************************************** - * 3D Vector Classes - * By Bill Perone (billperone@yahoo.com) - */ - -// -// 3x3 matrix implementation. -// -// Note that the matrix is organised in row-normal form (the same as -// applies to array indexing). -// -// In addition to the template, this header defines the following types: -// -// Matrix3i 3x3 matrix of signed integers -// Matrix3ui 3x3 matrix of unsigned integers -// Matrix3l 3x3 matrix of signed longs -// Matrix3ul 3x3 matrix of unsigned longs -// Matrix3f 3x3 matrix of signed floats -// - -#ifndef MATRIX3_H -#define MATRIX3_H - -#include "vector3.h" - -// 3x3 matrix with elements of type T -template -class Matrix3 { -public: - - // Vectors comprising the rows of the matrix - Vector3 a, b, c; - - // trivial ctor - // note that the Vector3 ctor will zero the vector elements - Matrix3() {} - - // setting ctor - Matrix3(const Vector3 a0, const Vector3 b0, const Vector3 c0): a(a0), b(b0), c(c0) {} - - // setting ctor - Matrix3(const T ax, const T ay, const T az, const T bx, const T by, const T bz, const T cx, const T cy, const T cz): a(ax,ay,az), b(bx,by,bz), c(cx,cy,cz) {} - - // function call operator - void operator () (const Vector3 a0, const Vector3 b0, const Vector3 c0) - { a = a0; b = b0; c = c0; } - - // test for equality - bool operator == (const Matrix3 &m) - { return (a==m.a && b==m.b && c==m.c); } - - // test for inequality - bool operator != (const Matrix3 &m) - { return (a!=m.a || b!=m.b || c!=m.c); } - - // negation - Matrix3 operator - (void) const - { return Matrix3(-a,-b,-c); } - - // addition - Matrix3 operator + (const Matrix3 &m) const - { return Matrix3(a+m.a, b+m.b, c+m.c); } - Matrix3 &operator += (const Matrix3 &m) - { return *this = *this + m; } - - // subtraction - Matrix3 operator - (const Matrix3 &m) const - { return Matrix3(a-m.a, b-m.b, c-m.c); } - Matrix3 &operator -= (const Matrix3 &m) - { return *this = *this - m; } - - // uniform scaling - Matrix3 operator * (const T num) const - { return Matrix3(a*num, b*num, c*num); } - Matrix3 &operator *= (const T num) - { return *this = *this * num; } - Matrix3 operator / (const T num) const - { return Matrix3(a/num, b/num, c/num); } - Matrix3 &operator /= (const T num) - { return *this = *this / num; } - - // multiplication by a vector - Vector3 operator *(const Vector3 &v) const; - - // multiplication of transpose by a vector - Vector3 mul_transpose(const Vector3 &v) const; - - // multiplication by another Matrix3 - Matrix3 operator *(const Matrix3 &m) const; - - Matrix3 &operator *=(const Matrix3 &m) - { return *this = *this * m; } - - // transpose the matrix - Matrix3 transposed(void) const - { - return Matrix3(Vector3(a.x, b.x, c.x), - Vector3(a.y, b.y, c.y), - Vector3(a.z, b.z, c.z)); - } - Matrix3 transpose(void) - { return *this = transposed(); } - - // zero the matrix - void zero(void) { - a.x = a.y = a.z = 0; - b.x = b.y = b.z = 0; - c.x = c.y = c.z = 0; - } - - // setup the identity matrix - void identity(void) { - a.x = b.y = c.z = 1; - a.y = a.z = 0; - b.x = b.z = 0; - c.x = c.y = 0; - } - - // check if any elements are NAN - bool is_nan(void) - { return a.is_nan() || b.is_nan() || c.is_nan(); } - - // fill in the matrix with a standard rotation - void rotation(enum Rotation rotation); - - // create a rotation matrix from Euler angles - void from_euler(float roll, float pitch, float yaw); - - // create eulers from a rotation matrix - void to_euler(float *roll, float *pitch, float *yaw); - - // apply an additional rotation from a body frame gyro vector - // to a rotation matrix. - void rotate(const Vector3 &g); -}; - -typedef Matrix3 Matrix3i; -typedef Matrix3 Matrix3ui; -typedef Matrix3 Matrix3l; -typedef Matrix3 Matrix3ul; -typedef Matrix3 Matrix3f; - -#endif // MATRIX3_H diff --git a/libraries/AP_Math/polygon.cpp b/libraries/AP_Math/polygon.cpp deleted file mode 100644 index 05374b5..0000000 --- a/libraries/AP_Math/polygon.cpp +++ /dev/null @@ -1,91 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -/* - * polygon.cpp - * Copyright (C) Andrew Tridgell 2011 - * - * This file 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 3 of the License, or - * (at your option) any later version. - * - * This file 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, see . - */ - -#include "AP_Math.h" - -/* - The point in polygon algorithm is based on: - http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html -*/ - - -/* - Polygon_outside(): test for a point in a polygon - Input: P = a point, - V[] = vertex points of a polygon V[n+1] with V[n]=V[0] - Return: true if P is outside the polygon - - This does not take account of the curvature of the earth, but we - expect that to be very small over the distances involved in the - fence boundary -*/ -bool Polygon_outside(const Vector2l &P, const Vector2l *V, unsigned n) -{ - unsigned i, j; - bool outside = true; - for (i = 0, j = n-1; i < n; j = i++) { - if ((V[i].y > P.y) == (V[j].y > P.y)) { - continue; - } - int32_t dx1, dx2, dy1, dy2; - dx1 = P.x - V[i].x; - dx2 = V[j].x - V[i].x; - dy1 = P.y - V[i].y; - dy2 = V[j].y - V[i].y; - int8_t dx1s, dx2s, dy1s, dy2s, m1, m2; -#define sign(x) ((x)<0?-1:1) - dx1s = sign(dx1); - dx2s = sign(dx2); - dy1s = sign(dy1); - dy2s = sign(dy2); - m1 = dx1s * dy2s; - m2 = dx2s * dy1s; - // we avoid the 64 bit multiplies if we can based on sign checks. - if (dy2 < 0) { - if (m1 > m2) { - outside = !outside; - } else if (m1 < m2) { - continue; - } else if ( dx1 * (int64_t)dy2 > dx2 * (int64_t)dy1 ) { - outside = !outside; - } - } else { - if (m1 < m2) { - outside = !outside; - } else if (m1 > m2) { - continue; - } else if ( dx1 * (int64_t)dy2 < dx2 * (int64_t)dy1 ) { - outside = !outside; - } - } - } - return outside; -} - -/* - check if a polygon is complete. - - We consider a polygon to be complete if we have at least 4 points, - and the first point is the same as the last point. That is the - minimum requirement for the Polygon_outside function to work - */ -bool Polygon_complete(const Vector2l *V, unsigned n) -{ - return (n >= 4 && V[n-1].x == V[0].x && V[n-1].y == V[0].y); -} diff --git a/libraries/AP_Math/polygon.h b/libraries/AP_Math/polygon.h deleted file mode 100644 index ae36ee3..0000000 --- a/libraries/AP_Math/polygon.h +++ /dev/null @@ -1,22 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -/* - * polygon.h - * Copyright (C) Andrew Tridgell 2011 - * - * This file 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 3 of the License, or - * (at your option) any later version. - * - * This file 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, see . - */ - -bool Polygon_outside(const Vector2l &P, const Vector2l *V, unsigned n); -bool Polygon_complete(const Vector2l *V, unsigned n); - diff --git a/libraries/AP_Math/quaternion.cpp b/libraries/AP_Math/quaternion.cpp deleted file mode 100644 index 014662a..0000000 --- a/libraries/AP_Math/quaternion.cpp +++ /dev/null @@ -1,89 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -/* - * quaternion.cpp - * Copyright (C) Andrew Tridgell 2012 - * - * This file 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 3 of the License, or - * (at your option) any later version. - * - * This file 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, see . - */ - -#include "AP_Math.h" - -// return the rotation matrix equivalent for this quaternion -void Quaternion::rotation_matrix(Matrix3f &m) -{ - float q3q3 = q3 * q3; - float q3q4 = q3 * q4; - float q2q2 = q2 * q2; - float q2q3 = q2 * q3; - float q2q4 = q2 * q4; - float q1q2 = q1 * q2; - float q1q3 = q1 * q3; - float q1q4 = q1 * q4; - float q4q4 = q4 * q4; - - m.a.x = 1-2*(q3q3 + q4q4); - m.a.y = 2*(q2q3 - q1q4); - m.a.z = 2*(q2q4 + q1q3); - m.b.x = 2*(q2q3 + q1q4); - m.b.y = 1-2*(q2q2 + q4q4); - m.b.z = 2*(q3q4 - q1q2); - m.c.x = 2*(q2q4 - q1q3); - m.c.y = 2*(q3q4 + q1q2); - m.c.z = 1-2*(q2q2 + q3q3); -} - -// convert a vector from earth to body frame -void Quaternion::earth_to_body(Vector3f &v) -{ - Matrix3f m; - // we reverse z before and afterwards because of the differing - // quaternion conventions from APM conventions. - v.z = -v.z; - rotation_matrix(m); - v = m * v; - v.z = -v.z; -} - -// create a quaternion from Euler angles -void Quaternion::from_euler(float roll, float pitch, float yaw) -{ - float cr2 = cos(roll*0.5); - float cp2 = cos(pitch*0.5); - float cy2 = cos(yaw*0.5); - float sr2 = sin(roll*0.5); - float sp2 = sin(pitch*0.5); - float sy2 = sin(yaw*0.5); - - q1 = cr2*cp2*cy2 + sr2*sp2*sy2; - q2 = sr2*cp2*cy2 - cr2*sp2*sy2; - q3 = cr2*sp2*cy2 + sr2*cp2*sy2; - q4 = cr2*cp2*sy2 - sr2*sp2*cy2; -} - -// create eulers from a quaternion -void Quaternion::to_euler(float *roll, float *pitch, float *yaw) -{ - if (roll) { - *roll = (atan2(2.0*(q1*q2 + q3*q4), - 1 - 2.0*(q2*q2 + q3*q3))); - } - if (pitch) { - // we let safe_asin() handle the singularities near 90/-90 in pitch - *pitch = safe_asin(2.0*(q1*q3 - q4*q2)); - } - if (yaw) { - *yaw = atan2(2.0*(q1*q4 + q2*q3), - 1 - 2.0*(q3*q3 + q4*q4)); - } -} diff --git a/libraries/AP_Math/quaternion.h b/libraries/AP_Math/quaternion.h deleted file mode 100644 index f1932db..0000000 --- a/libraries/AP_Math/quaternion.h +++ /dev/null @@ -1,48 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -// Copyright 2012 Andrew Tridgell, all rights reserved. - -// This library is free software; you can redistribute it and / or -// modify it under the terms of the GNU Lesser General Public -// License as published by the Free Software Foundation; either -// version 2.1 of the License, or (at your option) any later version. - -#ifndef QUATERNION_H -#define QUATERNION_H - -#include - -class Quaternion -{ -public: - float q1, q2, q3, q4; - - // constructor creates a quaternion equivalent - // to roll=0, pitch=0, yaw=0 - Quaternion() { q1 = 1; q2 = q3 = q4 = 0; } - - // setting constructor - Quaternion(const float _q1, const float _q2, const float _q3, const float _q4): - q1(_q1), q2(_q2), q3(_q3), q4(_q4) {} - - // function call operator - void operator ()(const float _q1, const float _q2, const float _q3, const float _q4) - { q1 = _q1; q2 = _q2; q3 = _q3; q4 = _q4; } - - // check if any elements are NAN - bool is_nan(void) - { return isnan(q1) || isnan(q2) || isnan(q3) || isnan(q4); } - - // return the rotation matrix equivalent for this quaternion - void rotation_matrix(Matrix3f &m); - - // convert a vector from earth to body frame - void earth_to_body(Vector3f &v); - - // create a quaternion from Euler angles - void from_euler(float roll, float pitch, float yaw); - - // create eulers from a quaternion - void to_euler(float *roll, float *pitch, float *yaw); -}; -#endif // QUATERNION_H diff --git a/libraries/AP_Math/rotations.h b/libraries/AP_Math/rotations.h deleted file mode 100644 index ca648bd..0000000 --- a/libraries/AP_Math/rotations.h +++ /dev/null @@ -1,46 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -/* - * rotations.h - * Copyright (C) Andrew Tridgell 2012 - * - * This file 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 3 of the License, or - * (at your option) any later version. - * - * This file 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, see . - */ - - -// these rotations form a full set - every rotation in the following -// list when combined with another in the list forms an entry which is -// also in the list. This is an important property. Please run the -// rotations test suite if you add to the list. - -// these rotation values are stored to EEPROM, so be careful not to -// change the numbering of any existing entry when adding a new entry. -enum Rotation { - ROTATION_NONE = 0, - ROTATION_YAW_45, - ROTATION_YAW_90, - ROTATION_YAW_135, - ROTATION_YAW_180, - ROTATION_YAW_225, - ROTATION_YAW_270, - ROTATION_YAW_315, - ROTATION_ROLL_180, - ROTATION_ROLL_180_YAW_45, - ROTATION_ROLL_180_YAW_90, - ROTATION_ROLL_180_YAW_135, - ROTATION_PITCH_180, - ROTATION_ROLL_180_YAW_225, - ROTATION_ROLL_180_YAW_270, - ROTATION_ROLL_180_YAW_315, - ROTATION_MAX -}; diff --git a/libraries/AP_Math/vector2.h b/libraries/AP_Math/vector2.h deleted file mode 100644 index 6a3240a..0000000 --- a/libraries/AP_Math/vector2.h +++ /dev/null @@ -1,157 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -// Copyright 2010 Michael Smith, all rights reserved. - -// This library is free software; you can redistribute it and / or -// modify it under the terms of the GNU Lesser General Public -// License as published by the Free Software Foundation; either -// version 2.1 of the License, or (at your option) any later version. - -// Derived closely from: -/**************************************** - * 2D Vector Classes - * By Bill Perone (billperone@yahoo.com) - * Original: 9-16-2002 - * Revised: 19-11-2003 - * 18-12-2003 - * 06-06-2004 - * - * © 2003, This code is provided "as is" and you can use it freely as long as - * credit is given to Bill Perone in the application it is used in - ****************************************/ - -#ifndef VECTOR2_H -#define VECTOR2_H - -#include - -template -struct Vector2 -{ - T x, y; - - // trivial ctor - Vector2() { x = y = 0; } - - // setting ctor - Vector2(const T x0, const T y0): x(x0), y(y0) {} - - // function call operator - void operator ()(const T x0, const T y0) - { x= x0; y= y0; } - - // test for equality - bool operator==(const Vector2 &v) - { return (x==v.x && y==v.y); } - - // test for inequality - bool operator!=(const Vector2 &v) - { return (x!=v.x || y!=v.y); } - - // negation - Vector2 operator -(void) const - { return Vector2(-x, -y); } - - // addition - Vector2 operator +(const Vector2 &v) const - { return Vector2(x+v.x, y+v.y); } - - // subtraction - Vector2 operator -(const Vector2 &v) const - { return Vector2(x-v.x, y-v.y); } - - // uniform scaling - Vector2 operator *(const T num) const - { - Vector2 temp(*this); - return temp*=num; - } - - // uniform scaling - Vector2 operator /(const T num) const - { - Vector2 temp(*this); - return temp/=num; - } - - // addition - Vector2 &operator +=(const Vector2 &v) - { - x+=v.x; y+=v.y; - return *this; - } - - // subtraction - Vector2 &operator -=(const Vector2 &v) - { - x-=v.x; y-=v.y; - return *this; - } - - // uniform scaling - Vector2 &operator *=(const T num) - { - x*=num; y*=num; - return *this; - } - - // uniform scaling - Vector2 &operator /=(const T num) - { - x/=num; y/=num; - return *this; - } - - // dot product - T operator *(const Vector2 &v) const - { return x*v.x + y*v.y; } - - // gets the length of this vector squared - T length_squared() const - { return (T)(*this * *this); } - - // gets the length of this vector - T length() const - { return (T)sqrt(*this * *this); } - - // normalizes this vector - void normalize() - { *this/=length(); } - - // returns the normalized vector - Vector2 normalized() const - { return *this/length(); } - - // reflects this vector about n - void reflect(const Vector2 &n) - { - Vector2 orig(*this); - project(n); - *this= *this*2 - orig; - } - - // projects this vector onto v - void project(const Vector2 &v) - { *this= v * (*this * v)/(v*v); } - - // returns this vector projected onto v - Vector2 projected(const Vector2 &v) - { return v * (*this * v)/(v*v); } - - // computes the angle between 2 arbitrary vectors - T angle(const Vector2 &v1, const Vector2 &v2) - { return (T)acosf((v1*v2) / (v1.length()*v2.length())); } - - // computes the angle between 2 normalized arbitrary vectors - T angle_normalized(const Vector2 &v1, const Vector2 &v2) - { return (T)acosf(v1*v2); } - -}; - -typedef Vector2 Vector2i; -typedef Vector2 Vector2ui; -typedef Vector2 Vector2l; -typedef Vector2 Vector2ul; -typedef Vector2 Vector2f; - -#endif // VECTOR2_H diff --git a/libraries/AP_Math/vector3.cpp b/libraries/AP_Math/vector3.cpp deleted file mode 100644 index cd37348..0000000 --- a/libraries/AP_Math/vector3.cpp +++ /dev/null @@ -1,115 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -/* - * vector3.cpp - * Copyright (C) Andrew Tridgell 2012 - * - * This file 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 3 of the License, or - * (at your option) any later version. - * - * This file 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, see . - */ - -#include "AP_Math.h" - -#define HALF_SQRT_2 0.70710678118654757 - -// rotate a vector by a standard rotation, attempting -// to use the minimum number of floating point operations -template -void Vector3::rotate(enum Rotation rotation) -{ - T tmp; - switch (rotation) { - case ROTATION_NONE: - case ROTATION_MAX: - return; - case ROTATION_YAW_45: { - tmp = HALF_SQRT_2*(x - y); - y = HALF_SQRT_2*(x + y); - x = tmp; - return; - } - case ROTATION_YAW_90: { - tmp = x; x = -y; y = tmp; - return; - } - case ROTATION_YAW_135: { - tmp = -HALF_SQRT_2*(x + y); - y = HALF_SQRT_2*(x - y); - x = tmp; - return; - } - case ROTATION_YAW_180: - x = -x; y = -y; - return; - case ROTATION_YAW_225: { - tmp = HALF_SQRT_2*(y - x); - y = -HALF_SQRT_2*(x + y); - x = tmp; - return; - } - case ROTATION_YAW_270: { - tmp = x; x = y; y = -tmp; - return; - } - case ROTATION_YAW_315: { - tmp = HALF_SQRT_2*(x + y); - y = HALF_SQRT_2*(y - x); - x = tmp; - return; - } - case ROTATION_ROLL_180: { - y = -y; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_45: { - tmp = HALF_SQRT_2*(x + y); - y = HALF_SQRT_2*(x - y); - x = tmp; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_90: { - tmp = x; x = y; y = tmp; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_135: { - tmp = HALF_SQRT_2*(y - x); - y = HALF_SQRT_2*(y + x); - x = tmp; z = -z; - return; - } - case ROTATION_PITCH_180: { - x = -x; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_225: { - tmp = -HALF_SQRT_2*(x + y); - y = HALF_SQRT_2*(y - x); - x = tmp; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_270: { - tmp = x; x = -y; y = -tmp; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_315: { - tmp = HALF_SQRT_2*(x - y); - y = -HALF_SQRT_2*(x + y); - x = tmp; z = -z; - return; - } - } -} - -// only define for signed numbers -template void Vector3::rotate(enum Rotation); -template void Vector3::rotate(enum Rotation); -template void Vector3::rotate(enum Rotation); diff --git a/libraries/AP_Math/vector3.h b/libraries/AP_Math/vector3.h deleted file mode 100644 index acabf17..0000000 --- a/libraries/AP_Math/vector3.h +++ /dev/null @@ -1,199 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -// Copyright 2010 Michael Smith, all rights reserved. - -// This library is free software; you can redistribute it and / or -// modify it under the terms of the GNU Lesser General Public -// License as published by the Free Software Foundation; either -// version 2.1 of the License, or (at your option) any later version. - -// Derived closely from: -/**************************************** - * 3D Vector Classes - * By Bill Perone (billperone@yahoo.com) - * Original: 9-16-2002 - * Revised: 19-11-2003 - * 11-12-2003 - * 18-12-2003 - * 06-06-2004 - * - * © 2003, This code is provided "as is" and you can use it freely as long as - * credit is given to Bill Perone in the application it is used in - * - * Notes: - * if a*b = 0 then a & b are orthogonal - * a%b = -b%a - * a*(b%c) = (a%b)*c - * a%b = a(cast to matrix)*b - * (a%b).length() = area of parallelogram formed by a & b - * (a%b).length() = a.length()*b.length() * sin(angle between a & b) - * (a%b).length() = 0 if angle between a & b = 0 or a.length() = 0 or b.length() = 0 - * a * (b%c) = volume of parallelpiped formed by a, b, c - * vector triple product: a%(b%c) = b*(a*c) - c*(a*b) - * scalar triple product: a*(b%c) = c*(a%b) = b*(c%a) - * vector quadruple product: (a%b)*(c%d) = (a*c)*(b*d) - (a*d)*(b*c) - * if a is unit vector along b then a%b = -b%a = -b(cast to matrix)*a = 0 - * vectors a1...an are linearly dependant if there exists a vector of scalars (b) where a1*b1 + ... + an*bn = 0 - * or if the matrix (A) * b = 0 - * - ****************************************/ - -#ifndef VECTOR3_H -#define VECTOR3_H - -#include -#include - -template -class Vector3 -{ -public: - T x, y, z; - - // trivial ctor - Vector3() { x = y = z = 0; } - - // setting ctor - Vector3(const T x0, const T y0, const T z0): x(x0), y(y0), z(z0) {} - - // function call operator - void operator ()(const T x0, const T y0, const T z0) - { x= x0; y= y0; z= z0; } - - // test for equality - bool operator==(const Vector3 &v) - { return (x==v.x && y==v.y && z==v.z); } - - // test for inequality - bool operator!=(const Vector3 &v) - { return (x!=v.x || y!=v.y || z!=v.z); } - - // negation - Vector3 operator -(void) const - { return Vector3(-x,-y,-z); } - - // addition - Vector3 operator +(const Vector3 &v) const - { return Vector3(x+v.x, y+v.y, z+v.z); } - - // subtraction - Vector3 operator -(const Vector3 &v) const - { return Vector3(x-v.x, y-v.y, z-v.z); } - - // uniform scaling - Vector3 operator *(const T num) const - { - Vector3 temp(*this); - return temp*=num; - } - - // uniform scaling - Vector3 operator /(const T num) const - { - Vector3 temp(*this); - return temp/=num; - } - - // addition - Vector3 &operator +=(const Vector3 &v) - { - x+=v.x; y+=v.y; z+=v.z; - return *this; - } - - // subtraction - Vector3 &operator -=(const Vector3 &v) - { - x-=v.x; y-=v.y; z-=v.z; - return *this; - } - - // uniform scaling - Vector3 &operator *=(const T num) - { - x*=num; y*=num; z*=num; - return *this; - } - - // uniform scaling - Vector3 &operator /=(const T num) - { - x/=num; y/=num; z/=num; - return *this; - } - - // dot product - T operator *(const Vector3 &v) const - { return x*v.x + y*v.y + z*v.z; } - - // cross product - Vector3 operator %(const Vector3 &v) const - { - Vector3 temp(y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x); - return temp; - } - - // gets the length of this vector squared - T length_squared() const - { return (T)(*this * *this); } - - // gets the length of this vector - float length() const - { return (T)sqrt(*this * *this); } - - // normalizes this vector - void normalize() - { *this/=length(); } - - // zero the vector - void zero() - { x = y = z = 0.0; } - - // returns the normalized version of this vector - Vector3 normalized() const - { return *this/length(); } - - // reflects this vector about n - void reflect(const Vector3 &n) - { - Vector3 orig(*this); - project(n); - *this= *this*2 - orig; - } - - // projects this vector onto v - void project(const Vector3 &v) - { *this= v * (*this * v)/(v*v); } - - // returns this vector projected onto v - Vector3 projected(const Vector3 &v) - { return v * (*this * v)/(v*v); } - - // computes the angle between 2 arbitrary vectors - T angle(const Vector3 &v1, const Vector3 &v2) - { return (T)acosf((v1*v2) / (v1.length()*v2.length())); } - - // computes the angle between 2 arbitrary normalized vectors - T angle_normalized(const Vector3 &v1, const Vector3 &v2) - { return (T)acosf(v1*v2); } - - // check if any elements are NAN - bool is_nan(void) - { return isnan(x) || isnan(y) || isnan(z); } - - // check if any elements are infinity - bool is_inf(void) - { return isinf(x) || isinf(y) || isinf(z); } - - // rotate by a standard rotation - void rotate(enum Rotation rotation); - -}; - -typedef Vector3 Vector3i; -typedef Vector3 Vector3ui; -typedef Vector3 Vector3l; -typedef Vector3 Vector3ul; -typedef Vector3 Vector3f; - -#endif // VECTOR3_H diff --git a/libraries/FastSerial/BetterStream.cpp b/libraries/BetterStream/BetterStream.cpp similarity index 83% rename from libraries/FastSerial/BetterStream.cpp rename to libraries/BetterStream/BetterStream.cpp index e8cb7b9..6d934f7 100644 --- a/libraries/FastSerial/BetterStream.cpp +++ b/libraries/BetterStream/BetterStream.cpp @@ -18,16 +18,16 @@ // Stream extensions//////////////////////////////////////////////////////////// void -BetterStream::print_P(const prog_char_t *s) +BetterStream::print_P(const char *s) { char c; - while ('\0' != (c = pgm_read_byte((const prog_char *)s++))) + while ('\0' != (c = pgm_read_byte(s++))) write(c); } void -BetterStream::println_P(const prog_char_t *s) +BetterStream::println_P(const char *s) { print_P(s); println(); @@ -44,7 +44,7 @@ BetterStream::printf(const char *fmt, ...) } void -BetterStream::_printf_P(const prog_char *fmt, ...) +BetterStream::_printf_P(const char *fmt, ...) { va_list ap; diff --git a/libraries/FastSerial/BetterStream.h b/libraries/BetterStream/BetterStream.h similarity index 77% rename from libraries/FastSerial/BetterStream.h rename to libraries/BetterStream/BetterStream.h index 791075a..fc11724 100644 --- a/libraries/FastSerial/BetterStream.h +++ b/libraries/BetterStream/BetterStream.h @@ -13,7 +13,6 @@ #include #include -#include "../AP_Common/AP_Common.h" class BetterStream : public Stream { public: @@ -21,16 +20,16 @@ class BetterStream : public Stream { } // Stream extensions - void print_P(const prog_char_t *); - void println_P(const prog_char_t *); + void print_P(const char *); + void println_P(const char *); void printf(const char *, ...) __attribute__ ((format(__printf__, 2, 3))); - void _printf_P(const prog_char *, ...); + void _printf_P(const char *, ...); __attribute__ ((format(__printf__, 2, 3))); virtual int txspace(void); -#define printf_P(fmt, ...) _printf_P((const prog_char *)fmt, ## __VA_ARGS__) +#define printf_P(fmt, ...) _printf_P((const char *)fmt, ## __VA_ARGS__) private: void _vprintf(unsigned char, const char *, va_list) @@ -38,4 +37,3 @@ class BetterStream : public Stream { }; #endif // __BETTERSTREAM_H - diff --git a/libraries/FastSerial/ftoa_engine.S b/libraries/BetterStream/ftoa_engine.S similarity index 100% rename from libraries/FastSerial/ftoa_engine.S rename to libraries/BetterStream/ftoa_engine.S diff --git a/libraries/FastSerial/ftoa_engine.h b/libraries/BetterStream/ftoa_engine.h similarity index 100% rename from libraries/FastSerial/ftoa_engine.h rename to libraries/BetterStream/ftoa_engine.h diff --git a/libraries/FastSerial/macros.inc b/libraries/BetterStream/macros.inc similarity index 100% rename from libraries/FastSerial/macros.inc rename to libraries/BetterStream/macros.inc diff --git a/libraries/FastSerial/ntz.h b/libraries/BetterStream/ntz.h similarity index 100% rename from libraries/FastSerial/ntz.h rename to libraries/BetterStream/ntz.h diff --git a/libraries/FastSerial/ultoa_invert.S b/libraries/BetterStream/ultoa_invert.S similarity index 100% rename from libraries/FastSerial/ultoa_invert.S rename to libraries/BetterStream/ultoa_invert.S diff --git a/libraries/FastSerial/vprintf.cpp b/libraries/BetterStream/vprintf.cpp similarity index 99% rename from libraries/FastSerial/vprintf.cpp rename to libraries/BetterStream/vprintf.cpp index 53c22c6..326d1ef 100644 --- a/libraries/FastSerial/vprintf.cpp +++ b/libraries/BetterStream/vprintf.cpp @@ -52,7 +52,7 @@ extern "C" { #undef PROGMEM #define PROGMEM __attribute__(( section(".progmem.data") )) #undef PSTR -#define PSTR(s) (__extension__({static prog_char __c[] PROGMEM = (s); &__c[0];})) +#define PSTR(s) (__extension__({static const char __c[] PROGMEM = (s); &__c[0];})) #define GETBYTE(flag, mask, pnt) ({ \ unsigned char __c; \ @@ -258,7 +258,7 @@ BetterStream::_vprintf (unsigned char in_progmem, const char *fmt, va_list ap) p = PSTR("inf"); if (vtype & FTOA_NAN) p = PSTR("nan"); - while ( (ndigs = pgm_read_byte((const prog_char *)p)) != 0) { + while ( (ndigs = pgm_read_byte((const char *)p)) != 0) { if (flags & FL_FLTUPP) ndigs += 'I' - 'i'; write(ndigs); diff --git a/libraries/FastSerial/xtoa_fast.h b/libraries/BetterStream/xtoa_fast.h similarity index 100% rename from libraries/FastSerial/xtoa_fast.h rename to libraries/BetterStream/xtoa_fast.h diff --git a/libraries/EEPROM/EEPROM.cpp b/libraries/EEPROM/EEPROM.cpp deleted file mode 100644 index dfa1deb..0000000 --- a/libraries/EEPROM/EEPROM.cpp +++ /dev/null @@ -1,50 +0,0 @@ -/* - EEPROM.cpp - EEPROM library - Copyright (c) 2006 David A. Mellis. All right reserved. - - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - - This library 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 - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with this library; if not, write to the Free Software - Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -/****************************************************************************** - * Includes - ******************************************************************************/ - -#include -#include "Arduino.h" -#include "EEPROM.h" - -/****************************************************************************** - * Definitions - ******************************************************************************/ - -/****************************************************************************** - * Constructors - ******************************************************************************/ - -/****************************************************************************** - * User API - ******************************************************************************/ - -uint8_t EEPROMClass::read(int address) -{ - return eeprom_read_byte((unsigned char *) address); -} - -void EEPROMClass::write(int address, uint8_t value) -{ - eeprom_write_byte((unsigned char *) address, value); -} - -EEPROMClass EEPROM; diff --git a/libraries/EEPROM/EEPROM.h b/libraries/EEPROM/EEPROM.h deleted file mode 100644 index aa2b577..0000000 --- a/libraries/EEPROM/EEPROM.h +++ /dev/null @@ -1,35 +0,0 @@ -/* - EEPROM.h - EEPROM library - Copyright (c) 2006 David A. Mellis. All right reserved. - - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - - This library 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 - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with this library; if not, write to the Free Software - Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef EEPROM_h -#define EEPROM_h - -#include - -class EEPROMClass -{ - public: - uint8_t read(int); - void write(int, uint8_t); -}; - -extern EEPROMClass EEPROM; - -#endif - diff --git a/libraries/EEPROM/examples/eeprom_clear/eeprom_clear.ino b/libraries/EEPROM/examples/eeprom_clear/eeprom_clear.ino deleted file mode 100644 index d1e29bd..0000000 --- a/libraries/EEPROM/examples/eeprom_clear/eeprom_clear.ino +++ /dev/null @@ -1,23 +0,0 @@ -/* - * EEPROM Clear - * - * Sets all of the bytes of the EEPROM to 0. - * This example code is in the public domain. - - */ - -#include - -void setup() -{ - // write a 0 to all 512 bytes of the EEPROM - for (int i = 0; i < 512; i++) - EEPROM.write(i, 0); - - // turn the LED on when we're done - digitalWrite(13, HIGH); -} - -void loop() -{ -} diff --git a/libraries/EEPROM/examples/eeprom_read/eeprom_read.ino b/libraries/EEPROM/examples/eeprom_read/eeprom_read.ino deleted file mode 100644 index 0709b2d..0000000 --- a/libraries/EEPROM/examples/eeprom_read/eeprom_read.ino +++ /dev/null @@ -1,43 +0,0 @@ -/* - * EEPROM Read - * - * Reads the value of each byte of the EEPROM and prints it - * to the computer. - * This example code is in the public domain. - */ - -#include - -// start reading from the first byte (address 0) of the EEPROM -int address = 0; -byte value; - -void setup() -{ - // initialize serial and wait for port to open: - Serial.begin(9600); - while (!Serial) { - ; // wait for serial port to connect. Needed for Leonardo only - } -} - -void loop() -{ - // read a byte from the current address of the EEPROM - value = EEPROM.read(address); - - Serial.print(address); - Serial.print("\t"); - Serial.print(value, DEC); - Serial.println(); - - // advance to the next address of the EEPROM - address = address + 1; - - // there are only 512 bytes of EEPROM, from 0 to 511, so if we're - // on address 512, wrap around to address 0 - if (address == 512) - address = 0; - - delay(500); -} diff --git a/libraries/EEPROM/examples/eeprom_write/eeprom_write.ino b/libraries/EEPROM/examples/eeprom_write/eeprom_write.ino deleted file mode 100644 index ae7c57e..0000000 --- a/libraries/EEPROM/examples/eeprom_write/eeprom_write.ino +++ /dev/null @@ -1,38 +0,0 @@ -/* - * EEPROM Write - * - * Stores values read from analog input 0 into the EEPROM. - * These values will stay in the EEPROM when the board is - * turned off and may be retrieved later by another sketch. - */ - -#include - -// the current address in the EEPROM (i.e. which byte -// we're going to write to next) -int addr = 0; - -void setup() -{ -} - -void loop() -{ - // need to divide by 4 because analog inputs range from - // 0 to 1023 and each byte of the EEPROM can only hold a - // value from 0 to 255. - int val = analogRead(0) / 4; - - // write the value to the appropriate byte of the EEPROM. - // these values will remain there when the board is - // turned off. - EEPROM.write(addr, val); - - // advance to the next address. there are 512 bytes in - // the EEPROM, so go back to 0 when we hit 512. - addr = addr + 1; - if (addr == 512) - addr = 0; - - delay(100); -} diff --git a/libraries/EEPROM/keywords.txt b/libraries/EEPROM/keywords.txt deleted file mode 100644 index d3218fe..0000000 --- a/libraries/EEPROM/keywords.txt +++ /dev/null @@ -1,18 +0,0 @@ -####################################### -# Syntax Coloring Map For Ultrasound -####################################### - -####################################### -# Datatypes (KEYWORD1) -####################################### - -EEPROM KEYWORD1 - -####################################### -# Methods and Functions (KEYWORD2) -####################################### - -####################################### -# Constants (LITERAL1) -####################################### - diff --git a/libraries/FastSerial/FastSerial.cpp b/libraries/FastSerial/FastSerial.cpp deleted file mode 100644 index 67f7148..0000000 --- a/libraries/FastSerial/FastSerial.cpp +++ /dev/null @@ -1,304 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -// -// Interrupt-driven serial transmit/receive library. -// -// Copyright (c) 2010 Michael Smith. All rights reserved. -// -// Receive and baudrate calculations derived from the Arduino -// HardwareSerial driver: -// -// Copyright (c) 2006 Nicholas Zambetti. All right reserved. -// -// Transmit algorithm inspired by work: -// -// Code Jose Julio and Jordi Munoz. DIYDrones.com -// -// This library is free software; you can redistribute it and/or -// modify it under the terms of the GNU Lesser General Public -// License as published by the Free Software Foundation; either -// version 2.1 of the License, or (at your option) any later version. -// -// This library 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 -// Lesser General Public License for more details. -// -// You should have received a copy of the GNU Lesser General Public -// License along with this library; if not, write to the Free Software -// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA -// - - -//#include "../AP_Common/AP_Common.h" -#include "FastSerial.h" - -#if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" -#else - #include "WProgram.h" -#endif - -#if defined(UDR3) -# define FS_MAX_PORTS 4 -#elif defined(UDR2) -# define FS_MAX_PORTS 3 -#elif defined(UDR1) -# define FS_MAX_PORTS 2 -#else -# define FS_MAX_PORTS 1 -#endif - -FastSerial::Buffer __FastSerial__rxBuffer[FS_MAX_PORTS]; -FastSerial::Buffer __FastSerial__txBuffer[FS_MAX_PORTS]; -uint8_t FastSerial::_serialInitialized = 0; - -// Constructor ///////////////////////////////////////////////////////////////// - -FastSerial::FastSerial(const uint8_t portNumber, volatile uint8_t *ubrrh, volatile uint8_t *ubrrl, - volatile uint8_t *ucsra, volatile uint8_t *ucsrb, const uint8_t u2x, - const uint8_t portEnableBits, const uint8_t portTxBits) : - _ubrrh(ubrrh), - _ubrrl(ubrrl), - _ucsra(ucsra), - _ucsrb(ucsrb), - _u2x(u2x), - _portEnableBits(portEnableBits), - _portTxBits(portTxBits), - _rxBuffer(&__FastSerial__rxBuffer[portNumber]), - _txBuffer(&__FastSerial__txBuffer[portNumber]) -{ - setInitialized(portNumber); - begin(57600); -} - -// Public Methods ////////////////////////////////////////////////////////////// - -void FastSerial::begin(long baud) -{ - begin(baud, 0, 0); -} - -void FastSerial::begin(long baud, unsigned int rxSpace, unsigned int txSpace) -{ - uint16_t ubrr; - bool use_u2x = true; - - // if we are currently open... - if (_open) { - // If the caller wants to preserve the buffer sizing, work out what - // it currently is... - if (0 == rxSpace) - rxSpace = _rxBuffer->mask + 1; - if (0 == txSpace) - txSpace = _txBuffer->mask + 1; - - // close the port in its current configuration, clears _open - end(); - } - - // allocate buffers - if (!_allocBuffer(_rxBuffer, rxSpace ? : _default_rx_buffer_size) || !_allocBuffer(_txBuffer, txSpace ? : _default_tx_buffer_size)) { - end(); - return; // couldn't allocate buffers - fatal - } - - // reset buffer pointers - _txBuffer->head = _txBuffer->tail = 0; - _rxBuffer->head = _rxBuffer->tail = 0; - - // mark the port as open - _open = true; - - // If the user has supplied a new baud rate, compute the new UBRR value. - if (baud > 0) { -#if F_CPU == 16000000UL - // hardcoded exception for compatibility with the bootloader shipped - // with the Duemilanove and previous boards and the firmware on the 8U2 - // on the Uno and Mega 2560. - if (baud == 57600) - use_u2x = false; -#endif - - if (use_u2x) { - *_ucsra = 1 << _u2x; - ubrr = (F_CPU / 4 / baud - 1) / 2; - } else { - *_ucsra = 0; - ubrr = (F_CPU / 8 / baud - 1) / 2; - } - - *_ubrrh = ubrr >> 8; - *_ubrrl = ubrr; - } - - *_ucsrb |= _portEnableBits; -} - -void FastSerial::end() -{ - *_ucsrb &= ~(_portEnableBits | _portTxBits); - - _freeBuffer(_rxBuffer); - _freeBuffer(_txBuffer); - _open = false; -} - -int FastSerial::available(void) -{ - if (!_open) - return (-1); - return ((_rxBuffer->head - _rxBuffer->tail) & _rxBuffer->mask); -} - -int FastSerial::txspace(void) -{ - if (!_open) - return (-1); - return ((_txBuffer->mask+1) - ((_txBuffer->head - _txBuffer->tail) & _txBuffer->mask)); -} - -int FastSerial::read(void) -{ - uint8_t c; - - // if the head and tail are equal, the buffer is empty - if (!_open || (_rxBuffer->head == _rxBuffer->tail)) - return (-1); - - // pull character from tail - c = _rxBuffer->bytes[_rxBuffer->tail]; - _rxBuffer->tail = (_rxBuffer->tail + 1) & _rxBuffer->mask; - - return (c); -} - -int FastSerial::peek(void) -{ - - // if the head and tail are equal, the buffer is empty - if (!_open || (_rxBuffer->head == _rxBuffer->tail)) - return (-1); - - // pull character from tail - return (_rxBuffer->bytes[_rxBuffer->tail]); -} - -void FastSerial::flush(void) -{ - // don't reverse this or there may be problems if the RX interrupt - // occurs after reading the value of _rxBuffer->head but before writing - // the value to _rxBuffer->tail; the previous value of head - // may be written to tail, making it appear as if the buffer - // don't reverse this or there may be problems if the RX interrupt - // occurs after reading the value of head but before writing - // the value to tail; the previous value of rx_buffer_head - // may be written to tail, making it appear as if the buffer - // were full, not empty. - _rxBuffer->head = _rxBuffer->tail; - - // don't reverse this or there may be problems if the TX interrupt - // occurs after reading the value of _txBuffer->tail but before writing - // the value to _txBuffer->head. - _txBuffer->tail = _txBuffer->head; -} - -#if defined(ARDUINO) && ARDUINO >= 100 -size_t FastSerial::write(uint8_t c) -{ - uint16_t i; - - if (!_open) // drop bytes if not open - return 0; - - // wait for room in the tx buffer - i = (_txBuffer->head + 1) & _txBuffer->mask; - - // if the port is set into non-blocking mode, then drop the byte - // if there isn't enough room for it in the transmit buffer - if (_nonblocking_writes && i == _txBuffer->tail) { - return 0; - } - - while (i == _txBuffer->tail) - ; - - // add byte to the buffer - _txBuffer->bytes[_txBuffer->head] = c; - _txBuffer->head = i; - - // enable the data-ready interrupt, as it may be off if the buffer is empty - *_ucsrb |= _portTxBits; - - // return number of bytes written (always 1) - return 1; -} -#else -void FastSerial::write(uint8_t c) -{ - uint16_t i; - - if (!_open) // drop bytes if not open - return; - - // wait for room in the tx buffer - i = (_txBuffer->head + 1) & _txBuffer->mask; - while (i == _txBuffer->tail) - ; - - // add byte to the buffer - _txBuffer->bytes[_txBuffer->head] = c; - _txBuffer->head = i; - - // enable the data-ready interrupt, as it may be off if the buffer is empty - *_ucsrb |= _portTxBits; -} -#endif - -// Buffer management /////////////////////////////////////////////////////////// - -bool FastSerial::_allocBuffer(Buffer *buffer, unsigned int size) -{ - uint16_t mask; - uint8_t shift; - - // init buffer state - buffer->head = buffer->tail = 0; - - // Compute the power of 2 greater or equal to the requested buffer size - // and then a mask to simplify wrapping operations. Using __builtin_clz - // would seem to make sense, but it uses a 256(!) byte table. - // Note that we ignore requests for more than BUFFER_MAX space. - for (shift = 1; (1U << shift) < min(_max_buffer_size, size); shift++) - ; - mask = (1 << shift) - 1; - - // If the descriptor already has a buffer allocated we need to take - // care of it. - if (buffer->bytes) { - - // If the allocated buffer is already the correct size then - // we have nothing to do - if (buffer->mask == mask) - return true; - - // Dispose of the old buffer. - free(buffer->bytes); - } - buffer->mask = mask; - - // allocate memory for the buffer - if this fails, we fail. - buffer->bytes = (uint8_t *) malloc(buffer->mask + 1); - - return (buffer->bytes != NULL); -} - -void FastSerial::_freeBuffer(Buffer *buffer) -{ - buffer->head = buffer->tail = 0; - buffer->mask = 0; - if (NULL != buffer->bytes) { - free(buffer->bytes); - buffer->bytes = NULL; - } -} - diff --git a/libraries/FastSerial/FastSerial.h b/libraries/FastSerial/FastSerial.h deleted file mode 100644 index b64ff1f..0000000 --- a/libraries/FastSerial/FastSerial.h +++ /dev/null @@ -1,334 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -// -// Interrupt-driven serial transmit/receive library. -// -// Copyright (c) 2010 Michael Smith. All rights reserved. -// -// Receive and baudrate calculations derived from the Arduino -// HardwareSerial driver: -// -// Copyright (c) 2006 Nicholas Zambetti. All right reserved. -// -// Transmit algorithm inspired by work: -// -// Code Jose Julio and Jordi Munoz. DIYDrones.com -// -// This library is free software; you can redistribute it and/or -// modify it under the terms of the GNU Lesser General Public -// License as published by the Free Software Foundation; either -// version 2.1 of the License, or (at your option) any later -// version. -// -// This library 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 Lesser General Public License for more -// details. -// -// You should have received a copy of the GNU Lesser General -// Public License along with this library; if not, write to the -// Free Software Foundation, Inc., 51 Franklin St, Fifth Floor, -// Boston, MA 02110-1301 USA -// - -// -// Note that this library does not pre-declare drivers for serial -// ports; the user must explicitly create drivers for the ports they -// wish to use. This is less friendly than the stock Arduino driver, -// but it saves a few bytes of RAM for every unused port and frees up -// the vector for another driver (e.g. MSPIM on USARTs). -// - -#ifndef FastSerial_h -#define FastSerial_h - -// disable the stock Arduino serial driver -#ifdef HardwareSerial_h -# error Must include FastSerial.h before the Arduino serial driver is defined. -#endif -#define HardwareSerial_h - -#include -#include -#include -#include - -#include "BetterStream.h" - - -/// @file FastSerial.h -/// @brief An enhanced version of the Arduino HardwareSerial class -/// implementing interrupt-driven transmission and flexible -/// buffer management. -/// -/// Because Arduino libraries aren't really libraries, but we want to -/// only define interrupt handlers for serial ports that are actually -/// used, we have to force our users to define them using a macro. -/// -/// FastSerialPort(, ) -/// -/// is the name of the object that will be created by the -/// macro. is the 0-based number of the port that will -/// be managed by the object. -/// -/// Previously ports were defined with a different macro for each port, -/// and these macros are retained for compatibility: -/// -/// FastSerialPort0() creates referencing serial port 0 -/// FastSerialPort1() creates referencing serial port 1 -/// FastSerialPort2() creates referencing serial port 2 -/// FastSerialPort3() creates referencing serial port 3 -/// -/// Note that compatibility macros are only defined for ports that -/// exist on the target device. -/// - -/// @name Compatibility -/// -/// Forward declarations for clients that want to assume that the -/// default Serial* objects exist. -/// -/// Note that the application is responsible for ensuring that these -/// actually get defined, otherwise Arduino will suck in the -/// HardwareSerial library and linking will fail. -//@{ -extern class FastSerial Serial; -extern class FastSerial Serial1; -extern class FastSerial Serial2; -extern class FastSerial Serial3; -//@} - -/// The FastSerial class definition -/// -class FastSerial: public BetterStream { -public: - - /// Constructor - FastSerial(const uint8_t portNumber, volatile uint8_t *ubrrh, volatile uint8_t *ubrrl, volatile uint8_t *ucsra, - volatile uint8_t *ucsrb, const uint8_t u2x, const uint8_t portEnableBits, const uint8_t portTxBits); - - /// @name Serial API - //@{ - virtual void begin(long baud); - virtual void end(void); - virtual int available(void); - virtual int txspace(void); - virtual int read(void); - virtual int peek(void); - virtual void flush(void); -#if defined(ARDUINO) && ARDUINO >= 100 - virtual size_t write(uint8_t c); -#else - virtual void write(uint8_t c); -#endif - using BetterStream::write; - //@} - - /// Extended port open method - /// - /// Allows for both opening with specified buffer sizes, and re-opening - /// to adjust a subset of the port's settings. - /// - /// @note Buffer sizes greater than ::_max_buffer_size will be rounded - /// down. - /// - /// @param baud Selects the speed that the port will be - /// configured to. If zero, the port speed is left - /// unchanged. - /// @param rxSpace Sets the receive buffer size for the port. If zero - /// then the buffer size is left unchanged if the port - /// is open, or set to ::_default_rx_buffer_size if it is - /// currently closed. - /// @param txSpace Sets the transmit buffer size for the port. If zero - /// then the buffer size is left unchanged if the port - /// is open, or set to ::_default_tx_buffer_size if it - /// is currently closed. - /// - virtual void begin(long baud, unsigned int rxSpace, unsigned int txSpace); - - /// Transmit/receive buffer descriptor. - /// - /// Public so the interrupt handlers can see it - struct Buffer { - volatile uint16_t head, tail; ///< head and tail pointers - uint16_t mask; ///< buffer size mask for pointer wrap - uint8_t *bytes; ///< pointer to allocated buffer - }; - - /// Tell if the serial port has been initialized - static bool getInitialized(uint8_t port) { - return (1< - -#undef PROGMEM -#define PROGMEM __attribute__(( section(".progmem.data") )) - -# undef PSTR -# define PSTR(s) (__extension__({static prog_char __c[] PROGMEM = (s); \ - (prog_char_t *)&__c[0];})) - -// -// Create a FastSerial driver that looks just like the stock Arduino -// driver. -// -FastSerialPort0(Serial); - -// -// To create a driver for a different serial port, on a board that -// supports more than one, use the appropriate macro: -// -//FastSerialPort2(Serial2); - - -void setup(void) -{ - // - // Set the speed for our replacement serial port. - // - Serial.begin(115200); - - // - // Test printing things - // - Serial.print("test"); - Serial.println(" begin"); - Serial.println(1000); - Serial.println(1000, 8); - Serial.println(1000, 10); - Serial.println(1000, 16); - Serial.println_P(PSTR("progmem")); - Serial.printf("printf %d %u %#x %p %f %S\n", -1000, 1000, 1000, 1000, 1.2345, PSTR("progmem")); - Serial.printf_P(PSTR("printf_P %d %u %#x %p %f %S\n"), -1000, 1000, 1000, 1000, 1.2345, PSTR("progmem")); - Serial.println("done"); -} - -void -loop(void) -{ - int c; - - // - // Perform a simple loopback operation. - // - c = Serial.read(); - if (-1 != c) - Serial.write(c); -} - diff --git a/libraries/FastSerial/examples/FastSerial/Makefile b/libraries/FastSerial/examples/FastSerial/Makefile deleted file mode 100644 index 85b4d88..0000000 --- a/libraries/FastSerial/examples/FastSerial/Makefile +++ /dev/null @@ -1,2 +0,0 @@ -BOARD = mega -include ../../../AP_Common/Arduino.mk diff --git a/libraries/FastSerial/keywords.txt b/libraries/FastSerial/keywords.txt deleted file mode 100644 index 5be403f..0000000 --- a/libraries/FastSerial/keywords.txt +++ /dev/null @@ -1,7 +0,0 @@ -FastSerial KEYWORD1 -begin KEYWORD2 -end KEYWORD2 -available KEYWORD2 -read KEYWORD2 -flush KEYWORD2 -write KEYWORD2 diff --git a/rx5808_pro_osd/ArduCam_Max7456.cpp b/rx5808_pro_osd/ArduCam_Max7456.cpp index 66a3249..de2aac5 100644 --- a/rx5808_pro_osd/ArduCam_Max7456.cpp +++ b/rx5808_pro_osd/ArduCam_Max7456.cpp @@ -1,5 +1,5 @@ -#include +#include // Needed for PROGMEM stuff //#include "BetterStream.h" #include "ArduCam_Max7456.h" diff --git a/rx5808_pro_osd/ArduCam_Max7456.h b/rx5808_pro_osd/ArduCam_Max7456.h index e3cb786..0aa1575 100644 --- a/rx5808_pro_osd/ArduCam_Max7456.h +++ b/rx5808_pro_osd/ArduCam_Max7456.h @@ -2,6 +2,7 @@ #ifndef ArduCam_Max7456_h #define ArduCam_Max7456_h +#include /******* FROM DATASHEET *******/ #define MAX7456_SELECT 6//SS diff --git a/rx5808_pro_osd/rx5808_pro_osd.ino b/rx5808_pro_osd/rx5808_pro_osd.ino index da7afa5..6154b42 100644 --- a/rx5808_pro_osd/rx5808_pro_osd.ino +++ b/rx5808_pro_osd/rx5808_pro_osd.ino @@ -50,7 +50,6 @@ along with this program. If not, see // AVR Includes -#include // better steam #include // Needed for PROGMEM stuff /* **********************************************/ @@ -207,7 +206,6 @@ const uint8_t MagicKey[] PROGMEM = { // key to check if EEprom matches software // Objects and Serial definitions -FastSerialPort0(Serial); // just for character update OSD osd; //OSD object // global variables @@ -2204,10 +2202,10 @@ void uploadFont() osd_print_P(1,11,P_text_5); // enable UART for update Serial.begin(TELEMETRY_SPEED); - Serial.printf_P(PSTR("\n\nReady for font upload. Please send *.mcm file.\n\n")); - Serial.printf_P(PSTR("You will get a message for each character stored.\n")); - Serial.printf_P(PSTR("System will reboot when all 255 characters are stored.\n")); - Serial.printf_P(PSTR("\nTo skip the update, just restart the system.\n")); + Serial.print(F("\n\nReady for font upload. Please send *.mcm file.\n\n")); + Serial.print(F("You will get a message for each character stored.\n")); + Serial.print(F("System will reboot when all 255 characters are stored.\n")); + Serial.print(F("\nTo skip the update, just restart the system.\n")); while(font_count < 255) { int8_t incomingByte = Serial.read(); @@ -2278,11 +2276,10 @@ void uploadFont() Serial.println(" chars done"); } } - Serial.printf_P(PSTR("Font update done. Restarting system..\n\n")); + Serial.print(F("Font update done. Restarting system..\n\n")); delay(2000); asm volatile (" jmp 0"); // softstart by jumping to start vector // REBOOT, WILL NEVER GET HERE - } From c93a4bdf845b819db49ed2896a638fdb56586248 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Thu, 7 Apr 2016 00:00:53 +0200 Subject: [PATCH 2/3] fixed last compilation error to get Arduino 1.6.7 compatibility --- rx5808_pro_osd/rx5808_pro_osd.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rx5808_pro_osd/rx5808_pro_osd.ino b/rx5808_pro_osd/rx5808_pro_osd.ino index 6154b42..1ad40df 100644 --- a/rx5808_pro_osd/rx5808_pro_osd.ino +++ b/rx5808_pro_osd/rx5808_pro_osd.ino @@ -1417,7 +1417,7 @@ void osd_print (uint8_t x, uint8_t y, const char string[30]) osd.closePanel(); } // special print using PROGMEM strings -void osd_print_P (uint8_t x, uint8_t y, const prog_char text[]) +void osd_print_P (uint8_t x, uint8_t y, const char* text) { char P_print_buffer[30]; strcpy_P(P_print_buffer,text); // copy PROGMEM string to local buffer From e83df518bab5ae5588b4d446ba0b84d2fd5b8311 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Wed, 20 Apr 2016 13:11:31 +0200 Subject: [PATCH 3/3] Use external sync when OSD off. --- rx5808_pro_osd/ArduCam_Max7456.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rx5808_pro_osd/ArduCam_Max7456.cpp b/rx5808_pro_osd/ArduCam_Max7456.cpp index de2aac5..a0edff5 100644 --- a/rx5808_pro_osd/ArduCam_Max7456.cpp +++ b/rx5808_pro_osd/ArduCam_Max7456.cpp @@ -239,7 +239,7 @@ OSD::control(uint8_t ctrl){ Spi.transfer(MAX7456_VM0_reg); switch(ctrl){ case 0: // OSD off - Spi.transfer(MAX7456_DISABLE_display | video_mode); + Spi.transfer(MAX7456_DISABLE_display | video_mode | MAX7456_SYNC_external); break; case 1: // OSD on with autosync //Spi.transfer((MAX7456_ENABLE_display_vert | video_mode) | MAX7456_SYNC_internal);