Example #1
0
bool AP_Param::configured_in_storage(void)
{
    uint32_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;

    // only vector3f can have non-zero idx for now
    return scan(&phdr, &ofs) && (phdr.type == AP_PARAM_VECTOR3F || idx == 0);
}
Example #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_token(const ParamToken *token, char *buffer, size_t buffer_size, bool force_scalar)
{
    uint32_t group_element;
    const struct GroupInfo *ginfo;
    uint8_t idx;
    const struct AP_Param::Info *info = find_var_info_token(token, &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);
    }
}
Example #3
0
// convert one old vehicle parameter to new object parameter
void AP_Param::convert_old_parameter(const struct ConversionInfo *info)
{

    // find the old value in EEPROM.
    uint16_t pofs;
    AP_Param::Param_header header;
    header.type = PGM_UINT8(&info->type);
    header.key = PGM_UINT8(&info->old_key);
    header.group_element = PGM_UINT8(&info->old_group_element);
    if (!scan(&header, &pofs)) {
        // the old parameter isn't saved in the EEPROM. It was
        // probably still set to the default value, which isn't stored
        // no need to convert
        return;
    }

    // load the old value from EEPROM
    uint8_t old_value[type_size((enum ap_var_type)header.type)];
    _storage.read_block(old_value, pofs+sizeof(header), sizeof(old_value));
    const AP_Param *ap = (const AP_Param *)&old_value[0];

    // find the new variable in the variable structures
    enum ap_var_type ptype;
    AP_Param *ap2;
    ap2 = find_P((const prog_char_t *)&info->new_name[0], &ptype);
    if (ap2 == NULL) {
        hal.console->printf_P(PSTR("Unknown conversion '%s'\n"), info->new_name);
        return;
    }

    // see if we can load it from EEPROM
    if (ap2->load()) {
        // the new parameter already has a value set by the user, or
        // has already been converted
        return;
    }

    // see if they are the same type
    if (ptype == (ap_var_type)header.type) {
        // copy the value over only if the new parameter does not already
        // have the old value (via a default).
        if (memcmp(ap2, ap, sizeof(old_value)) != 0) {
            memcpy(ap2, ap, sizeof(old_value));
            // and save
            ap2->save();
        }
    } else if (ptype <= AP_PARAM_FLOAT && header.type <= AP_PARAM_FLOAT) {
        // perform scalar->scalar conversion
        float v = ap->cast_to_float((enum ap_var_type)header.type);
        if (!is_equal(v,ap2->cast_to_float(ptype))) {
            // the value needs to change
            set_value(ptype, ap2, v);
            ap2->save();
        }
    } else {
        // can't do vector<->scalar conversion, or different vector types
        hal.console->printf_P(PSTR("Bad conversion type '%s'\n"), info->new_name);
    }
}
Example #4
0
/*
  print FMT specifiers for log dumps where we have wrapped in the
  dataflash and so have no formats. This assumes the log being dumped
  using the same log formats as the current formats, but it is better
  than falling back to old defaults in the GCS
 */
void DataFlash_Block::_print_log_formats(AP_HAL::BetterStream *port)
{
    for (uint8_t i=0; i<_num_types; i++) {
        const struct LogStructure *s = &_structures[i];
        port->printf_P(PSTR("FMT, %u, %u, %S, %S, %S\n"),
                       (unsigned)PGM_UINT8(&s->msg_type),
                       (unsigned)PGM_UINT8(&s->msg_len),
                       s->name, s->format, s->labels);
    }
}
Example #5
0
/*
  write a structure format to the log
 */
void DataFlash_Class::Log_Fill_Format(const struct LogStructure *s, struct log_Format &pkt)
{
    memset(&pkt, 0, sizeof(pkt));
    pkt.head1 = HEAD_BYTE1;
    pkt.head2 = HEAD_BYTE2;
    pkt.msgid = LOG_FORMAT_MSG;
    pkt.type = PGM_UINT8(&s->msg_type);
    pkt.length = PGM_UINT8(&s->msg_len);
    strncpy_P(pkt.name, s->name, sizeof(pkt.name));
    strncpy_P(pkt.format, s->format, sizeof(pkt.format));
    strncpy_P(pkt.labels, s->labels, sizeof(pkt.labels));
}
Example #6
0
// Load the variable from EEPROM, if supported
//
bool AP_Param::load(void)
{
    uint32_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)) {
        // 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)),
                      get_default_value(&ginfo->def_value));
        } else {
            set_value((enum ap_var_type)phdr.type, (void*)PGM_POINTER(&info->ptr), 
                      get_default_value(&info->def_value));
        }
        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
    _storage.read_block(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));
    return true;
}
Example #7
0
// 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;
}
Example #8
0
// 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                             prefix_length)
{
    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) {
                Debug("double group nesting in %S", group_info[i].name);
                return false;
            }
            if (ginfo == NULL ||
                !check_group_info(ginfo, total_size, group_shift + _group_level_shift, prefix_length + strlen_P(group_info[i].name))) {
                return false;
            }
            continue;
        }
#endif // AP_NESTED_GROUPS_ENABLED
        uint8_t idx = PGM_UINT8(&group_info[i].idx);
        if (idx >= (1<<_group_level_shift)) {
            Debug("idx too large (%u) in %S", idx, group_info[i].name);
            return false;
        }
        if ((int8_t)idx <= max_idx) {
            Debug("indexes must be in increasing order in %S", group_info[i].name);
            return false;
        }
        max_idx = (int8_t)idx;
        uint8_t size = type_size((enum ap_var_type)type);
        if (size == 0) {
            Debug("invalid type in %S", group_info[i].name);
            return false;
        }
        if (prefix_length + strlen_P(group_info[i].name) > 16) {
            Debug("suffix is too long in %S", group_info[i].name);
            return false;
        }
        (*total_size) += size + sizeof(struct Param_header);
    }
    return true;
}
Example #9
0
// find the info structure for a variable
const struct AP_Param::Info *AP_Param::find_var_info_token(const ParamToken *token,
                                                           uint32_t *                 group_element,
                                                           const struct GroupInfo **  group_ret,
                                                           uint8_t *                  idx)
{
    uint8_t i = token->key;
    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;
}
Example #10
0
// 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;
}
Example #11
0
void AP_Param::copy_name_info(const struct AP_Param::Info *info, const struct GroupInfo *ginfo, uint8_t idx, char *buffer, size_t buffer_size, bool force_scalar) const
{
    strncpy(buffer, info->name, buffer_size);
    if (ginfo != NULL) {
        uint8_t len = strnlen(buffer, buffer_size);
        if (len < buffer_size) {
            strncpy(&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);
    }
}
Example #12
0
/// 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;
}
Example #13
0
// 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;
}
Example #14
0
// 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;
}
Example #15
0
// load default values for all scalars in a sketch. This does not
// recurse into sub-objects
void AP_Param::setup_sketch_defaults(void)
{
    setup();
    for (uint8_t i=0; i<_num_vars; i++) {
        uint8_t type = PGM_UINT8(&_var_info[i].type);//返回参数类型,强制转换为uint8,包括PARAM_NONE等
        if (type <= AP_PARAM_FLOAT) {//按道理讲uint8_t一定会小于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));//把参数变量设为一个特定的值
        }
    }
}
Example #16
0
// load default values for all scalars in a sketch. This does not
// recurse into sub-objects
void AP_Param::setup_sketch_defaults(void)
{
    setup();
    for (uint8_t i=0; i<_num_vars; i++) {
        uint8_t type = PGM_UINT8(&_var_info[i].type);
        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));
        }
    }
}
Example #17
0
// 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,
                                                           uint32_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;
}
Example #18
0
// 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;
}
Example #19
0
/// 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;
}
Example #20
0
// 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));
}
Example #21
0
// load default values for scalars in a group. This does not recurse
// into other objects. This is a static function that should be called
// in the objects constructor
void AP_Param::setup_object_defaults(const void *object_pointer, const struct GroupInfo *group_info)
{
    uintptr_t base = (uintptr_t)object_pointer;
    uint8_t type;
    for (uint8_t i=0;
         (type=PGM_UINT8(&group_info[i].type)) != AP_PARAM_NONE;
         i++) {
        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));
        }
    }
}
Example #22
0
// notify GCS of current value of parameter
void AP_Param::notify() const {
    uint32_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) {
        // this is probably very bad
        return;
    }

    char name[AP_MAX_NAME_SIZE+1];
    copy_name_info(info, ginfo, idx, name, sizeof(name), true);

    uint32_t param_header_type;
    if (ginfo != NULL) {
        param_header_type = PGM_UINT8(&ginfo->type);
    } else {
        param_header_type = PGM_UINT8(&info->type);
    }

    send_parameter(name, (enum ap_var_type)param_header_type);
}
Example #23
0
// 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;
}
Example #24
0
// 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;
        }
    }

    // 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;
}
Example #25
0
// set a value directly in an object. This should only be used by
// example code, not by mainline vehicle code
void AP_Param::set_object_value(const void *object_pointer, 
                                const struct GroupInfo *group_info, 
                                const char *name, float value)
{
    uintptr_t base = (uintptr_t)object_pointer;
    uint8_t type;
    for (uint8_t i=0;
         (type=PGM_UINT8(&group_info[i].type)) != AP_PARAM_NONE;
         i++) {
        if (strcmp(name, group_info[i].name) == 0 && type <= AP_PARAM_FLOAT) {
            void *ptr = (void *)(base + PGM_UINT16(&group_info[i].offset));
            set_value((enum ap_var_type)type, ptr, value);
        }
    }
}
Example #26
0
// 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 = strnlen_P(group_info[i].name, AP_MAX_NAME_SIZE);
            if (strncmp_P(name, group_info[i].name, suffix_len) == 0 &&
                name[suffix_len] == '_' &&
                (name[suffix_len+1] == 'X' ||
                 name[suffix_len+1] == 'Y' ||
                 name[suffix_len+1] == 'Z')) {
                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;
}
Example #27
0
// 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;
}
Example #28
0
/*
  read and print a log entry using the format strings from the given structure
 */
void DataFlash_Class::_print_log_entry(uint8_t msg_type,
                                       print_mode_fn print_mode,
                                       AP_HAL::BetterStream *port)
{
    uint8_t i;
    for (i=0; i<_num_types; i++) {
        if (msg_type == PGM_UINT8(&_structures[i].msg_type)) {
            break;
        }
    }
    if (i == _num_types) {
        port->printf_P(PSTR("UNKN, %u\n"), (unsigned)msg_type);
        return;
    }
    uint8_t msg_len = PGM_UINT8(&_structures[i].msg_len) - 3;
    uint8_t pkt[msg_len];
    ReadBlock(pkt, msg_len);
    port->printf_P(PSTR("%S, "), _structures[i].name);
    for (uint8_t ofs=0, fmt_ofs=0; ofs<msg_len; fmt_ofs++) {
        char fmt = PGM_UINT8(&_structures[i].format[fmt_ofs]);
        switch (fmt) {
        case 'b': {
            port->printf_P(PSTR("%d"), (int)pkt[ofs]);
            ofs += 1;
            break;
        }
        case 'B': {
            port->printf_P(PSTR("%u"), (unsigned)pkt[ofs]);
            ofs += 1;
            break;
        }
        case 'h': {
            int16_t v;
            memcpy(&v, &pkt[ofs], sizeof(v));
            port->printf_P(PSTR("%d"), (int)v);
            ofs += sizeof(v);
            break;
        }
        case 'H': {
            uint16_t v;
            memcpy(&v, &pkt[ofs], sizeof(v));
            port->printf_P(PSTR("%u"), (unsigned)v);
            ofs += sizeof(v);
            break;
        }
        case 'i': {
            int32_t v;
            memcpy(&v, &pkt[ofs], sizeof(v));
            port->printf_P(PSTR("%ld"), (long)v);
            ofs += sizeof(v);
            break;
        }
        case 'I': {
            uint32_t v;
            memcpy(&v, &pkt[ofs], sizeof(v));
            port->printf_P(PSTR("%lu"), (unsigned long)v);
            ofs += sizeof(v);
            break;
        }
        case 'f': {
            float v;
            memcpy(&v, &pkt[ofs], sizeof(v));
            port->printf_P(PSTR("%f"), (double)v);
            ofs += sizeof(v);
            break;
        }
        case 'c': {
            int16_t v;
            memcpy(&v, &pkt[ofs], sizeof(v));
            port->printf_P(PSTR("%.2f"), (double)(0.01f*v));
            ofs += sizeof(v);
            break;
        }
        case 'C': {
            uint16_t v;
            memcpy(&v, &pkt[ofs], sizeof(v));
            port->printf_P(PSTR("%.2f"), (double)(0.01f*v));
            ofs += sizeof(v);
            break;
        }
        case 'e': {
            int32_t v;
            memcpy(&v, &pkt[ofs], sizeof(v));
            port->printf_P(PSTR("%.2f"), (double)(0.01f*v));
            ofs += sizeof(v);
            break;
        }
        case 'E': {
            uint32_t v;
            memcpy(&v, &pkt[ofs], sizeof(v));
            port->printf_P(PSTR("%.2f"), (double)(0.01f*v));
            ofs += sizeof(v);
            break;
        }
        case 'L': {
            int32_t v;
            memcpy(&v, &pkt[ofs], sizeof(v));
            print_latlon(port, v);
            ofs += sizeof(v);
            break;
        }
        case 'n': {
            char v[5];
            memcpy(&v, &pkt[ofs], sizeof(v));
            v[sizeof(v)-1] = 0;
            port->printf_P(PSTR("%s"), v);
            ofs += sizeof(v)-1;
            break;
        }
        case 'N': {
            char v[17];
            memcpy(&v, &pkt[ofs], sizeof(v));
            v[sizeof(v)-1] = 0;
            port->printf_P(PSTR("%s"), v);
            ofs += sizeof(v)-1;
            break;
        }
        case 'Z': {
            char v[65];
            memcpy(&v, &pkt[ofs], sizeof(v));
            v[sizeof(v)-1] = 0;
            port->printf_P(PSTR("%s"), v);
            ofs += sizeof(v)-1;
            break;
        }
        case 'M': {
            print_mode(port, pkt[ofs]);
            ofs += 1;
            break;
        }
        default:
            ofs = msg_len;
            break;
        }
        if (ofs < msg_len) {
            port->printf_P(PSTR(", "));
        }
    }
    port->println();
}
Example #29
0
/*
  read and print a log entry using the format strings from the given structure
  - this really should in in the frontend, not the backend
 */
void DataFlash_Backend::_print_log_entry(uint8_t msg_type,
                                         print_mode_fn print_mode,
                                         AP_HAL::BetterStream *port)
{
    uint8_t i;
    for (i=0; i<_num_types; i++) {
        if (msg_type == PGM_UINT8(&_structures[i].msg_type)) {
            break;
        }
    }
    if (i == _num_types) {
        port->printf_P(PSTR("UNKN, %u\n"), (unsigned)msg_type);
        return;
    }
    uint8_t msg_len = PGM_UINT8(&_structures[i].msg_len) - 3;
    uint8_t pkt[msg_len];
    if (!ReadBlock(pkt, msg_len)) {
        return;
    }
    port->printf_P(PSTR("%S, "), _structures[i].name);
    for (uint8_t ofs=0, fmt_ofs=0; ofs<msg_len; fmt_ofs++) {
        char fmt = PGM_UINT8(&_structures[i].format[fmt_ofs]);
        switch (fmt) {
        case 'b': {
            port->printf_P(PSTR("%d"), (int)pkt[ofs]);
            ofs += 1;
            break;
        }
        case 'B': {
            port->printf_P(PSTR("%u"), (unsigned)pkt[ofs]);
            ofs += 1;
            break;
        }
        case 'h': {
            int16_t v;
            memcpy(&v, &pkt[ofs], sizeof(v));
            port->printf_P(PSTR("%d"), (int)v);
            ofs += sizeof(v);
            break;
        }
        case 'H': {
            uint16_t v;
            memcpy(&v, &pkt[ofs], sizeof(v));
            port->printf_P(PSTR("%u"), (unsigned)v);
            ofs += sizeof(v);
            break;
        }
        case 'i': {
            int32_t v;
            memcpy(&v, &pkt[ofs], sizeof(v));
            port->printf_P(PSTR("%ld"), (long)v);
            ofs += sizeof(v);
            break;
        }
        case 'I': {
            uint32_t v;
            memcpy(&v, &pkt[ofs], sizeof(v));
            port->printf_P(PSTR("%lu"), (unsigned long)v);
            ofs += sizeof(v);
            break;
        }
        case 'q': {
            int64_t v;
            memcpy(&v, &pkt[ofs], sizeof(v));
            port->printf_P(PSTR("%lld"), (long long)v);
            ofs += sizeof(v);
            break;
        }
        case 'Q': {
            uint64_t v;
            memcpy(&v, &pkt[ofs], sizeof(v));
            port->printf_P(PSTR("%llu"), (unsigned long long)v);
            ofs += sizeof(v);
            break;
        }
        case 'f': {
            float v;
            memcpy(&v, &pkt[ofs], sizeof(v));
            port->printf_P(PSTR("%f"), (double)v);
            ofs += sizeof(v);
            break;
        }
        case 'd': {
            double v;
            memcpy(&v, &pkt[ofs], sizeof(v));
            // note that %f here *really* means a single-precision
            // float, so we lose precision printing this double out
            // dtoa_engine needed....
            port->printf_P(PSTR("%f"), (double)v);
            ofs += sizeof(v);
            break;
        }
        case 'c': {
            int16_t v;
            memcpy(&v, &pkt[ofs], sizeof(v));
            port->printf_P(PSTR("%.2f"), (double)(0.01f*v));
            ofs += sizeof(v);
            break;
        }
        case 'C': {
            uint16_t v;
            memcpy(&v, &pkt[ofs], sizeof(v));
            port->printf_P(PSTR("%.2f"), (double)(0.01f*v));
            ofs += sizeof(v);
            break;
        }
        case 'e': {
            int32_t v;
            memcpy(&v, &pkt[ofs], sizeof(v));
            port->printf_P(PSTR("%.2f"), (double)(0.01f*v));
            ofs += sizeof(v);
            break;
        }
        case 'E': {
            uint32_t v;
            memcpy(&v, &pkt[ofs], sizeof(v));
            port->printf_P(PSTR("%.2f"), (double)(0.01f*v));
            ofs += sizeof(v);
            break;
        }
        case 'L': {
            int32_t v;
            memcpy(&v, &pkt[ofs], sizeof(v));
            print_latlon(port, v);
            ofs += sizeof(v);
            break;
        }
        case 'n': {
            char v[5];
            memcpy(&v, &pkt[ofs], sizeof(v));
            v[sizeof(v)-1] = 0;
            port->printf_P(PSTR("%s"), v);
            ofs += sizeof(v)-1;
            break;
        }
        case 'N': {
            char v[17];
            memcpy(&v, &pkt[ofs], sizeof(v));
            v[sizeof(v)-1] = 0;
            port->printf_P(PSTR("%s"), v);
            ofs += sizeof(v)-1;
            break;
        }
        case 'Z': {
            char v[65];
            memcpy(&v, &pkt[ofs], sizeof(v));
            v[sizeof(v)-1] = 0;
            port->printf_P(PSTR("%s"), v);
            ofs += sizeof(v)-1;
            break;
        }
        case 'M': {
            print_mode(port, pkt[ofs]);
            ofs += 1;
            break;
        }
        default:
            ofs = msg_len;
            break;
        }
        if (ofs < msg_len) {
            port->printf_P(PSTR(", "));
        }
    }
    port->println();
}
Example #30
0
// Save the variable to EEPROM, if supported
//
bool AP_Param::save(void)
{
    uint32_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;
    }

    // if the value is the default value then don't save
    if (phdr.type <= AP_PARAM_FLOAT) {
        float v1 = cast_to_float((enum ap_var_type)phdr.type);
        float v2;
        if (ginfo != NULL) {
            v2 = PGM_FLOAT(&ginfo->def_value);
        } else {
            v2 = PGM_FLOAT(&info->def_value);
        }
        if (v1 == v2) {
            return true;
        }
        if (phdr.type != AP_PARAM_INT32 &&
            (fabsf(v1-v2) < 0.0001f*fabsf(v1))) {
            // for other than 32 bit integers, we accept values within
            // 0.01 percent of the current value as being the same
            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));
    eeprom_write_check(&phdr, ofs, sizeof(phdr));
    return true;
}