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);
}
// print the value of all variables
void AP_Param::show_all(void)
{
ParamToken token;
AP_Param *ap;
enum ap_var_type type;
for (ap=AP_Param::first(&token, &type);
ap;
ap=AP_Param::next_scalar(&token, &type)) {
char s[AP_MAX_NAME_SIZE+1];
ap->copy_name_token(&token, s, sizeof(s), true);
s[AP_MAX_NAME_SIZE] = 0;
switch (type) {
case AP_PARAM_INT8:
hal.console->printf_P(PSTR("%s: %d\n"), s, (int)((AP_Int8 *)ap)->get());
break;
case AP_PARAM_INT16:
hal.console->printf_P(PSTR("%s: %d\n"), s, (int)((AP_Int16 *)ap)->get());
break;
case AP_PARAM_INT32:
hal.console->printf_P(PSTR("%s: %ld\n"), s, (long)((AP_Int32 *)ap)->get());
break;
case AP_PARAM_FLOAT:
hal.console->printf_P(PSTR("%s: %f\n"), s, ((AP_Float *)ap)->get());
break;
default:
break;
}
}
}
FlightAxis::FlightAxis(const char *home_str, const char *frame_str) :
Aircraft(home_str, frame_str)
{
use_time_sync = false;
rate_hz = 250 / target_speedup;
heli_demix = strstr(frame_str, "helidemix") != nullptr;
rev4_servos = strstr(frame_str, "rev4") != nullptr;
const char *colon = strchr(frame_str, ':');
if (colon) {
controller_ip = colon+1;
}
for (uint8_t i=0; i<ARRAY_SIZE(sim_defaults); i++) {
AP_Param::set_default_by_name(sim_defaults[i].name, sim_defaults[i].value);
if (sim_defaults[i].save) {
enum ap_var_type ptype;
AP_Param *p = AP_Param::find(sim_defaults[i].name, &ptype);
if (!p->configured()) {
p->save();
}
}
}
/* Create the thread that will be waiting for data from FlightAxis */
mutex = hal.util->new_semaphore();
int ret = pthread_create(&thread, NULL, update_thread, this);
if (ret != 0) {
AP_HAL::panic("SIM_FlightAxis: failed to create thread");
}
}
/**
* @brief Send the next pending parameter, called from deferred message
* handling code
*/
void
GCS_MAVLINK::queued_param_send()
{
if (!initialised || _queued_parameter == NULL) {
return;
}
uint16_t bytes_allowed;
uint8_t count;
uint32_t tnow = hal.scheduler->millis();
// use at most 30% of bandwidth on parameters. The constant 26 is
// 1/(1000 * 1/8 * 0.001 * 0.3)
bytes_allowed = 57 * (tnow - _queued_parameter_send_time_ms) * 26;
if (bytes_allowed > comm_get_txspace(chan)) {
bytes_allowed = comm_get_txspace(chan);
}
count = bytes_allowed / (MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES);
// when we don't have flow control we really need to keep the
// param download very slow, or it tends to stall
if (!have_flow_control() && count > 5) {
count = 5;
}
while (_queued_parameter != NULL && count--) {
AP_Param *vp;
float value;
// copy the current parameter and prepare to move to the next
vp = _queued_parameter;
// if the parameter can be cast to float, report it here and break out of the loop
value = vp->cast_to_float(_queued_parameter_type);
char param_name[AP_MAX_NAME_SIZE];
vp->copy_name_token(_queued_parameter_token, param_name, sizeof(param_name), true);
mavlink_msg_param_value_send(
chan,
param_name,
value,
mav_var_type(_queued_parameter_type),
_queued_parameter_count,
_queued_parameter_index);
_queued_parameter = AP_Param::next_scalar(&_queued_parameter_token, &_queued_parameter_type);
_queued_parameter_index++;
}
_queued_parameter_send_time_ms = tnow;
}
Morse::Morse(const char *home_str, const char *frame_str) :
Aircraft(home_str, frame_str)
{
char *saveptr = nullptr;
char *s = strdup(frame_str);
char *frame_option = strtok_r(s, ":", &saveptr);
char *args1 = strtok_r(nullptr, ":", &saveptr);
char *args2 = strtok_r(nullptr, ":", &saveptr);
char *args3 = strtok_r(nullptr, ":", &saveptr);
/*
allow setting of IP, sensors port and control port
format morse:IPADDRESS:SENSORS_PORT:CONTROL_PORT
*/
if (args1) {
morse_ip = args1;
}
if (args2) {
morse_sensors_port = atoi(args2);
morse_control_port = morse_sensors_port+1;
}
if (args3) {
morse_control_port = atoi(args3);
}
if (strstr(frame_option, "-rover")) {
output_type = OUTPUT_ROVER;
} else if (strstr(frame_option, "-quad")) {
output_type = OUTPUT_QUAD;
} else if (strstr(frame_option, "-pwm")) {
output_type = OUTPUT_PWM;
} else {
// default to rover
output_type = OUTPUT_ROVER;
}
for (uint8_t i=0; i<ARRAY_SIZE(sim_defaults); i++) {
AP_Param::set_default_by_name(sim_defaults[i].name, sim_defaults[i].value);
if (sim_defaults[i].save) {
enum ap_var_type ptype;
AP_Param *p = AP_Param::find(sim_defaults[i].name, &ptype);
if (!p->configured()) {
p->save();
}
}
}
printf("Started Morse with %s:%u:%u type %u\n",
morse_ip, morse_sensors_port, morse_control_port,
(unsigned)output_type);
}
/*
find the def_value for a variable by name
*/
const float *
AP_Param::find_def_value_ptr(const char *name)
{
enum ap_var_type ptype;
AP_Param *vp = find(name, &ptype);
if (vp == NULL) {
return NULL;
}
uint32_t group_element;
const struct GroupInfo *ginfo;
uint8_t gidx;
const struct AP_Param::Info *info = vp->find_var_info(&group_element, &ginfo, &gidx);
if (info == NULL) {
return NULL;
}
if (ginfo != NULL) {
return &ginfo->def_value;
}
return &info->def_value;
}
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);
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)
{
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)
{
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_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));
}
}
}
/*
load a default set of parameters from a file
*/
bool AP_Param::load_defaults_file(const char *filename)
{
FILE *f = fopen(filename, "r");
if (f == NULL) {
return false;
}
char line[100];
/*
work out how many parameter default structures to allocate
*/
uint16_t num_defaults = 0;
while (fgets(line, sizeof(line)-1, f)) {
char *pname;
float value;
if (!parse_param_line(line, &pname, value)) {
continue;
}
if (!find_def_value_ptr(pname)) {
fclose(f);
return false;
}
num_defaults++;
}
fclose(f);
if (param_overrides != NULL) {
free(param_overrides);
}
num_param_overrides = 0;
param_overrides = new param_override[num_defaults];
if (param_overrides == NULL) {
return false;
}
/*
re-open to avoid possible seek issues with NuttX
*/
f = fopen(filename, "r");
if (f == NULL) {
return false;
}
uint16_t idx = 0;
while (fgets(line, sizeof(line)-1, f)) {
char *pname;
float value;
if (!parse_param_line(line, &pname, value)) {
continue;
}
const float *def_value_ptr = find_def_value_ptr(pname);
if (!def_value_ptr) {
fclose(f);
return false;
}
param_overrides[idx].def_value_ptr = def_value_ptr;
param_overrides[idx].value = value;
idx++;
enum ap_var_type var_type;
AP_Param *vp = AP_Param::find(pname, &var_type);
if (!vp) {
fclose(f);
return false;
}
vp->set_float(value, var_type);
}
fclose(f);
num_param_overrides = num_defaults;
return true;
}
