void GCS_MAVLINK::handle_param_request_read(mavlink_message_t *msg)
{
mavlink_param_request_read_t packet;
mavlink_msg_param_request_read_decode(msg, &packet);
enum ap_var_type p_type;
AP_Param *vp;
char param_name[AP_MAX_NAME_SIZE+1];
if (packet.param_index != -1) {
AP_Param::ParamToken token;
vp = AP_Param::find_by_index(packet.param_index, &p_type, &token);
if (vp == NULL) {
return;
}
vp->copy_name_token(token, param_name, AP_MAX_NAME_SIZE, true);
param_name[AP_MAX_NAME_SIZE] = 0;
} else {
strncpy(param_name, packet.param_id, AP_MAX_NAME_SIZE);
param_name[AP_MAX_NAME_SIZE] = 0;
vp = AP_Param::find(param_name, &p_type);
if (vp == NULL) {
return;
}
}
float value = vp->cast_to_float(p_type);
mavlink_msg_param_value_send_buf(
msg,
chan,
param_name,
value,
mav_var_type(p_type),
_count_parameters(),
packet.param_index);
}
void GCS_MAVLINK::handle_param_set(mavlink_message_t *msg, DataFlash_Class *DataFlash)
{
mavlink_param_set_t packet;
mavlink_msg_param_set_decode(msg, &packet);
enum ap_var_type var_type;
// set parameter
AP_Param *vp;
char key[AP_MAX_NAME_SIZE+1];
strncpy(key, (char *)packet.param_id, AP_MAX_NAME_SIZE);
key[AP_MAX_NAME_SIZE] = 0;
// find existing param so we can get the old value
vp = AP_Param::find(key, &var_type);
if (vp == NULL) {
return;
}
float old_value = vp->cast_to_float(var_type);
// set the value
vp->set_float(packet.param_value, var_type);
/*
we force the save if the value is not equal to the old
value. This copes with the use of override values in
constructors, such as PID elements. Otherwise a set to the
default value which differs from the constructor value doesn't
save the change
*/
bool force_save = !is_equal(packet.param_value, old_value);
// save the change
vp->save(force_save);
// Report back the new value if we accepted the change
// we send the value we actually set, which could be
// different from the value sent, in case someone sent
// a fractional value to an integer type
mavlink_msg_param_value_send_buf(
msg,
chan,
key,
vp->cast_to_float(var_type),
mav_var_type(var_type),
_count_parameters(),
-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));
}
}
void GCS_MAVLINK::handle_param_request_list(mavlink_message_t *msg)
{
mavlink_param_request_list_t packet;
mavlink_msg_param_request_list_decode(msg, &packet);
// send system ID if we can
char sysid[40];
if (hal.util->get_system_id(sysid)) {
send_text(MAV_SEVERITY_WARNING, sysid);
}
// Start sending parameters - next call to ::update will kick the first one out
_queued_parameter = AP_Param::first(&_queued_parameter_token, &_queued_parameter_type);
_queued_parameter_index = 0;
_queued_parameter_count = _count_parameters();
}
/*
send a parameter value message to all active MAVLink connections
*/
void GCS_MAVLINK::send_parameter_value_all(const char *param_name, ap_var_type param_type, float param_value)
{
for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
if ((1U<<i) & mavlink_active) {
mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0+i);
if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_PARAM_VALUE_LEN) {
mavlink_msg_param_value_send(
chan,
param_name,
param_value,
mav_var_type(param_type),
_count_parameters(),
-1);
}
}
}
}
void GCS_MAVLINK::handle_param_request_list(mavlink_message_t *msg)
{
mavlink_param_request_list_t packet;
mavlink_msg_param_request_list_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) {
return;
}
#if CONFIG_HAL_BOARD != HAL_BOARD_APM1 && CONFIG_HAL_BOARD != HAL_BOARD_APM2
// send system ID if we can
char sysid[40];
if (hal.util->get_system_id(sysid)) {
send_text(SEVERITY_LOW, sysid);
}
#endif
// Start sending parameters - next call to ::update will kick the first one out
_queued_parameter = AP_Param::first(&_queued_parameter_token, &_queued_parameter_type);
_queued_parameter_index = 0;
_queued_parameter_count = _count_parameters();
}
void GCS_MAVLINK::handle_param_set(mavlink_message_t *msg, DataFlash_Class *DataFlash)
{
mavlink_param_set_t packet;
mavlink_msg_param_set_decode(msg, &packet);
enum ap_var_type var_type;
// set parameter
AP_Param *vp;
char key[AP_MAX_NAME_SIZE+1];
strncpy(key, (char *)packet.param_id, AP_MAX_NAME_SIZE);
key[AP_MAX_NAME_SIZE] = 0;
vp = AP_Param::set_param_by_name(key, packet.param_value, &var_type);
if (vp == NULL) {
return;
}
// save the change
vp->save();
// Report back the new value if we accepted the change
// we send the value we actually set, which could be
// different from the value sent, in case someone sent
// a fractional value to an integer type
mavlink_msg_param_value_send_buf(
msg,
chan,
key,
vp->cast_to_float(var_type),
mav_var_type(var_type),
_count_parameters(),
-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));
}
}
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_msg_param_set_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) {
return;
}
// set parameter
char key[AP_MAX_NAME_SIZE+1];
strncpy(key, (char *)packet.param_id, AP_MAX_NAME_SIZE);
key[AP_MAX_NAME_SIZE] = 0;
// find the requested parameter
vp = AP_Param::find(key, &var_type);
if ((NULL != vp) && // exists
!isnan(packet.param_value) && // not nan
!isinf(packet.param_value)) { // not inf
// 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_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
// we send the value we actually set, which could be
// different from the value sent, in case someone sent
// a fractional value to an integer type
mavlink_msg_param_value_send_buf(
msg,
chan,
key,
vp->cast_to_float(var_type),
mav_var_type(var_type),
_count_parameters(),
-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));
}
}
}
