mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
AP_Param: added set_param_by_name()
this simplifies the GCS_MAVLink code
This commit is contained in:
parent
c1a0375562
commit
5ca38e3d75
@ -1161,3 +1161,56 @@ void AP_Param::convert_old_parameters(const struct ConversionInfo *conversion_ta
|
|||||||
convert_old_parameter(&conversion_table[i]);
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
|
@ -196,6 +196,14 @@ public:
|
|||||||
// set a AP_Param variable to a specified value
|
// set a AP_Param variable to a specified value
|
||||||
static void set_value(enum ap_var_type type, void *ptr, float def_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
|
// load default values for scalars in a group
|
||||||
static void setup_object_defaults(const void *object_pointer, const struct GroupInfo *group_info);
|
static void setup_object_defaults(const void *object_pointer, const struct GroupInfo *group_info);
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user