AP_Param: major update to use default values in var_info table

this stores the default value for all scalar variables in the var_info
table, which makes it possible to avoid storing default values in
eeprom. That allows us to oversubscribe the eeprom space with a much
lower risk of overrun.

libraries/AP_Common/AP_Param.cpp

@@ -31,6 +31,7 @@
 // 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_FLOAT(addr) pgm_read_float((const float *)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
@@ -197,29 +198,32 @@ bool AP_Param::check_var_info(void)
             return false;
         }
     }
-    if (total_size > _eeprom_size) {
-        serialDebug("total_size %u exceeds _eeprom_size %u",
-                    total_size, _eeprom_size);
-        return false;
-    }
+
+    // we no longer check if total_size is larger than _eeprom_size,
+    // as we allow for more variables than could fit, relying on not
+    // saving default values
+
     return true;
 }
 
 
 // setup the _var_info[] table
-bool AP_Param::setup(const AP_Param::Info *info, uint8_t num_vars, uint16_t eeprom_size)
+bool AP_Param::setup(const struct AP_Param::Info *info, uint16_t eeprom_size)
 {
     struct EEPROM_header hdr;
+    uint8_t i;
 
     _eeprom_size = eeprom_size;
     _var_info = info;
-    _num_vars = num_vars;
+
+    for (i=0; PGM_UINT8(&info[i].type) != AP_PARAM_NONE; i++) ;
+    _num_vars = i;
 
     if (!check_var_info()) {
         return false;
     }
 
-    serialDebug("setup %u vars", (unsigned)num_vars);
+    serialDebug("setup %u vars", (unsigned)_num_vars);
 
     // check the header
     eeprom_read_block(&hdr, 0, sizeof(hdr));
@@ -621,6 +625,17 @@ bool AP_Param::save(void)
         return false;
     }
 
+    // if the value is the default value then don't save
+    if (phdr.type <= AP_PARAM_FLOAT && 
+        cast_to_float((enum ap_var_type)phdr.type) == PGM_FLOAT(&info->def_value)) {
+        return true;
+    }
+
+    if (ofs+type_size((enum ap_var_type)phdr.type)+2*sizeof(phdr) >= _eeprom_size) {
+        // we are out of room for saving variables
+        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));
@@ -655,6 +670,14 @@ bool AP_Param::load(void)
     // scan EEPROM to find the right location
     uint16_t ofs;
     if (!scan(&phdr, &ofs)) {
+        // if the value isn't stored in EEPROM then set the default value
+        if (ginfo != NULL) {
+            uintptr_t base = PGM_POINTER(&info->ptr);
+            set_value((enum ap_var_type)phdr.type, (void*)(base + PGM_UINT16(&ginfo->offset)), 
+                      PGM_FLOAT(&ginfo->def_value));
+        } else {
+            set_value((enum ap_var_type)phdr.type, (void*)PGM_POINTER(&info->ptr), PGM_FLOAT(&info->def_value));
+        }
         return false;
     }
 
@@ -674,12 +697,69 @@ bool AP_Param::load(void)
     return true;
 }
 
+// set a AP_Param variable to a specified value 
+void AP_Param::set_value(enum ap_var_type type, void *ptr, float def_value)
+{
+    switch (type) {
+    case AP_PARAM_INT8:
+        ((AP_Int8 *)ptr)->set(def_value);
+        break;
+    case AP_PARAM_INT16:
+        ((AP_Int16 *)ptr)->set(def_value);
+        break;
+    case AP_PARAM_INT32:
+        ((AP_Int32 *)ptr)->set(def_value);
+        break;
+    case AP_PARAM_FLOAT:
+        ((AP_Float *)ptr)->set(def_value);
+        break;
+    default:
+        break;
+    }
+}
+
+// load default values for scalars in a group
+void AP_Param::load_defaults_group(const struct GroupInfo *group_info, uintptr_t base)
+{
+    uint8_t type;
+    for (uint8_t i=0;
+         (type=PGM_UINT8(&group_info[i].type)) != AP_PARAM_NONE;
+         i++) {
+        if (type == AP_PARAM_GROUP) {
+            const struct GroupInfo *ginfo = (const struct GroupInfo *)PGM_POINTER(&group_info[i].group_info);
+            load_defaults_group(ginfo, base);
+        } else if (type <= AP_PARAM_FLOAT) {
+            void *ptr = (void *)(base + PGM_UINT16(&group_info[i].offset));
+            set_value((enum ap_var_type)type, ptr, PGM_FLOAT(&group_info[i].def_value));    
+        }
+    }
+}
+
+
+// load default values for all scalars
+void AP_Param::load_defaults(void)
+{
+    for (uint8_t i=0; i<_num_vars; i++) {
+        uint8_t 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);
+            uintptr_t base = PGM_POINTER(&_var_info[i].ptr);
+            load_defaults_group(group_info, base);
+        } else if (type <= AP_PARAM_FLOAT) {
+            void *ptr = (void*)PGM_POINTER(&_var_info[i].ptr);
+            set_value((enum ap_var_type)type, ptr, PGM_FLOAT(&_var_info[i].def_value));
+        }
+    }
+}
+
+
 // 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

libraries/AP_Common/AP_Param.h

@@ -31,14 +31,15 @@
 #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) }
+#define AP_GROUPINFO(name, idx, class, element, def) { AP_CLASSTYPE(class, element), idx, name, AP_VAROFFSET(class, element), {def_value:def} }
 
 // 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 }
+#define AP_NESTEDGROUPINFO(class, idx) { AP_PARAM_GROUP, idx, "", 0, { group_info: class::var_info } }
 #endif
 
-#define AP_GROUPEND	{ AP_PARAM_NONE, 0xFF, "" }
+#define AP_GROUPEND	{ AP_PARAM_NONE, 0xFF, "", 0, { group_info : NULL } }
+#define AP_VAREND	{ AP_PARAM_NONE, "", 0, NULL, { group_info : NULL } }
 
 enum ap_var_type {
     AP_PARAM_NONE    = 0,
@@ -67,27 +68,42 @@ public:
         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;
+        union {
+            const struct GroupInfo *group_info;
+            const float def_value;
+        };
     };
     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;
+        union {
+            const struct GroupInfo *group_info;
+            const float def_value;
+        };
     };
 
-    // 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);
+    static bool setup(const struct Info *info, uint16_t eeprom_size);
+
+    // constructor to load default values and setup var_info table
+    AP_Param(const struct Info *info, uint16_t eeprom_size) {
+        setup(info, eeprom_size);
+        load_defaults();
+    }
+
+    // empty constructor for child classes
+    AP_Param() {}
+
+    // a token used for first()/next() state
+    typedef struct {
+        uint32_t key:8;
+        uint32_t idx:6; // offset into array types
+        uint32_t group_element:18;
+    } ParamToken;
 
     // return true if AP_Param has been initialised via setup()
     static bool initialised(void);
@@ -136,6 +152,15 @@ public:
     ///
     static bool load_all(void);
 
+    // set a AP_Param variable to a specified value 
+    static void set_value(enum ap_var_type type, void *ptr, float def_value);
+
+    // load default values for scalars in a group
+    static void load_defaults_group(const struct GroupInfo *group_info, uintptr_t base);
+
+    // load default values for all scalars
+    static void load_defaults(void);
+
     /// Erase all variables in EEPROM.
     ///
     static void erase_all(void);
@@ -179,24 +204,21 @@ private:
    - key: the k_param enum value from Parameter.h in the sketch
 
    - group_element: This is zero for top level parameters. For
-                    parameters stored within an object the top 4 bits
-                    are the idx field of the GroupInfo structue (the index into
-                    the first level of object indirection). The second
-                    4 bits are the idx field from the second level of
-                    object indirection. This allows for two levels of
-                    object to be stored in the eeprom
+                    parameters stored within an object this is divided
+                    into 3 lots of 6 bits, allowing for three levels
+                    of object to be stored in the eeprom
 
    - type: the ap_var_type value for the variable
 */
     struct Param_header {
-        uint8_t    key;
-        uint8_t    group_element;
-        uint8_t    type;
+        uint32_t   key:8;
+        uint32_t   type:6;
+        uint32_t   group_element:18;
     };
 
     // 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 _group_level_shift = 6;
+    static const uint8_t _group_bits  = 18;
 
     static const uint8_t  _sentinal_key   = 0xFF;
     static const uint8_t  _sentinal_type  = 0xFF;
@@ -241,7 +263,7 @@ private:
     // 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
+    static const uint8_t   k_EEPROM_revision    = 6;      ///< current format revision
 };
 
 /// Template class for scalar variables.
@@ -256,18 +278,6 @@ template<typename T, ap_var_type PT>
 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<T,PT> (const T initial_value = 0) :
-        AP_Param(),
-        _value(initial_value)
-    {
-    }
-
     static const ap_var_type vtype = PT;
 
     /// Value getter
@@ -346,6 +356,7 @@ template<typename T, ap_var_type PT>
 class AP_ParamV : public AP_Param
 {
 public:
+
     static const ap_var_type vtype = PT;
 
     /// Value getter
@@ -406,6 +417,7 @@ template<typename T, uint8_t N, ap_var_type PT>
 class AP_ParamA : public AP_Param
 {
 public:
+
     static const ap_var_type vtype = PT;
 
     /// Array operator accesses members.

libraries/AP_Common/tools/eedump_apparam.c

@@ -18,7 +18,7 @@ struct EEPROM_header {
 
 static const uint16_t   k_EEPROM_magic0     = 0x50;
 static const uint16_t   k_EEPROM_magic1     = 0x41;
-static const uint16_t   k_EEPROM_revision   = 5;
+static const uint16_t   k_EEPROM_revision   = 6;
 
 enum ap_var_type {
     AP_PARAM_NONE    = 0,
@@ -37,9 +37,9 @@ static const char *type_names[8] = {
 };
 
 struct Param_header {
-        uint8_t    key;
-        uint8_t    group_element;
-        uint8_t    type;
+        uint32_t   key:8;
+        uint32_t   type:6;
+        uint32_t   group_element:18;
 };
 
 

libraries/AP_Common/tools/eedump_apparam.pl

@@ -13,7 +13,7 @@ if (ord($buffer2) != 0x41 && ord($buffer) != 0x50) {
 	exit;
 }
 read(IN,$buffer,1);
-if (ord($buffer) != 5) {
+if (ord($buffer) != 6) {
 	print "bad version";
 	exit;
 }
@@ -26,47 +26,53 @@ $a = 0;
 while (read(IN,$buffer,1)) {	
 	$pos = (tell(IN) - 1);
 
-	if (ord($buffer) == 0xff) {
+	my $key = ord($buffer);
+
+	if ($key == 0xff) {
 		printf("end sentinel at %u\n", $pos);
 		last;
 	}
     
-    read(IN,$buffer2,1);
-    read(IN,$buffer3,1);
+	read(IN,$buffer2,1);
+	read(IN,$buffer3,1);
+	read(IN,$buffer4,1);
+
+	my $itype = ord($buffer2)&0x3F;
+	my $group_element = (ord($buffer2)>>6) | (ord($buffer3)<<8) | (ord($buffer4)<<16);
     
-    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;
-    }
+	if ($itype  == 0) { #none
+		$size = 0;
+		$type = "NONE";
+	} elsif ($itype == 1) { #int8
+		$size = 1;
+		$type = "INT8";
+	} elsif ($itype  == 2) { #int16
+		$size = 2;
+		$type = "INT16";
+	} elsif ($itype == 3) { #int32
+		$size = 4;
+		$type = "INT32";
+	} elsif ($itype == 4) { #float
+		$size = 4;
+		$type = "FLOAT";
+	} elsif ($itype == 5) { #vector 3
+		$size = 3*4;
+		$type = "VECTOR3F";
+	} elsif ($itype == 6) { #vector6
+		$size = 6*4;
+		$type = "VECTOR6F";
+	} elsif ($itype == 7) { #matrix
+		$size = 3*3*4;
+		$type = "MATRIX6F";
+	} elsif ($itype == 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);
+	printf("%04x: type %u ($type) key %u group_element %u size %d\n ", $pos, $itype, $key, $group_element, $size);
 
 	for ($i = 0; $i < ($size); $i++) {
 		read(IN,$buffer,1);