Browse Source

AP_Param: added set_param_by_name()

this simplifies the GCS_MAVLink code
mission-4.1.18
Andrew Tridgell 10 years ago
parent
commit
5ca38e3d75
  1. 53
      libraries/AP_Param/AP_Param.cpp
  2. 8
      libraries/AP_Param/AP_Param.h

53
libraries/AP_Param/AP_Param.cpp

@ -1161,3 +1161,56 @@ void AP_Param::convert_old_parameters(const struct ConversionInfo *conversion_ta @@ -1161,3 +1161,56 @@ void AP_Param::convert_old_parameters(const struct ConversionInfo *conversion_ta
convert_old_parameter(&conversion_table[i]);
}
}
/*
set a parameter by name
*/
AP_Param *AP_Param::set_param_by_name(const char *pname, float value, enum ap_var_type *ptype)
{
AP_Param *vp;
enum ap_var_type var_type;
if (isnan(value) || isinf(value)) {
return NULL;
}
// find the requested parameter
vp = AP_Param::find(pname, &var_type);
if (vp == NULL) {
return NULL;
}
if (ptype != NULL) {
*ptype = var_type;
}
// add a small amount before casting parameter values
// from float to integer to avoid truncating to the
// next lower integer value.
float rounding_addition = 0.01;
// handle variables with standard type IDs
if (var_type == AP_PARAM_FLOAT) {
((AP_Float *)vp)->set(value);
} else if (var_type == AP_PARAM_INT32) {
if (value < 0) rounding_addition = -rounding_addition;
float v = value+rounding_addition;
v = constrain_float(v, -2147483648.0, 2147483647.0);
((AP_Int32 *)vp)->set(v);
} else if (var_type == AP_PARAM_INT16) {
if (value < 0) rounding_addition = -rounding_addition;
float v = value+rounding_addition;
v = constrain_float(v, -32768, 32767);
((AP_Int16 *)vp)->set(v);
} else if (var_type == AP_PARAM_INT8) {
if (value < 0) rounding_addition = -rounding_addition;
float v = value+rounding_addition;
v = constrain_float(v, -128, 127);
((AP_Int8 *)vp)->set(v);
} else {
// we don't support mavlink set on this parameter
return NULL;
}
return vp;
}

8
libraries/AP_Param/AP_Param.h

@ -196,6 +196,14 @@ public: @@ -196,6 +196,14 @@ public:
// set a AP_Param variable to a specified value
static void set_value(enum ap_var_type type, void *ptr, float def_value);
/*
set a parameter by name
The parameter pointer is returned on success
*/
static AP_Param *set_param_by_name(const char *pname, float value, enum ap_var_type *ptype);
// load default values for scalars in a group
static void setup_object_defaults(const void *object_pointer, const struct GroupInfo *group_info);

Loading…
Cancel
Save