Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-08 08:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: use AP_Param::set_param_by_name()

Browse Source
This commit is contained in:
Andrew Tridgell 2015-04-01 15:54:52 -07:00
parent 5ca38e3d75
commit 82a51e8791
1 changed files with 23 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -572,66 +572,39 @@ void GCS_MAVLINK::handle_param_request_read(mavlink_message_t *msg)
void GCS_MAVLINK::handle_param_set(mavlink_message_t *msg, DataFlash_Class *DataFlash) void GCS_MAVLINK::handle_param_set(mavlink_message_t *msg, DataFlash_Class *DataFlash)
{ {
AP_Param *vp;
enum ap_var_type var_type;
mavlink_param_set_t packet; mavlink_param_set_t packet;
mavlink_msg_param_set_decode(msg, &packet); mavlink_msg_param_set_decode(msg, &packet);
enum ap_var_type var_type;
// set parameter // set parameter
AP_Param *vp;
char key[AP_MAX_NAME_SIZE+1]; char key[AP_MAX_NAME_SIZE+1];
strncpy(key, (char *)packet.param_id, AP_MAX_NAME_SIZE); strncpy(key, (char *)packet.param_id, AP_MAX_NAME_SIZE);
key[AP_MAX_NAME_SIZE] = 0; key[AP_MAX_NAME_SIZE] = 0;
// find the requested parameter vp = AP_Param::set_param_by_name(key, packet.param_value, &var_type);
vp = AP_Param::find(key, &var_type); if (vp == NULL) {
if ((NULL != vp) && // exists return;
!isnan(packet.param_value) && // not nan }
!isinf(packet.param_value)) { // not inf
// add a small amount before casting parameter values // save the change
// from float to integer to avoid truncating to the vp->save();
// 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_and_save(packet.param_value);
} else if (var_type == AP_PARAM_INT32) {
if (packet.param_value < 0) rounding_addition = -rounding_addition;
float v = packet.param_value+rounding_addition;
v = constrain_float(v, -2147483648.0, 2147483647.0);
((AP_Int32 *)vp)->set_and_save(v);
} else if (var_type == AP_PARAM_INT16) {
if (packet.param_value < 0) rounding_addition = -rounding_addition;
float v = packet.param_value+rounding_addition;
v = constrain_float(v, -32768, 32767);
((AP_Int16 *)vp)->set_and_save(v);
} else if (var_type == AP_PARAM_INT8) {
if (packet.param_value < 0) rounding_addition = -rounding_addition;
float v = packet.param_value+rounding_addition;
v = constrain_float(v, -128, 127);
((AP_Int8 *)vp)->set_and_save(v);
} else {
// we don't support mavlink set on this parameter
return;
}
// Report back the new value if we accepted the change // Report back the new value if we accepted the change
// we send the value we actually set, which could be // we send the value we actually set, which could be
// different from the value sent, in case someone sent // different from the value sent, in case someone sent
// a fractional value to an integer type // a fractional value to an integer type
mavlink_msg_param_value_send_buf( mavlink_msg_param_value_send_buf(
msg, msg,
chan, chan,
key, key,
vp->cast_to_float(var_type), vp->cast_to_float(var_type),
mav_var_type(var_type), mav_var_type(var_type),
_count_parameters(), _count_parameters(),
-1); // XXX we don't actually know what its index is... -1); // XXX we don't actually know what its index is...
if (DataFlash != NULL) {
DataFlash->Log_Write_Parameter(key, vp->cast_to_float(var_type)); if (DataFlash != NULL) {
} DataFlash->Log_Write_Parameter(key, vp->cast_to_float(var_type));
} }
} }