AP_Param: allow top level parameters to be pointers

this will allow for the attitude_control variable in quadplane to be a
pointer

libraries/AP_Param/AP_Param.cpp

@@ -221,6 +221,10 @@ bool AP_Param::check_var_info(void)
         if (duplicate_key(i, key)) {
             return false;
         }
+        if (type != AP_PARAM_GROUP && (_var_info[i].flags & AP_PARAM_FLAG_POINTER)) {
+            // only groups can be pointers
+            return false;
+        }
     }
 
     // we no longer check if total_size is larger than _eeprom_size,
@@ -270,17 +274,36 @@ bool AP_Param::adjust_group_offset(uint16_t vindex, const struct GroupInfo &grou
     }
     if (group_info.flags & AP_PARAM_FLAG_POINTER) {
         // group_info.offset refers to a pointer
-        void **p = (void **)(((ptrdiff_t)_var_info[vindex].ptr) + new_offset + group_info.offset);
+        ptrdiff_t base;
+        if (!get_base(_var_info[vindex], base)) {
+            // the object is not allocated yet
+            return false;
+        }
+        void **p = (void **)(base + new_offset + group_info.offset);
         if (*p == nullptr) {
             // the object is not allocated yet
             return false;
         }
         // calculate offset that is needed to take base object and adjust for this object
-        new_offset = ((ptrdiff_t)*p) - (ptrdiff_t)_var_info[vindex].ptr;
+        new_offset = ((ptrdiff_t)*p) - base;
     }
     return true;
 }
 
+/*
+  get the base pointer for a variable, accounting for AP_PARAM_FLAG_POINTER
+ */
+bool AP_Param::get_base(const struct Info &info, ptrdiff_t &base)
+{
+    if (info.flags & AP_PARAM_FLAG_POINTER) {
+        base = *(ptrdiff_t *)info.ptr;
+        return (base != (ptrdiff_t)0);
+    }
+    base = (ptrdiff_t)info.ptr;
+    return true;
+}
+
+
 // 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,
@@ -318,7 +341,11 @@ const struct AP_Param::Info *AP_Param::find_by_header_group(struct Param_header
         }
         if (group_id(group_info, group_base, i, group_shift) == phdr.group_element && type == phdr.type) {
             // found a group element
-            *ptr = (void*)((ptrdiff_t)_var_info[vindex].ptr + group_info[i].offset + group_offset);
+            ptrdiff_t base;
+            if (!get_base(_var_info[vindex], base)) {
+                continue;
+            }
+            *ptr = (void*)(base + group_info[i].offset + group_offset);
             return &_var_info[vindex];
         }
     }
@@ -343,7 +370,11 @@ const struct AP_Param::Info *AP_Param::find_by_header(struct Param_header phdr,
         }
         if (type == phdr.type) {
             // found it
-            *ptr = (void*)_var_info[i].ptr;
+            ptrdiff_t base;
+            if (!get_base(_var_info[i], base)) {
+                return NULL;
+            }
+            *ptr = (void*)base;
             return &_var_info[i];
         }
     }
@@ -361,7 +392,10 @@ const struct AP_Param::Info *AP_Param::find_var_info_group(const struct GroupInf
                                                            struct GroupNesting      &group_nesting,
                                                            uint8_t *                idx) const
 {
-    ptrdiff_t base = (ptrdiff_t)_var_info[vindex].ptr;
+    ptrdiff_t base;
+    if (!get_base(_var_info[vindex], base)) {
+        return NULL;
+    }
     uint8_t type;
     for (uint8_t i=0;
          (type=group_info[i].type) != AP_PARAM_NONE;
@@ -425,7 +459,10 @@ const struct AP_Param::Info *AP_Param::find_var_info(uint32_t *
     
     for (uint16_t i=0; i<_num_vars; i++) {
         uint8_t type = _var_info[i].type;
-        ptrdiff_t base = (ptrdiff_t)_var_info[i].ptr;
+        ptrdiff_t base;
+        if (!get_base(_var_info[i], base)) {
+            continue;
+        }
         if (type == AP_PARAM_GROUP) {
             const struct GroupInfo *group_info = _var_info[i].group_info;
             const struct AP_Param::Info *info;
@@ -460,7 +497,10 @@ const struct AP_Param::Info *AP_Param::find_var_info_token(const ParamToken &tok
 {
     uint16_t i = token.key;
     uint8_t type = _var_info[i].type;
-    ptrdiff_t base = (ptrdiff_t)_var_info[i].ptr;
+    ptrdiff_t base;
+    if (!get_base(_var_info[i], base)) {
+        return NULL;
+    }
     group_ret = NULL;
     
     if (type == AP_PARAM_GROUP) {
@@ -658,9 +698,12 @@ AP_Param::find_group(const char *name, uint16_t vindex, ptrdiff_t group_offset,
                 return ap;
             }
         } else if (strcasecmp(name, group_info[i].name) == 0) {
-            ptrdiff_t p = (ptrdiff_t)_var_info[vindex].ptr;
+            ptrdiff_t base;
+            if (!get_base(_var_info[vindex], base)) {
+                continue;
+            }
             *ptype = (enum ap_var_type)type;
-            return (AP_Param *)(p + group_info[i].offset + group_offset);
+            return (AP_Param *)(base + group_info[i].offset + group_offset);
         } else if (type == AP_PARAM_VECTOR3F) {
             // special case for finding Vector3f elements
             uint8_t suffix_len = strnlen(group_info[i].name, AP_MAX_NAME_SIZE);
@@ -669,8 +712,11 @@ AP_Param::find_group(const char *name, uint16_t vindex, ptrdiff_t group_offset,
                 (name[suffix_len+1] == 'X' ||
                  name[suffix_len+1] == 'Y' ||
                  name[suffix_len+1] == 'Z')) {
-                ptrdiff_t p = (ptrdiff_t)_var_info[vindex].ptr;
-                AP_Float *v = (AP_Float *)(p + group_info[i].offset + group_offset);
+                ptrdiff_t base;
+                if (!get_base(_var_info[vindex], base)) {
+                    continue;
+                }
+                AP_Float *v = (AP_Float *)(base + group_info[i].offset + group_offset);
                 *ptype = AP_PARAM_FLOAT;
                 switch (name[suffix_len+1]) {
                 case 'X':
@@ -709,7 +755,11 @@ AP_Param::find(const char *name, enum ap_var_type *ptype)
             // parameters, for example CAM_P_G
         } else if (strcasecmp(name, _var_info[i].name) == 0) {
             *ptype = (enum ap_var_type)type;
-            return (AP_Param *)_var_info[i].ptr;
+            ptrdiff_t base;
+            if (!get_base(_var_info[i], base)) {
+                return NULL;
+            }
+            return (AP_Param *)base;
         }
     }
     return NULL;
@@ -757,17 +807,61 @@ AP_Param::find_by_index(uint16_t idx, enum ap_var_type *ptype, ParamToken *token
 
 
 /*
-  Find a variable by pointer, returning a ParamToken
+  Find a variable by pointer, returning key. This is used for loading pointer variables
 */
-bool AP_Param::find_by_pointer(AP_Param *p, ParamToken &token)
+bool AP_Param::find_key_by_pointer_group(const void *ptr, uint16_t vindex,
+                                         const struct GroupInfo *group_info,
+                                         ptrdiff_t offset, uint16_t &key)
 {
-    AP_Param *ap;
-    enum ap_var_type ptype;
-    for (ap=AP_Param::first(&token, &ptype);
-         ap && ap != p;
-         ap=AP_Param::next(&token, &ptype)) {
+    for (uint8_t i=0; group_info[i].type != AP_PARAM_NONE; i++) {
+        if (group_info[i].type != AP_PARAM_GROUP) {
+            continue;
+        }
+        ptrdiff_t base;
+        if (!get_base(_var_info[vindex], base)) {
+            continue;
+        }
+        if (group_info[i].flags & AP_PARAM_FLAG_POINTER) {
+            if (ptr == *(void **)(base+group_info[i].offset+offset)) {
+                key = _var_info[vindex].key;
+                return true;
+            }
+        } else if (ptr == (void *)(base+group_info[i].offset+offset)) {
+            key = _var_info[vindex].key;
+            return true;
+        }
+        ptrdiff_t new_offset = offset;
+        if (!adjust_group_offset(vindex, group_info[i], new_offset)) {
+            continue;
+        }
+        if (find_key_by_pointer_group(ptr, vindex, group_info[i].group_info, new_offset, key)) {
+            return true;
+        }
+    }
+    return false;
+}
+
+
+/*
+  Find a variable by pointer, returning key. This is used for loading pointer variables
+*/
+bool AP_Param::find_key_by_pointer(const void *ptr, uint16_t &key)
+{
+    for (uint16_t i=0; i<_num_vars; i++) {
+        if (_var_info[i].type != AP_PARAM_GROUP) {
+            continue;
+        }
+        if ((_var_info[i].flags & AP_PARAM_FLAG_POINTER) &&
+            ptr == *(void **)_var_info[i].ptr) {
+            key = _var_info[i].key;
+            return true;
+        }
+        ptrdiff_t offset = 0;
+        if (find_key_by_pointer_group(ptr, i, _var_info[i].group_info, offset, key)) {
+            return true;
+        }
     }
-    return ap == p;
+    return false;
 }
 
 
@@ -778,7 +872,11 @@ AP_Param::find_object(const char *name)
 {
     for (uint16_t i=0; i<_num_vars; i++) {
         if (strcasecmp(name, _var_info[i].name) == 0) {
-            return (AP_Param *)_var_info[i].ptr;
+            ptrdiff_t base;
+            if (!get_base(_var_info[i], base)) {
+                return NULL;
+            }
+            return (AP_Param *)base;
         }
     }
     return NULL;
@@ -933,9 +1031,12 @@ bool AP_Param::load(void)
     uint16_t ofs;
     if (!scan(&phdr, &ofs)) {
         // if the value isn't stored in EEPROM then set the default value
-        if (ginfo != NULL) {
-            ptrdiff_t base = (ptrdiff_t)info->ptr;
+        ptrdiff_t base;
+        if (!get_base(*info, base)) {
+            return false;
+        }
 
+        if (ginfo != NULL) {
             // add in nested group offset
             ptrdiff_t group_offset = 0;
             for (uint8_t i=0; i<group_nesting.level; i++) {
@@ -944,7 +1045,7 @@ bool AP_Param::load(void)
             set_value((enum ap_var_type)phdr.type, (void*)(base + ginfo->offset + group_offset),
                       get_default_value(&ginfo->def_value));
         } else {
-            set_value((enum ap_var_type)phdr.type, (void*)info->ptr, 
+            set_value((enum ap_var_type)phdr.type, (void*)base, 
                       get_default_value(&info->def_value));
         }
         return false;
@@ -1090,8 +1191,10 @@ void AP_Param::setup_sketch_defaults(void)
     for (uint16_t i=0; i<_num_vars; i++) {
         uint8_t type = _var_info[i].type;
         if (type <= AP_PARAM_FLOAT) {
-            void *ptr = (void*)_var_info[i].ptr;
-            set_value((enum ap_var_type)type, ptr, get_default_value(&_var_info[i].def_value));
+            ptrdiff_t base;
+            if (get_base(_var_info[i], base)) {
+                set_value((enum ap_var_type)type, (void*)base, get_default_value(&_var_info[i].def_value));
+            }
         }
     }
 }
@@ -1146,27 +1249,17 @@ bool AP_Param::load_all(void)
 void AP_Param::load_object_from_eeprom(const void *object_pointer, const struct GroupInfo *group_info)
 {
     struct Param_header phdr;
-    ParamToken token;
-
-    // find first storable value
-    uint8_t i;
-    for (i=0; group_info[i].type != AP_PARAM_NONE; i++) {
-        if (group_info[i].type < AP_PARAM_GROUP) break;
-    }
-    if (group_info[i].type == AP_PARAM_NONE) {
-        // no storable parameters
-        return;
-    }
+    uint16_t key;
 
-    if (!find_by_pointer((AP_Param *)(((ptrdiff_t)object_pointer)+group_info[i].offset), token)) {
+    if (!find_key_by_pointer(object_pointer, key)) {
         hal.console->printf("ERROR: Unable to find param pointer\n");
         return;
     }
     
-    for (i=0; group_info[i].type != AP_PARAM_NONE; i++) {
+    for (uint8_t i=0; group_info[i].type != AP_PARAM_NONE; i++) {
         if (group_info[i].type == AP_PARAM_GROUP) {
             ptrdiff_t new_offset = 0;
-            if (!adjust_group_offset(token.key, group_info[i], new_offset)) {
+            if (!adjust_group_offset(key, group_info[i], new_offset)) {
                 continue;
             }
             load_object_from_eeprom((void *)(((ptrdiff_t)object_pointer)+new_offset), group_info[i].group_info);
@@ -1181,7 +1274,7 @@ void AP_Param::load_object_from_eeprom(const void *object_pointer, const struct
                 // we've reached the sentinal
                 break;
             }
-            if (get_key(phdr) == _var_info[token.key].key) {
+            if (get_key(phdr) == key) {
                 const struct AP_Param::Info *info;
                 void *ptr;
                 
@@ -1211,7 +1304,12 @@ AP_Param *AP_Param::first(ParamToken *token, enum ap_var_type *ptype)
     if (ptype != NULL) {
         *ptype = (enum ap_var_type)_var_info[0].type;
     }
-    return (AP_Param *)(_var_info[0].ptr);
+    ptrdiff_t base;
+    if (!get_base(_var_info[0], base)) {
+        // should be impossible, first var needs to be non-pointer
+        return NULL;
+    }
+    return (AP_Param *)base;
 }
 
 /// Returns the next variable in a group, recursing into groups
@@ -1252,7 +1350,11 @@ AP_Param *AP_Param::next_group(uint16_t vindex, const struct GroupInfo *group_in
                 if (ptype != NULL) {
                     *ptype = type;
                 }
-                return (AP_Param*)((ptrdiff_t)_var_info[vindex].ptr + group_info[i].offset + group_offset);
+                ptrdiff_t base;
+                if (!get_base(_var_info[vindex], base)) {
+                    continue;
+                }
+                return (AP_Param*)(base + group_info[i].offset + group_offset);
             }
             if (group_id(group_info, group_base, i, group_shift) == token->group_element) {
                 *found_current = true;
@@ -1263,7 +1365,11 @@ AP_Param *AP_Param::next_group(uint16_t vindex, const struct GroupInfo *group_in
                     if (ptype != NULL) {
                         *ptype = AP_PARAM_FLOAT;
                     }
-                    ptrdiff_t ofs = (ptrdiff_t)_var_info[vindex].ptr + group_info[i].offset + group_offset;
+                    ptrdiff_t base;
+                    if (!get_base(_var_info[vindex], base)) {
+                        continue;
+                    }
+                    ptrdiff_t ofs = base + group_info[i].offset + group_offset;
                     ofs += sizeof(float)*(token->idx - 1u);
                     return (AP_Param *)ofs;
                 }

libraries/AP_Param/AP_Param.h

@@ -118,6 +118,7 @@ public:
             const struct GroupInfo *group_info;
             const float def_value;
         };
+        uint8_t flags;
     };
     struct ConversionInfo {
         uint16_t old_key; // k_param_*
@@ -216,8 +217,10 @@ public:
     ///
     ///
     /// @param  p               Pointer to variable
-    /// @return                 token for variable
-    static bool find_by_pointer(AP_Param *p, ParamToken &token);
+    /// @return                 key for variable
+    static bool find_key_by_pointer_group(const void *ptr, uint16_t vindex, const struct GroupInfo *group_info,
+                                          ptrdiff_t offset, uint16_t &key);
+    static bool find_key_by_pointer(const void *ptr, uint16_t &key);
     
     /// Find a object in the top level var_info table
     ///
@@ -381,6 +384,7 @@ private:
     static bool                 duplicate_key(uint16_t vindex, uint16_t key);
 
     static bool adjust_group_offset(uint16_t vindex, const struct GroupInfo &group_info, ptrdiff_t &new_offset);
+    static bool get_base(const struct Info &info, ptrdiff_t &base);
 
     const struct Info *         find_var_info_group(
                                     const struct GroupInfo *    group_info,