mirror of https://github.com/ArduPilot/ardupilot
AP_Param: add set_default, configured
This commit is contained in:
parent
87ea9acc7f
commit
39246c13ad
|
@ -816,6 +816,63 @@ bool AP_Param::load(void)
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool AP_Param::configured_in_defaults_file(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;
|
||||||
|
}
|
||||||
|
|
||||||
|
const float* def_value_ptr;
|
||||||
|
|
||||||
|
if (ginfo != NULL) {
|
||||||
|
def_value_ptr = &ginfo->def_value;
|
||||||
|
} else {
|
||||||
|
def_value_ptr = &info->def_value;
|
||||||
|
}
|
||||||
|
|
||||||
|
for (uint16_t i=0; i<num_param_overrides; i++) {
|
||||||
|
if (def_value_ptr == param_overrides[i].def_value_ptr) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
// set a AP_Param variable to a specified value
|
// set a AP_Param variable to a specified value
|
||||||
void AP_Param::set_value(enum ap_var_type type, void *ptr, float value)
|
void AP_Param::set_value(enum ap_var_type type, void *ptr, float value)
|
||||||
{
|
{
|
||||||
|
|
|
@ -259,6 +259,15 @@ public:
|
||||||
// check var table for consistency
|
// check var table for consistency
|
||||||
static bool check_var_info(void);
|
static bool check_var_info(void);
|
||||||
|
|
||||||
|
// return true if the parameter is configured in the defaults file
|
||||||
|
bool configured_in_defaults_file(void);
|
||||||
|
|
||||||
|
// return true if the parameter is configured in EEPROM/FRAM
|
||||||
|
bool configured_in_storage(void);
|
||||||
|
|
||||||
|
// return true if the parameter is configured
|
||||||
|
bool configured(void) { return configured_in_defaults_file() || configured_in_storage(); }
|
||||||
|
|
||||||
private:
|
private:
|
||||||
/// EEPROM header
|
/// EEPROM header
|
||||||
///
|
///
|
||||||
|
@ -417,6 +426,14 @@ public:
|
||||||
_value = v;
|
_value = v;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets if the parameter is unconfigured
|
||||||
|
///
|
||||||
|
void set_default(const T &v) {
|
||||||
|
if (!configured()) {
|
||||||
|
set(v);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/// Combined set and save
|
/// Combined set and save
|
||||||
///
|
///
|
||||||
bool set_and_save(const T &v) {
|
bool set_and_save(const T &v) {
|
||||||
|
|
Loading…
Reference in New Issue