diff --git a/libraries/AP_Common/AP_ExpandingArray.cpp b/libraries/AP_Common/AP_ExpandingArray.cpp
new file mode 100644
index 0000000000..661ca56390
--- /dev/null
+++ b/libraries/AP_Common/AP_ExpandingArray.cpp
@@ -0,0 +1,66 @@
+/*
+ This program 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 program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+
+#include "AP_ExpandingArray.h"
+
+AP_ExpandingArrayGeneric::~AP_ExpandingArrayGeneric(void)
+{
+ // free chunks
+ for (uint16_t i=0; i= chunk_count_max) {
+ uint16_t chunk_ptr_size = chunk_count + num_chunks + chunk_ptr_increment;
+ chunk_ptr_t *chunk_ptrs_new = (chunk_ptr_t*)realloc(chunk_ptrs, chunk_ptr_size * sizeof(chunk_ptr_t));
+ if (chunk_ptrs_new == nullptr) {
+ return false;
+ }
+
+ // use new pointers array
+ chunk_ptrs = chunk_ptrs_new;
+ chunk_count_max = chunk_ptr_size;
+ }
+
+ // allocate new chunks
+ for (uint16_t i = 0; i < num_chunks; i++) {
+ uint8_t *new_chunk = (uint8_t *)calloc(chunk_size, elem_size);
+ if (new_chunk == nullptr) {
+ // failed to allocate new chunk
+ return false;
+ }
+ chunk_ptrs[chunk_count] = new_chunk;
+ chunk_count++;
+ }
+ return true;
+}
+
+// expand to hold at least num_items
+bool AP_ExpandingArrayGeneric::expand_to_hold(uint16_t num_items)
+{
+ // check if already big enough
+ if (num_items <= max_items()) {
+ return true;
+ }
+ uint16_t chunks_required = ((num_items - max_items()) / chunk_size) + 1;
+ return expand(chunks_required);
+}
diff --git a/libraries/AP_Common/AP_ExpandingArray.h b/libraries/AP_Common/AP_ExpandingArray.h
index b7296ba7ec..f7c343fd3d 100644
--- a/libraries/AP_Common/AP_ExpandingArray.h
+++ b/libraries/AP_Common/AP_ExpandingArray.h
@@ -16,6 +16,9 @@
/*
* ExpandingArray class description
*
+ * ExpandingArrayGeneric implements most of the required functionality and is type agnostic allowing smaller overall code size
+ * ExpandingArray is the template and implements the small number of type specific methods
+ *
* Elements are organised into "chunks" with each chunk holding "chunk_size" elements
* The "chunk_ptrs" array holds pointers to all allocated chunks
*
@@ -38,96 +41,69 @@
#include
-template
-class AP_ExpandingArray
+class AP_ExpandingArrayGeneric
{
public:
- AP_ExpandingArray(uint16_t elements_per_chunk) :
+ AP_ExpandingArrayGeneric(uint16_t element_size, uint16_t elements_per_chunk) :
+ elem_size(element_size),
chunk_size(elements_per_chunk)
{}
- ~AP_ExpandingArray(void)
- {
- // free chunks
- for (uint16_t i=0; i(const AP_ExpandingArray &other) = delete;
- AP_ExpandingArray &operator=(const AP_ExpandingArray&) = delete;
+ AP_ExpandingArrayGeneric(const AP_ExpandingArrayGeneric &other) = delete;
+ AP_ExpandingArrayGeneric &operator=(const AP_ExpandingArrayGeneric&) = delete;
// current maximum number of items (using expand may increase this)
uint16_t max_items() const { return chunk_size * chunk_count; }
- // allow use as an array for assigning to elements. no bounds checking is performed
- T &operator[](uint16_t i)
- {
- const uint16_t chunk_num = i / chunk_size;
- const uint16_t chunk_index = i % chunk_size;
- return chunk_ptrs[chunk_num][chunk_index];
- }
-
- // allow use as an array for accessing elements. no bounds checking is performed
- const T &operator[](uint16_t i) const
- {
- const uint16_t chunk_num = i / chunk_size;
- const uint16_t chunk_index = i % chunk_size;
- return chunk_ptrs[chunk_num][chunk_index];
- }
-
// expand the array by specified number of chunks, returns true on success
- bool expand(uint16_t num_chunks = 1)
- {
- // expand chunk_ptrs array if necessary
- if (chunk_count + num_chunks >= chunk_count_max) {
- uint16_t chunk_ptr_size = chunk_count + num_chunks + chunk_ptr_increment;
- chunk_ptr_t *chunk_ptrs_new = (chunk_ptr_t*)realloc(chunk_ptrs, chunk_ptr_size * sizeof(T*));
- if (chunk_ptrs_new == nullptr) {
- return false;
- }
-
- // use new pointers array
- chunk_ptrs = chunk_ptrs_new;
- chunk_count_max = chunk_ptr_size;
- }
-
- // allocate new chunks
- for (uint16_t i = 0; i < num_chunks; i++) {
- T *new_chunk = (T *)calloc(chunk_size, sizeof(T));
- if (new_chunk == nullptr) {
- // failed to allocate new chunk
- return false;
- }
- chunk_ptrs[chunk_count] = new_chunk;
- chunk_count++;
- }
- return true;
- }
+ bool expand(uint16_t num_chunks = 1);
// expand to hold at least num_items
- bool expand_to_hold(uint16_t num_items)
- {
- // check if already big enough
- if (num_items <= max_items()) {
- return true;
- }
- uint16_t chunks_required = ((num_items - max_items()) / chunk_size) + 1;
- return expand(chunks_required);
- }
+ bool expand_to_hold(uint16_t num_items);
-private:
+protected:
+ const uint16_t elem_size; // number of bytes for each element
const uint16_t chunk_size; // the number of T elements in each chunk
const uint16_t chunk_ptr_increment = 32; // chunk_ptrs array is grown by this many elements each time it fills
- typedef T* chunk_ptr_t; // pointer to a chunk
+ typedef uint8_t* chunk_ptr_t; // pointer to a chunk
chunk_ptr_t *chunk_ptrs; // array of pointers to allocated chunks
uint16_t chunk_count_max; // number of elements in chunk_ptrs array
uint16_t chunk_count; // number of allocated chunks
};
+
+template
+class AP_ExpandingArray : public AP_ExpandingArrayGeneric
+{
+public:
+
+ AP_ExpandingArray(uint16_t elements_per_chunk) :
+ AP_ExpandingArrayGeneric(sizeof(T), elements_per_chunk)
+ {}
+
+ /* Do not allow copies */
+ AP_ExpandingArray(const AP_ExpandingArray &other) = delete;
+ AP_ExpandingArray &operator=(const AP_ExpandingArray&) = delete;
+
+ // allow use as an array for assigning to elements. no bounds checking is performed
+ T &operator[](uint16_t i)
+ {
+ const uint16_t chunk_num = i / chunk_size;
+ const uint16_t chunk_index = (i % chunk_size) * elem_size;
+ return (T &)(chunk_ptrs[chunk_num][chunk_index]);
+ }
+
+ // allow use as an array for accessing elements. no bounds checking is performed
+ const T &operator[](uint16_t i) const
+ {
+ const uint16_t chunk_num = i / chunk_size;
+ const uint16_t chunk_index = (i % chunk_size) * elem_size;
+ return (const T &)(chunk_ptrs[chunk_num][chunk_index]);
+ }
+};