void copy_params_to_shmem(struct param_info_s *param_info_base)
{
param_t param;
unsigned int i;
if (get_shmem_lock(__FILE__, __LINE__) != 0) {
PX4_ERR("Could not get shmem lock");
return;
}
PX4_DEBUG("%d krait params allocated", param_count());
for (param = 0; param < param_count(); param++) {
struct param_wbuf_s *s = param_find_changed(param);
if (s == NULL) {
shmem_info_p->params_val[param] = param_info_base[param].val;
} else {
shmem_info_p->params_val[param] = s->val;
}
#ifdef SHMEM_DEBUG
if (param_type(param) == PARAM_TYPE_INT32) {
{
PX4_INFO("%d: written %d for param %s to shared mem",
param, shmem_info_p->params_val[param].i, param_name(param));
}
} else if (param_type(param) == PARAM_TYPE_FLOAT) {
{
PX4_INFO("%d: written %f for param %s to shared mem",
param, (double)shmem_info_p->params_val[param].f, param_name(param));
}
}
#endif
}
//PX4_DEBUG("written %u params to shmem offset %lu", param_count(), (unsigned char*)&shmem_info_p->params_count-(unsigned char*)shmem_info_p);
for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) {
shmem_info_p->krait_changed_index[i] = 0;
adsp_changed_index[i] = 0;
}
release_shmem_lock();
}
static void
do_show(const char *search_string)
{
printf("Symbols: x = used, + = saved, * = unsaved\n");
param_foreach(do_show_print, (char *)search_string, false, false);
printf("\n %u parameters total, %u used.\n", param_count(), param_count_used());
}
void
MavlinkParametersManager::send(const hrt_abstime t)
{
/* send all parameters if requested */
if (_send_all_index >= 0) {
/* skip if no space is available */
if (_mavlink->get_free_tx_buf() < get_size()) {
return;
}
/* look for the first parameter which is used */
param_t p;
do {
/* walk through all parameters, including unused ones */
p = param_for_index(_send_all_index);
_send_all_index++;
} while (p != PARAM_INVALID && !param_used(p));
if (p != PARAM_INVALID) {
send_param(p);
}
if ((p == PARAM_INVALID) || (_send_all_index >= (int) param_count())) {
_send_all_index = -1;
}
}
}
void
MavlinkParametersManager::send(const hrt_abstime t)
{
/* send all parameters if requested, but only after the system has booted */
if (_send_all_index >= 0 && _mavlink->boot_complete()) {
/* skip if no space is available */
if (_mavlink->get_free_tx_buf() < get_size()) {
return;
}
/* look for the first parameter which is used */
param_t p;
do {
/* walk through all parameters, including unused ones */
p = param_for_index(_send_all_index);
_send_all_index++;
} while (p != PARAM_INVALID && !param_used(p));
if (p != PARAM_INVALID) {
send_param(p);
}
if ((p == PARAM_INVALID) || (_send_all_index >= (int) param_count())) {
_send_all_index = -1;
}
} else if (_send_all_index == 0 && hrt_absolute_time() > 20 * 1000 * 1000) {
/* the boot did not seem to ever complete, warn user and set boot complete */
_mavlink->send_statustext_critical("WARNING: SYSTEM BOOT INCOMPLETE. CHECK CONFIG.");
_mavlink->set_boot_complete();
}
}
bool
MavlinkParametersManager::send_untransmitted()
{
bool sent_one = false;
// Check for untransmitted system parameters
if (_mavlink_parameter_sub < 0) {
_mavlink_parameter_sub = orb_subscribe(ORB_ID(parameter_update));
}
bool param_ready;
orb_check(_mavlink_parameter_sub, ¶m_ready);
if (param_ready) {
// Clear the ready flag
struct parameter_update_s value;
orb_copy(ORB_ID(parameter_update), _mavlink_parameter_sub, &value);
// Schedule an update if not already the case
if (_param_update_time == 0) {
_param_update_time = value.timestamp;
_param_update_index = 0;
}
}
if ((_param_update_time != 0) && ((_param_update_time + 5 * 1000) < hrt_absolute_time())) {
param_t param = 0;
// send out all changed values
do {
// skip over all parameters which are not invalid and not used
do {
param = param_for_index(_param_update_index);
++_param_update_index;
} while (param != PARAM_INVALID && !param_used(param));
// send parameters which are untransmitted while there is
// space in the TX buffer
if ((param != PARAM_INVALID) && param_value_unsaved(param)) {
int ret = send_param(param);
char buf[100];
strncpy(&buf[0], param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
sent_one = true;
if (ret != PX4_OK) {
break;
}
}
} while ((_mavlink->get_free_tx_buf() >= get_size()) && (_param_update_index < (int) param_count()));
// Flag work as done once all params have been sent
if (_param_update_index >= (int) param_count()) {
_param_update_time = 0;
}
}
return sent_one;
}
/* TAC to stdout */
void print_tac(tac_quad *quad) {
if (quad==NULL) return;
switch(quad->type) {
case TT_LABEL:
printf("%s:\n", to_string(quad->operand1));
break;
case TT_FN_DEF:
printf("_%s:\n", to_string(quad->operand1));
break;
case TT_FN_CALL:
printf("%s = CallFn _%s\n", correct_string_rep(quad->result), correct_string_rep(quad->operand1));
break;
case TT_INIT_FRAME:
printf("InitFrame %s\n", correct_string_rep(quad->operand1));
break;
case TT_POP_PARAM:
printf("PopParam %s\n", quad->operand1->identifier);
break;
case TT_PUSH_PARAM:
printf("PushParam %s\n", correct_string_rep(quad->operand1));
break;
case TT_IF:
printf("If %s Goto %s\n", correct_string_rep(quad->operand1), correct_string_rep(quad->result));
break;
case TT_ASSIGN:
printf("%s %s %s\n", correct_string_rep(quad->result), quad->op, correct_string_rep(quad->operand1));
break;
case TT_GOTO:
printf("Goto %s\n", correct_string_rep(quad->operand1));
break;
case TT_OP:
printf("%s = %s %s %s\n", correct_string_rep(quad->result), correct_string_rep(quad->operand1), quad->op, correct_string_rep(quad->operand2));
break;
case TT_RETURN:
if (quad->operand1) {
printf("Return %s\n", correct_string_rep(quad->operand1));
}
else {
printf("Return");
}
break;
case TT_PREPARE:
printf("PrepareToCall %d\n", param_count(quad->operand1));
break;
case TT_BEGIN_FN:
printf("BeginFn %s\n", correct_string_rep(quad->operand1));
break;
case TT_FN_BODY:
printf("FnBody\n");
break;
case TT_END_FN:
printf("EndFn\n");
break;
default:
fatal("Unknown TAC Quad type '%d'", quad->type);
break;
}
print_tac(quad->next);
}
bool
MavlinkParametersManager::send_one()
{
if (_send_all_index >= 0 && _mavlink->boot_complete()) {
/* send all parameters if requested, but only after the system has booted */
/* The first thing we send is a hash of all values for the ground
* station to try and quickly load a cached copy of our params
*/
if (_send_all_index == PARAM_HASH) {
/* return hash check for cached params */
uint32_t hash = param_hash_check();
/* build the one-off response message */
mavlink_param_value_t msg;
msg.param_count = param_count_used();
msg.param_index = -1;
strncpy(msg.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
msg.param_type = MAV_PARAM_TYPE_UINT32;
memcpy(&msg.param_value, &hash, sizeof(hash));
mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg);
/* after this we should start sending all params */
_send_all_index = 0;
/* No further action, return now */
return true;
}
/* look for the first parameter which is used */
param_t p;
do {
/* walk through all parameters, including unused ones */
p = param_for_index(_send_all_index);
_send_all_index++;
} while (p != PARAM_INVALID && !param_used(p));
if (p != PARAM_INVALID) {
send_param(p);
}
if ((p == PARAM_INVALID) || (_send_all_index >= (int) param_count())) {
_send_all_index = -1;
return false;
} else {
return true;
}
} else if (_send_all_index == PARAM_HASH && hrt_absolute_time() > 20 * 1000 * 1000) {
/* the boot did not seem to ever complete, warn user and set boot complete */
_mavlink->send_statustext_critical("WARNING: SYSTEM BOOT INCOMPLETE. CHECK CONFIG.");
_mavlink->set_boot_complete();
}
return false;
}
static int
do_show(const char *search_string, bool only_changed)
{
PARAM_PRINT("Symbols: x = used, + = saved, * = unsaved\n");
param_foreach(do_show_print, (char *)search_string, only_changed, false);
PARAM_PRINT("\n %u parameters total, %u used.\n", param_count(), param_count_used());
return 0;
}
int mavlink_pm_queued_send()
{
if (mavlink_param_queue_index < param_count()) {
mavlink_pm_send_param(param_for_index(mavlink_param_queue_index));
mavlink_param_queue_index++;
return 0;
} else {
return 1;
}
}
void
MavlinkParametersManager::send(const hrt_abstime t)
{
/* send all parameters if requested */
if (_send_all_index >= 0) {
send_param(param_for_index(_send_all_index));
_send_all_index++;
if (_send_all_index >= (int) param_count()) {
_send_all_index = -1;
}
}
}
int Mavlink::mavlink_pm_send_param(param_t param)
{
if (param == PARAM_INVALID) { return 1; }
/* buffers for param transmission */
char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
float val_buf;
mavlink_message_t tx_msg;
/* query parameter type */
param_type_t type = param_type(param);
/* copy parameter name */
strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/*
* Map onboard parameter type to MAVLink type,
* endianess matches (both little endian)
*/
uint8_t mavlink_type;
if (type == PARAM_TYPE_INT32) {
mavlink_type = MAVLINK_TYPE_INT32_T;
} else if (type == PARAM_TYPE_FLOAT) {
mavlink_type = MAVLINK_TYPE_FLOAT;
} else {
mavlink_type = MAVLINK_TYPE_FLOAT;
}
/*
* get param value, since MAVLink encodes float and int params in the same
* space during transmission, copy param onto float val_buf
*/
int ret;
if ((ret = param_get(param, &val_buf)) != OK) {
return ret;
}
mavlink_msg_param_value_pack_chan(mavlink_system.sysid,
mavlink_system.compid,
_channel,
&tx_msg,
name_buf,
val_buf,
mavlink_type,
param_count(),
param_get_index(param));
send_message(&tx_msg);
return OK;
}
void copy_params_to_shmem(const param_info_s *param_info_base)
{
param_t param;
unsigned int i;
if (get_shmem_lock(__FILE__, __LINE__) != 0) {
PX4_INFO("Could not get shmem lock\n");
return;
}
//else PX4_INFO("Got lock\n");
for (param = 0; param < param_count(); param++) {
//{PX4_INFO("writing to offset %d\n", (unsigned char*)(shmem_info_p->adsp_params[param].name)-(unsigned char*)shmem_info_p);}
struct param_wbuf_s *s = param_find_changed(param);
if (s == NULL) {
shmem_info_p->params_val[param] = param_info_base[param].val;
}
else {
shmem_info_p->params_val[param] = s->val;
}
#ifdef SHMEM_DEBUG
if (param_type(param) == PARAM_TYPE_INT32) {
PX4_INFO("%d: written %d for param %s to shared mem", param, shmem_info_p->params_val[param].i, param_name(param));
} else if (param_type(param) == PARAM_TYPE_FLOAT) {
PX4_INFO("%d: written %f for param %s to shared mem", param, shmem_info_p->params_val[param].f, param_name(param));
}
#endif
}
for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) {
shmem_info_p->adsp_changed_index[i] = 0;
krait_changed_index[i] = 0;
}
release_shmem_lock(__FILE__, __LINE__);
//PX4_INFO("Released lock\n");
}
void
MavlinkParametersManager::send_param(param_t param)
{
if (param == PARAM_INVALID) {
return;
}
mavlink_param_value_t msg;
/*
* get param value, since MAVLink encodes float and int params in the same
* space during transmission, copy param onto float val_buf
*/
if (param_get(param, &msg.param_value) != OK) {
return;
}
msg.param_count = param_count();
msg.param_index = param_get_index(param);
/* copy parameter name */
strncpy(msg.param_id, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* query parameter type */
param_type_t type = param_type(param);
/*
* Map onboard parameter type to MAVLink type,
* endianess matches (both little endian)
*/
if (type == PARAM_TYPE_INT32) {
msg.param_type = MAVLINK_TYPE_INT32_T;
} else if (type == PARAM_TYPE_FLOAT) {
msg.param_type = MAVLINK_TYPE_FLOAT;
} else {
msg.param_type = MAVLINK_TYPE_FLOAT;
}
_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg);
}
int write_parameters(int fd)
{
/* construct parameter message */
struct {
LOG_PACKET_HEADER;
struct log_PARM_s body;
} log_msg_PARM = {
LOG_PACKET_HEADER_INIT(LOG_PARM_MSG),
};
int written = 0;
param_t params_cnt = param_count();
for (param_t param = 0; param < params_cnt; param++) {
/* fill parameter message and write it */
strncpy(log_msg_PARM.body.name, param_name(param), sizeof(log_msg_PARM.body.name));
float value = NAN;
switch (param_type(param)) {
case PARAM_TYPE_INT32: {
int32_t i;
param_get(param, &i);
value = i; // cast integer to float
break;
}
case PARAM_TYPE_FLOAT:
param_get(param, &value);
break;
default:
break;
}
log_msg_PARM.body.value = value;
written += write(fd, &log_msg_PARM, sizeof(log_msg_PARM));
}
return written;
}
bool ParameterTest::exportImportAll()
{
static constexpr float MAGIC_FLOAT_VAL = 0.217828f;
// backup current parameters
const char *param_file_name = PX4_STORAGEDIR "/param_backup";
int fd = open(param_file_name, O_WRONLY | O_CREAT, PX4_O_MODE_666);
if (fd < 0) {
PX4_ERR("open '%s' failed (%i)", param_file_name, errno);
return false;
}
int result = param_export(fd, false);
if (result != PX4_OK) {
PX4_ERR("param_export failed");
close(fd);
return false;
}
close(fd);
bool ret = true;
int N = param_count();
// set all params to corresponding param_t value
for (unsigned i = 0; i < N; i++) {
param_t p = param_for_index(i);
if (p == PARAM_INVALID) {
PX4_ERR("param invalid: %d(%d)", p, i);
break;
}
if (param_type(p) == PARAM_TYPE_INT32) {
const int32_t set_val = p;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
int32_t get_val = 0;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", p, get_val);
}
if (param_type(p) == PARAM_TYPE_FLOAT) {
const float set_val = (float)p + MAGIC_FLOAT_VAL;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
float get_val = 0.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", p, (float)p + MAGIC_FLOAT_VAL);
}
}
// save
if (param_save_default() != PX4_OK) {
PX4_ERR("param_save_default failed");
return false;
}
// zero all params and verify, but don't save
for (unsigned i = 0; i < N; i++) {
param_t p = param_for_index(i);
if (param_type(p) == PARAM_TYPE_INT32) {
const int32_t set_val = 0;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param set failed: %d", p);
ut_assert("param_set_no_notification failed", false);
}
int32_t get_val = -1;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", set_val, get_val);
}
if (param_type(p) == PARAM_TYPE_FLOAT) {
float set_val = 0.0f;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param set failed: %d", p);
ut_assert("param_set_no_notification failed", false);
}
float get_val = -1.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", set_val, get_val);
}
}
// load saved params
if (param_load_default() != PX4_OK) {
PX4_ERR("param_save_default failed");
ret = true;
}
// check every param
for (unsigned i = 0; i < N; i++) {
param_t p = param_for_index(i);
if (param_type(p) == PARAM_TYPE_INT32) {
int32_t get_val = 0;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", p, get_val);
}
if (param_type(p) == PARAM_TYPE_FLOAT) {
float get_val = 0.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", p, (float)p + MAGIC_FLOAT_VAL);
}
}
param_reset_all();
// restore original params
fd = open(param_file_name, O_RDONLY);
if (fd < 0) {
PX4_ERR("open '%s' failed (%i)", param_file_name, errno);
return false;
}
result = param_import(fd);
close(fd);
if (result < 0) {
PX4_ERR("importing from '%s' failed (%i)", param_file_name, result);
return false;
}
// save
if (param_save_default() != PX4_OK) {
PX4_ERR("param_save_default failed");
return false;
}
return ret;
}
void
MavlinkParametersManager::send(const hrt_abstime t)
{
bool space_available = _mavlink->get_free_tx_buf() >= get_size();
/* Send parameter values received from the UAVCAN topic */
if (_uavcan_parameter_value_sub < 0) {
_uavcan_parameter_value_sub = orb_subscribe(ORB_ID(uavcan_parameter_value));
}
bool param_value_ready;
orb_check(_uavcan_parameter_value_sub, ¶m_value_ready);
if (space_available && param_value_ready) {
struct uavcan_parameter_value_s value;
orb_copy(ORB_ID(uavcan_parameter_value), _uavcan_parameter_value_sub, &value);
mavlink_param_value_t msg;
msg.param_count = value.param_count;
msg.param_index = value.param_index;
strncpy(msg.param_id, value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
if (value.param_type == MAV_PARAM_TYPE_REAL32) {
msg.param_type = MAVLINK_TYPE_FLOAT;
msg.param_value = value.real_value;
} else {
int32_t val;
val = (int32_t)value.int_value;
memcpy(&msg.param_value, &val, sizeof(int32_t));
msg.param_type = MAVLINK_TYPE_INT32_T;
}
_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg, value.node_id);
} else if (_send_all_index >= 0 && _mavlink->boot_complete()) {
/* send all parameters if requested, but only after the system has booted */
/* skip if no space is available */
if (!space_available) {
return;
}
/* The first thing we send is a hash of all values for the ground
* station to try and quickly load a cached copy of our params
*/
if (_send_all_index == PARAM_HASH) {
/* return hash check for cached params */
uint32_t hash = param_hash_check();
/* build the one-off response message */
mavlink_param_value_t msg;
msg.param_count = param_count_used();
msg.param_index = -1;
strncpy(msg.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
msg.param_type = MAV_PARAM_TYPE_UINT32;
memcpy(&msg.param_value, &hash, sizeof(hash));
_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg);
/* after this we should start sending all params */
_send_all_index = 0;
/* No further action, return now */
return;
}
/* look for the first parameter which is used */
param_t p;
do {
/* walk through all parameters, including unused ones */
p = param_for_index(_send_all_index);
_send_all_index++;
} while (p != PARAM_INVALID && !param_used(p));
if (p != PARAM_INVALID) {
send_param(p);
}
if ((p == PARAM_INVALID) || (_send_all_index >= (int) param_count())) {
_send_all_index = -1;
}
} else if (_send_all_index == PARAM_HASH && hrt_absolute_time() > 20 * 1000 * 1000) {
/* the boot did not seem to ever complete, warn user and set boot complete */
_mavlink->send_statustext_critical("WARNING: SYSTEM BOOT INCOMPLETE. CHECK CONFIG.");
_mavlink->set_boot_complete();
}
}
int
Mavlink::task_main(int argc, char *argv[])
{
int ch;
_baudrate = 57600;
_datarate = 0;
_mode = MAVLINK_MODE_NORMAL;
/* work around some stupidity in task_create's argv handling */
argc -= 2;
argv += 2;
/* don't exit from getopt loop to leave getopt global variables in consistent state,
* set error flag instead */
bool err_flag = false;
while ((ch = getopt(argc, argv, "b:r:d:m:fpvwx")) != EOF) {
switch (ch) {
case 'b':
_baudrate = strtoul(optarg, NULL, 10);
if (_baudrate < 9600 || _baudrate > 921600) {
warnx("invalid baud rate '%s'", optarg);
err_flag = true;
}
break;
case 'r':
_datarate = strtoul(optarg, NULL, 10);
if (_datarate < 10 || _datarate > MAX_DATA_RATE) {
warnx("invalid data rate '%s'", optarg);
err_flag = true;
}
break;
case 'd':
_device_name = optarg;
break;
// case 'e':
// mavlink_link_termination_allowed = true;
// break;
case 'm':
if (strcmp(optarg, "custom") == 0) {
_mode = MAVLINK_MODE_CUSTOM;
} else if (strcmp(optarg, "camera") == 0) {
_mode = MAVLINK_MODE_CAMERA;
}
break;
case 'f':
_forwarding_on = true;
break;
case 'p':
_passing_on = true;
break;
case 'v':
_verbose = true;
break;
case 'w':
_wait_to_transmit = true;
break;
case 'x':
_ftp_on = true;
break;
default:
err_flag = true;
break;
}
}
if (err_flag) {
usage();
return ERROR;
}
if (_datarate == 0) {
/* convert bits to bytes and use 1/2 of bandwidth by default */
_datarate = _baudrate / 20;
}
if (_datarate > MAX_DATA_RATE) {
_datarate = MAX_DATA_RATE;
}
if (Mavlink::instance_exists(_device_name, this)) {
warnx("mavlink instance for %s already running", _device_name);
return ERROR;
}
/* inform about mode */
switch (_mode) {
case MAVLINK_MODE_NORMAL:
warnx("mode: NORMAL");
break;
case MAVLINK_MODE_CUSTOM:
warnx("mode: CUSTOM");
break;
case MAVLINK_MODE_CAMERA:
warnx("mode: CAMERA");
break;
default:
warnx("ERROR: Unknown mode");
break;
}
warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate);
/* flush stdout in case MAVLink is about to take it over */
fflush(stdout);
struct termios uart_config_original;
/* default values for arguments */
_uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original, &_is_usb_uart);
if (_uart_fd < 0) {
warn("could not open %s", _device_name);
return ERROR;
}
/* initialize mavlink text message buffering */
mavlink_logbuffer_init(&_logbuffer, 5);
/* if we are passing on mavlink messages, we need to prepare a buffer for this instance */
if (_passing_on || _ftp_on) {
/* initialize message buffer if multiplexing is on or its needed for FTP.
* make space for two messages plus off-by-one space as we use the empty element
* marker ring buffer approach.
*/
if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN + 2)) {
errx(1, "can't allocate message buffer, exiting");
}
/* initialize message buffer mutex */
pthread_mutex_init(&_message_buffer_mutex, NULL);
}
/* create the device node that's used for sending text log messages, etc. */
register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL);
/* initialize logging device */
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
/* Initialize system properties */
mavlink_update_system();
/* start the MAVLink receiver */
_receive_thread = MavlinkReceiver::receive_start(this);
_mission_result_sub = orb_subscribe(ORB_ID(mission_result));
/* create mission manager */
_mission_manager = new MavlinkMissionManager(this);
_mission_manager->set_verbose(_verbose);
_task_running = true;
MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
uint64_t param_time = 0;
MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
uint64_t status_time = 0;
struct vehicle_status_s status;
status_sub->update(&status_time, &status);
MavlinkCommandsStream commands_stream(this, _channel);
/* add default streams depending on mode and intervals depending on datarate */
float rate_mult = get_rate_mult();
configure_stream("HEARTBEAT", 1.0f);
switch (_mode) {
case MAVLINK_MODE_NORMAL:
configure_stream("SYS_STATUS", 1.0f);
configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
configure_stream("HIGHRES_IMU", 1.0f * rate_mult);
configure_stream("ATTITUDE", 10.0f * rate_mult);
configure_stream("VFR_HUD", 8.0f * rate_mult);
configure_stream("GPS_RAW_INT", 1.0f * rate_mult);
configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult);
configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult);
configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult);
configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult);
configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult);
configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult);
configure_stream("DISTANCE_SENSOR", 0.5f);
break;
case MAVLINK_MODE_CAMERA:
configure_stream("SYS_STATUS", 1.0f);
configure_stream("ATTITUDE", 15.0f * rate_mult);
configure_stream("GLOBAL_POSITION_INT", 15.0f * rate_mult);
configure_stream("CAMERA_CAPTURE", 1.0f);
break;
default:
break;
}
/* don't send parameters on startup without request */
_mavlink_param_queue_index = param_count();
MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult);
/* set main loop delay depending on data rate to minimize CPU overhead */
_main_loop_delay = MAIN_LOOP_DELAY / rate_mult;
/* now the instance is fully initialized and we can bump the instance count */
LL_APPEND(_mavlink_instances, this);
while (!_task_should_exit) {
/* main loop */
usleep(_main_loop_delay);
perf_begin(_loop_perf);
hrt_abstime t = hrt_absolute_time();
if (param_sub->update(¶m_time, nullptr)) {
/* parameters updated */
mavlink_update_system();
}
if (status_sub->update(&status_time, &status)) {
/* switch HIL mode if required */
set_hil_enabled(status.hil_state == HIL_STATE_ON);
}
/* update commands stream */
commands_stream.update(t);
/* check for requested subscriptions */
if (_subscribe_to_stream != nullptr) {
if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
if (_subscribe_to_stream_rate > 0.0f) {
warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name,
(double)_subscribe_to_stream_rate);
} else {
warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name);
}
} else {
warnx("stream %s on device %s not found", _subscribe_to_stream, _device_name);
}
delete _subscribe_to_stream;
_subscribe_to_stream = nullptr;
}
/* update streams */
MavlinkStream *stream;
LL_FOREACH(_streams, stream) {
stream->update(t);
}
if (fast_rate_limiter.check(t)) {
mavlink_pm_queued_send();
_mission_manager->eventloop();
if (!mavlink_logbuffer_is_empty(&_logbuffer)) {
struct mavlink_logmessage msg;
int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg);
if (lb_ret == OK) {
send_statustext(msg.severity, msg.text);
}
}
}
/* pass messages from other UARTs or FTP worker */
if (_passing_on || _ftp_on) {
bool is_part;
uint8_t *read_ptr;
uint8_t *write_ptr;
pthread_mutex_lock(&_message_buffer_mutex);
int available = message_buffer_get_ptr((void **)&read_ptr, &is_part);
pthread_mutex_unlock(&_message_buffer_mutex);
if (available > 0) {
// Reconstruct message from buffer
mavlink_message_t msg;
write_ptr = (uint8_t *)&msg;
// Pull a single message from the buffer
size_t read_count = available;
if (read_count > sizeof(mavlink_message_t)) {
read_count = sizeof(mavlink_message_t);
}
memcpy(write_ptr, read_ptr, read_count);
// We hold the mutex until after we complete the second part of the buffer. If we don't
// we may end up breaking the empty slot overflow detection semantics when we mark the
// possibly partial read below.
pthread_mutex_lock(&_message_buffer_mutex);
message_buffer_mark_read(read_count);
/* write second part of buffer if there is some */
if (is_part && read_count < sizeof(mavlink_message_t)) {
write_ptr += read_count;
available = message_buffer_get_ptr((void **)&read_ptr, &is_part);
read_count = sizeof(mavlink_message_t) - read_count;
memcpy(write_ptr, read_ptr, read_count);
message_buffer_mark_read(available);
}
pthread_mutex_unlock(&_message_buffer_mutex);
_mavlink_resend_uart(_channel, &msg);
}
}
/* update TX/RX rates*/
if (t > _bytes_timestamp + 1000000) {
if (_bytes_timestamp != 0) {
float dt = (t - _bytes_timestamp) / 1000.0f;
_rate_tx = _bytes_tx / dt;
_rate_txerr = _bytes_txerr / dt;
_rate_rx = _bytes_rx / dt;
_bytes_tx = 0;
_bytes_txerr = 0;
_bytes_rx = 0;
}
_bytes_timestamp = t;
}
perf_end(_loop_perf);
}
pthread_addr_t UavcanServers::run(pthread_addr_t)
{
prctl(PR_SET_NAME, "uavcan fw srv", 0);
Lock lock(_subnode_mutex);
/*
Copy any firmware bundled in the ROMFS to the appropriate location on the
SD card, unless the user has copied other firmware for that device.
*/
unpackFwFromROMFS(UAVCAN_FIRMWARE_PATH, UAVCAN_ROMFS_FW_PATH);
/* the subscribe call needs to happen in the same thread,
* so not in the constructor */
int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
int param_request_sub = orb_subscribe(ORB_ID(uavcan_parameter_request));
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
/* Set up shared service clients */
_param_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_getset));
_param_opcode_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_opcode));
_param_restartnode_client.setCallback(RestartNodeCallback(this, &UavcanServers::cb_restart));
_enumeration_client.setCallback(EnumerationBeginCallback(this, &UavcanServers::cb_enumeration_begin));
_enumeration_indication_sub.start(EnumerationIndicationCallback(this, &UavcanServers::cb_enumeration_indication));
_enumeration_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_enumeration_getset));
_enumeration_save_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_enumeration_save));
_count_in_progress = _param_in_progress = _param_list_in_progress = _cmd_in_progress = _param_list_all_nodes = false;
memset(_param_counts, 0, sizeof(_param_counts));
_esc_enumeration_active = false;
memset(_esc_enumeration_ids, 0, sizeof(_esc_enumeration_ids));
_esc_enumeration_index = 0;
while (!_subnode_thread_should_exit) {
if (_check_fw == true) {
_check_fw = false;
_node_info_retriever.invalidateAll();
}
const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(10));
if (spin_res < 0) {
warnx("node spin error %i", spin_res);
}
// Check for parameter requests (get/set/list)
bool param_request_ready;
orb_check(param_request_sub, ¶m_request_ready);
if (param_request_ready && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
struct uavcan_parameter_request_s request;
orb_copy(ORB_ID(uavcan_parameter_request), param_request_sub, &request);
if (_param_counts[request.node_id]) {
/*
* We know how many parameters are exposed by this node, so
* process the request.
*/
if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_READ) {
uavcan::protocol::param::GetSet::Request req;
if (request.param_index >= 0) {
req.index = request.param_index;
} else {
req.name = (char*)request.param_id;
}
int call_res = _param_getset_client.call(request.node_id, req);
if (call_res < 0) {
warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res);
} else {
_param_in_progress = true;
_param_index = request.param_index;
}
} else if (request.message_type == MAVLINK_MSG_ID_PARAM_SET) {
uavcan::protocol::param::GetSet::Request req;
if (request.param_index >= 0) {
req.index = request.param_index;
} else {
req.name = (char*)request.param_id;
}
if (request.param_type == MAV_PARAM_TYPE_REAL32) {
req.value.to<uavcan::protocol::param::Value::Tag::real_value>() = request.real_value;
} else if (request.param_type == MAV_PARAM_TYPE_UINT8) {
req.value.to<uavcan::protocol::param::Value::Tag::boolean_value>() = request.int_value;
} else {
req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = request.int_value;
}
// Set the dirty bit for this node
set_node_params_dirty(request.node_id);
int call_res = _param_getset_client.call(request.node_id, req);
if (call_res < 0) {
warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res);
} else {
_param_in_progress = true;
_param_index = request.param_index;
}
} else if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
// This triggers the _param_list_in_progress case below.
_param_index = 0;
_param_list_in_progress = true;
_param_list_node_id = request.node_id;
_param_list_all_nodes = false;
warnx("UAVCAN command bridge: starting component-specific param list");
}
} else if (request.node_id == MAV_COMP_ID_ALL) {
if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
/*
* This triggers the _param_list_in_progress case below,
* but additionally iterates over all active nodes.
*/
_param_index = 0;
_param_list_in_progress = true;
_param_list_node_id = get_next_active_node_id(1);
_param_list_all_nodes = true;
warnx("UAVCAN command bridge: starting global param list with node %hhu", _param_list_node_id);
if (_param_counts[_param_list_node_id] == 0) {
param_count(_param_list_node_id);
}
}
} else {
/*
* Need to know how many parameters this node has before we can
* continue; count them now and then process the request.
*/
param_count(request.node_id);
}
}
// Handle parameter listing index/node ID advancement
if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
if (_param_index >= _param_counts[_param_list_node_id]) {
warnx("UAVCAN command bridge: completed param list for node %hhu", _param_list_node_id);
// Reached the end of the current node's parameter set.
_param_list_in_progress = false;
if (_param_list_all_nodes) {
// We're listing all parameters for all nodes -- get the next node ID
uint8_t next_id = get_next_active_node_id(_param_list_node_id);
if (next_id < 128) {
_param_list_node_id = next_id;
/*
* If there is a next node ID, check if that node's parameters
* have been counted before. If not, do it now.
*/
if (_param_counts[_param_list_node_id] == 0) {
param_count(_param_list_node_id);
}
// Keep on listing.
_param_index = 0;
_param_list_in_progress = true;
warnx("UAVCAN command bridge: started param list for node %hhu", _param_list_node_id);
}
}
}
}
// Check if we're still listing, and need to get the next parameter
if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
// Ready to request the next value -- _param_index is incremented
// after each successful fetch by cb_getset
uavcan::protocol::param::GetSet::Request req;
req.index = _param_index;
int call_res = _param_getset_client.call(_param_list_node_id, req);
if (call_res < 0) {
_param_list_in_progress = false;
warnx("UAVCAN command bridge: couldn't send param list GetSet: %d", call_res);
} else {
_param_in_progress = true;
}
}
// Check for ESC enumeration commands
bool cmd_ready;
orb_check(cmd_sub, &cmd_ready);
if (cmd_ready && !_cmd_in_progress) {
struct vehicle_command_s cmd;
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN) {
int command_id = static_cast<int>(cmd.param1 + 0.5f);
int node_id = static_cast<int>(cmd.param2 + 0.5f);
int call_res;
warnx("UAVCAN command bridge: received UAVCAN command ID %d, node ID %d", command_id, node_id);
switch (command_id) {
case 0:
case 1: {
_esc_enumeration_active = command_id;
_esc_enumeration_index = 0;
_esc_count = 0;
uavcan::protocol::enumeration::Begin::Request req;
// TODO: Incorrect implementation; the parameter name field should be left empty.
// Leaving it as-is to avoid breaking compatibility with non-compliant nodes.
req.parameter_name = "esc_index";
req.timeout_sec = _esc_enumeration_active ? 65535 : 0;
call_res = _enumeration_client.call(get_next_active_node_id(1), req);
if (call_res < 0) {
warnx("UAVCAN ESC enumeration: couldn't send initial Begin request: %d", call_res);
beep(BeepFrequencyError);
} else {
beep(BeepFrequencyGenericIndication);
}
break;
}
default: {
warnx("UAVCAN command bridge: unknown command ID %d", command_id);
break;
}
}
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE) {
int command_id = static_cast<int>(cmd.param1 + 0.5f);
warnx("UAVCAN command bridge: received storage command ID %d", command_id);
switch (command_id) {
case 1: {
// Param save request
int node_id;
node_id = get_next_dirty_node_id(1);
if (node_id < 128) {
_param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE;
param_opcode(node_id);
}
break;
}
case 2: {
// Command is a param erase request -- apply it to all active nodes by setting the dirty bit
_param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_ERASE;
for (int i = 1; i < 128; i = get_next_active_node_id(i)) {
set_node_params_dirty(i);
}
param_opcode(get_next_dirty_node_id(1));
break;
}
}
}
}
// Shut down once armed
// TODO (elsewhere): start up again once disarmed?
bool updated;
orb_check(armed_sub, &updated);
if (updated) {
struct actuator_armed_s armed;
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
if (armed.armed && !armed.lockdown) {
warnx("UAVCAN command bridge: system armed, exiting now.");
break;
}
}
}
_subnode_thread_should_exit = false;
warnx("exiting");
return (pthread_addr_t) 0;
}
