int sendCmdProcess(const struct _cmdInput *in ,struct _cmdOutput *out) {
int i = 0;
for (i = 0; i < LEN_SEND_CMDS; i++) {
if (g_send_cmds[i].cmd == in->cmd) {
CMD_DEBUG("send a cmd:%s\r\n", g_send_cmds[i].name);
if (g_send_cmds[i].process) {
return g_send_cmds[i].process(out);
}
}
}
CMD_DEBUG("\r\n CMD:%04x is not pair any command!\r\n",in->cmd);
return -1;
}
/**
* command process function
*/
int cmdProcess(struct _cmdInput *in, struct _cmdOutput *out,
struct _protocolData *data) {
int i = 0;
for (i = 0; i < LEN_CMDS; i++) {
if (g_cmds[i].cmd == in->cmd) {
CMD_DEBUG("get a cmd:%s\r\n", g_cmds[i].name);
if (g_cmds[i].process) {
return g_cmds[i].process(in, out, data);
}
break;
}
}
CMD_DEBUG("\r\n CMD:%04x is not pair any command!\r\n", in->cmd);
return -1;
}
int MavlinkCommandSender::handle_vehicle_command(const struct vehicle_command_s &command, mavlink_channel_t channel)
{
lock();
CMD_DEBUG("getting vehicle command with timestamp %" PRIu64 ", channel: %d", command.timestamp, channel);
mavlink_command_long_t msg = {};
msg.target_system = command.target_system;
msg.target_component = command.target_component;
msg.command = command.command;
msg.confirmation = command.confirmation;
msg.param1 = command.param1;
msg.param2 = command.param2;
msg.param3 = command.param3;
msg.param4 = command.param4;
msg.param5 = command.param5;
msg.param6 = command.param6;
msg.param7 = command.param7;
mavlink_msg_command_long_send_struct(channel, &msg);
bool already_existing = false;
_commands.reset_to_start();
while (command_item_t *item = _commands.get_next()) {
if (item->timestamp_us == command.timestamp) {
// We should activate the channel by setting num_sent_per_channel from -1 to 0.
item->num_sent_per_channel[channel] = 0;
CMD_DEBUG("already existing");
already_existing = true;
break;
}
}
if (!already_existing) {
command_item_t new_item;
new_item.command = msg;
new_item.timestamp_us = command.timestamp;
new_item.num_sent_per_channel[channel] = 0;
new_item.last_time_sent_us = hrt_absolute_time();
_commands.put(new_item);
}
unlock();
return 0;
}
int doUserCommand(const char* cmd,struct _cmdOutput *out) {
int i = 0;
for (i = 0; i < LEN_SEND_CMDS; i++) {
if ( strstr(cmd,g_send_cmds[i].name) ) {
out->cmd = g_send_cmds[i].cmd;
// printf("***get a cmd:%s ****\r\n", g_send_cmds[i].name);
if (g_send_cmds[i].process) {
return g_send_cmds[i].process(out);
}
else {
CMD_DEBUG("find cmd,but no funcs!!!\r\n");
}
}
}
CMD_DEBUG("\r\nUSER CMD:%s is not pair any command!\r\n",cmd);
//printAllCmd();
return -1;
}
// Debug Output
void EqlCmd_DebugDump (EqlCmdPtr self)
{
if (self) {
int i;
CMD_DEBUG(
"\n"
"EQLCMD:\n"
" adapter = %s\n"
" subtype = %s\n"
" action = %s\n",
String_Get(self->adapter),
String_Get(self->subtype),
String_Get(self->action));
for (i = 0; i < self->messages->items; i++)
CMD_DEBUG(
" messages[%d] = %s\n",
i, self->messages->list[i]);
for (i = 0; i < self->parameters->items; i++)
CMD_DEBUG(
" parameters[%d] = %s\n",
i, self->parameters->list[i]);
for (i = 0; i < self->values->items; i++)
CMD_DEBUG(
" values[%d] = %s (%s)\n",
i, self->datatypes->list[i], self->values->list[i]);
for (i = 0; i < self->options->items; i++)
CMD_DEBUG(
" options[%d] = %s\n",
i, self->options->list[i]);
for (i = 0; i < self->results->size; i++) {
EsifDataPtr data = &self->results->elements[i];
char *strdata = EsifData_ToString(data);
if (strdata) {
CMD_DEBUG(
" results[%d] = %s (buf_len=%d data_len=%d)\n"
"%s\n",
i, esif_data_type_str(self->results->elements[i].type),
self->results->elements[i].buf_len, self->results->elements[i].data_len,
strdata);
esif_ccb_free(strdata);
}
}
CMD_DEBUG("\n");
}
}
/* show help of command */
static void cmd_mem_help(char *cmd_name, void *extobj)
{
VALID_EXTOBJ_NORTN(extobj);
if (cmd_name == NULL) {
/* cmd_name not valid */
return;
}
CMD_DEBUG("Usage: %s [OPTION]... [ADDR] [LENGTH] [VALUE]\r\n"
"Read or Write data in the memory\r\n"
" -h/H/? Show the help information\r\n"
" -d <address> <length> Dump memory\r\n"
" -f <address> <length> <value> Fill memory\r\n"
" -e <address> <value>... Edit memory\r\n"
"Examples: \r\n"
" mem -f <addr> <length> <value> Fill <value> to memory from <addr> to <addr> + <length>\r\n"
" mem -d 0x0 1000 Dump memory from 0x0, length 1000\r\n"
" mem -h Show the help information\r\n", cmd_name);
error_exit:
return;
}
void MavlinkCommandSender::handle_mavlink_command_ack(const mavlink_command_ack_t &ack,
uint8_t from_sysid, uint8_t from_compid)
{
CMD_DEBUG("handling result %d for command %d: %d from %d",
ack.result, ack.command, from_sysid, from_compid);
lock();
_commands.reset_to_start();
while (command_item_t *item = _commands.get_next()) {
// Check if the incoming ack matches any of the commands that we have sent.
if (item->command.command == ack.command &&
from_sysid == item->command.target_system &&
from_compid == item->command.target_component) {
// Drop it anyway because the command seems to have arrived at the destination, even if we
// receive IN_PROGRESS because we trust that it will be handled after that.
_commands.drop_current();
break;
}
}
unlock();
}
static int cmd_mem(int argc, char **argv, void *extobj)
{
int32_t ercd = E_OK;
int32_t opt;
long ram_address, length, value;
char *ptr;
VALID_EXTOBJ(extobj, -1);
NTSHELL_IO_GET(extobj);
if(argc < 2) {
ercd = E_SYS;
CMD_DEBUG("command error!\r\n");
CMD_DEBUG("Try '%s -h' for more information\r\n", argv[0]);
goto error_exit;
}
opterr = 0;
optind = 1;
while ((opt=getopt(argc, argv, "d:f:e:hH?")) != -1) {
switch (opt) {
case 'h':
case '?':
case 'H':
cmd_mem_help(argv[0], extobj);
goto error_exit;
break;
case 'd':
if (!xatoi(&argv[2], &ram_address) || !xatoi(&argv[3], &length)){
ercd = E_SYS;
CMD_DEBUG("Parameter error!\r\n");
goto error_exit;
}
for (ptr = (char*)ram_address; length >= 16 ; ptr += 16, length -= 16)
cmds_put_dump(ptr, (uint32_t)ptr, 16, 1, extobj);
if (length) cmds_put_dump((uint8_t*)ptr, (uint32_t)ptr, length, 1, extobj);
break;
case 'f':
if (!xatoi(&argv[2], &ram_address) || !xatoi(&argv[3], &length) || !xatoi(&argv[4], &value)){
ercd = E_SYS;
CMD_DEBUG("Parameter error!\r\n");
goto error_exit;
}
while (length--) {
*(uint8_t*)ram_address = (uint8_t)value;
ram_address++;
}
break;
case 'e':
if (!xatoi(&argv[2], &ram_address) || !xatoi(&argv[3], &value)) {
ercd = E_SYS;
CMD_DEBUG("Parameter error!\r\n");
goto error_exit;
}
length = argc - 3;
while(length--) {
*(uint8_t*)ram_address = (uint8_t)value;
ram_address++;
if(!xatoi(&argv[argc - length], &value)) break;
}
break;
default:
CMD_DEBUG("%s: unrecognized option:%c\r\n", argv[0], opt);
CMD_DEBUG("Try '%s -h' for more information\r\n",argv[0]);
break;
}
}
return E_OK;
error_exit:
return ercd;
}
static void printCmd(const struct _sendCmdType *p) {
CMD_DEBUG("name:%s cmd:%04X \r\n",p->name,p->cmd);
}
void MavlinkCommandSender::check_timeout(mavlink_channel_t channel)
{
lock();
_commands.reset_to_start();
while (command_item_t *item = _commands.get_next()) {
if (hrt_elapsed_time(&item->last_time_sent_us) <= TIMEOUT_US) {
// We keep waiting for the timeout.
continue;
}
// The goal of this is to retry from all channels. Therefore, we keep
// track of the retry count for each channel.
//
// When the first channel does a retry, the timeout is reset.
// (e.g. all channel have done 2 retries, then channel 0 is called
// and does retry number 3, and also resets the timeout timestamp).
// First, we need to determine what the current max and min retry level
// are because we can only level up, if all have caught up.
// If num_sent_per_channel is at -1, the channel is inactive.
int8_t max_sent = 0;
int8_t min_sent = INT8_MAX;
for (unsigned i = 0; i < MAX_MAVLINK_CHANNEL; ++i) {
if (item->num_sent_per_channel[i] > max_sent) {
max_sent = item->num_sent_per_channel[i];
}
if ((item->num_sent_per_channel[i] != -1) &&
(item->num_sent_per_channel[i] < min_sent)) {
min_sent = item->num_sent_per_channel[i];
}
}
if (item->num_sent_per_channel[channel] < max_sent) {
// We are behind and need to do a retransmission.
mavlink_msg_command_long_send_struct(channel, &item->command);
item->num_sent_per_channel[channel]++;
CMD_DEBUG("%p timeout (behind), retries: %d/%d, channel: %d",
item, item->num_sent_per_channel[channel], max_sent, channel);
} else if (item->num_sent_per_channel[channel] == max_sent &&
min_sent == max_sent) {
// If the next retry would be above the needed retries anyway, we can
// drop the item, and continue with other items.
if (item->num_sent_per_channel[channel] + 1 > RETRIES) {
_commands.drop_current();
CMD_DEBUG("%p, timeout dropped", item);
continue;
}
// We are the first of a new retransmission series.
mavlink_msg_command_long_send_struct(channel, &item->command);
item->num_sent_per_channel[channel]++;
// Therefore, we are the ones setting the timestamp of this retry round.
item->last_time_sent_us = hrt_absolute_time();
CMD_DEBUG("%p timeout (first), retries: %d/%d, channel: %d",
item, item->num_sent_per_channel[channel], max_sent, channel);
} else {
// We are already ahead, so this should not happen.
// If it ever does, just ignore it. It will timeout eventually.
continue;
}
}
unlock();
}
// Retrieve a single value from a DataVault
eEsifError DataVault_GetValue (
DataVaultPtr self,
EsifDataPtr path,
EsifDataPtr value
)
{
eEsifError rc = ESIF_E_NOT_FOUND;
DataCacheEntryPtr keypair = NULL;
if (!self)
return ESIF_E_PARAMETER_IS_NULL;
// TODO: Locking
// Debug: Dump Entire Contents of DataVault if path="*"
if (strcmp((esif_string)path->buf_ptr, "*") == 0) {
UInt32 context = 0;
EsifDataPtr nameSpace = EsifData_CreateAs(ESIF_DATA_STRING, esif_ccb_strdup(self->name), ESIFAUTOLEN, ESIFAUTOLEN);
EsifDataPtr key = EsifData_CreateAs(ESIF_DATA_AUTO, NULL, ESIF_DATA_ALLOCATE, 0);
EsifDataPtr value = EsifData_CreateAs(ESIF_DATA_AUTO, NULL, ESIF_DATA_ALLOCATE, 0);
#ifdef _DEBUG
// Dump DataCache if Debug Mode
UInt32 i;
for (i = 0; i < self->cache->size; i++) {
DataCacheEntryPtr keypair = &self->cache->elements[i];
CMD_DEBUG("\n"
"id=%d\n"
"key=%s\n"
"flags=%08X\n"
"value.type = %s (%d)\n"
"value.buf_ptr = %p %s\n"
"value.buf_len = %d\n"
"value.data_len= %d\n",
i,
(char*)keypair->key.buf_ptr,
keypair->flags,
esif_data_type_str(keypair->value.type), keypair->value.type,
keypair->value.buf_ptr,
((keypair->flags & (ESIF_SERVICE_CONFIG_FILELINK | ESIF_SERVICE_CONFIG_REGLINK)) ? (esif_string)keypair->value.buf_ptr : ""),
keypair->value.buf_len,
keypair->value.data_len
);
}
if (i > 0) {
CMD_DEBUG("*****************\n");
}
#endif
// Iterate the Contents of the Data Cache
if ((rc = EsifConfigFindFirst(nameSpace, key, value, &context)) == ESIF_OK) {
do {
StringPtr valuestr = EsifData_ToString(value);
CMD_DEBUG("\n"
"EsifFind%s(\"%s\",%d):\n"
" path = %s\n"
" value= %s%s%s\n"
" type = %s, len=%d\n",
(context <= 1 ? "First" : "Next"), (char*)nameSpace->buf_ptr, context - 1,
(char*)key->buf_ptr,
(value->type == ESIF_DATA_STRING ? "\"" : ""), (valuestr ? valuestr : ""), (value->type == ESIF_DATA_STRING ? "\"" : ""),
esif_data_type_str(value->type), value->data_len
);
esif_ccb_free(valuestr);
EsifData_Set(key, ESIF_DATA_AUTO, NULL, ESIF_DATA_ALLOCATE, 0);
EsifData_Set(value, ESIF_DATA_AUTO, NULL, ESIF_DATA_ALLOCATE, 0);
} while ((rc = EsifConfigFindNext(nameSpace, key, value, &context)) == ESIF_OK);
if (rc == ESIF_E_NOT_FOUND) {
rc = ESIF_OK;
}
}
EsifData_Destroy(nameSpace);
EsifData_Destroy(key);
EsifData_Destroy(value);
return ESIF_OK;
}
// Write to Log before retrieval if AUTO
if (value->type == ESIF_DATA_AUTO || value->buf_len == ESIF_DATA_ALLOCATE) {
DataVault_WriteLog(self, "AUTO", (esif_string)(self->name), path, 0, value);
}
keypair = DataCache_GetValue(self->cache, (esif_string)path->buf_ptr);
if (NULL != keypair) {
UInt32 data_len = keypair->value.data_len;
void *buf_ptr = keypair->value.buf_ptr;
// File Redirect?
if (keypair->flags & ESIF_SERVICE_CONFIG_FILELINK) {
if (DataVault_ReadFile(self, (esif_string)buf_ptr, &buf_ptr, &data_len) != ESIF_OK) {
value->data_len = 0;
if (value->type == ESIF_DATA_AUTO) {
value->type = keypair->value.type;
}
if (value->buf_len == ESIF_DATA_ALLOCATE) {
value->buf_len = 0;
value->buf_ptr = 0;
}
return ESIF_OK; // return OK and a blank buffer if file not found/error
}
}
#ifdef ESIF_ATTR_OS_WINDOWS
// Registry Redirect?
else if (keypair->flags & ESIF_SERVICE_CONFIG_REGLINK) {
if (DataVault_ReadRegistry(self, (esif_string)buf_ptr, &buf_ptr, &data_len) != ESIF_OK) {
value->data_len = 0;
if (value->type == ESIF_DATA_AUTO) {
value->type = keypair->value.type;
}
if (value->buf_len == ESIF_DATA_ALLOCATE) {
value->buf_len = 0;
value->buf_ptr = 0;
}
return ESIF_OK; // return OK and a blank buffer if Registry value not found/error
}
}
#endif
// Match Found. Verify Data Type matches unless AUTO
if (value->type != keypair->value.type && value->type != ESIF_DATA_AUTO) {
rc = ESIF_E_UNSUPPORTED_RESULT_DATA_TYPE; // TODO: ESIF_E_INVALID_DATA_TYPE
}
// Verify Data Buffer is large enough unless Auto-Allocate
else if (value->buf_len < data_len && value->buf_len != ESIF_DATA_ALLOCATE) {
value->buf_len = data_len;
rc = ESIF_E_NEED_LARGER_BUFFER;
}
// Return pointer to static contents if this is a static vault
else if ((self->flags & ESIF_SERVICE_CONFIG_STATIC) &&
(value->type == ESIF_DATA_AUTO) &&
(value->buf_len == ESIF_DATA_ALLOCATE)) {
value->type = keypair->value.type;
value->data_len = data_len;
value->buf_len = 0; // Caller MUST NOT Free!
value->buf_ptr = buf_ptr;
rc = ESIF_OK;
} else {
// Set Data Type and Auto-Allocate Buffer?
if (value->type == ESIF_DATA_AUTO) {
value->type = keypair->value.type;
}
if (ESIF_DATA_ALLOCATE == value->buf_len) {
value->buf_len = data_len;
value->buf_ptr = esif_ccb_malloc(value->buf_len);
if (!value->buf_ptr) {
return ESIF_E_NO_MEMORY;
}
}
// Read from file if NOCACHE option
if ((keypair->flags & ESIF_SERVICE_CONFIG_NOCACHE) && keypair->value.buf_len == 0) {
size_t offset = (size_t)keypair->value.buf_ptr;
if (DataVault_ReadBlock(self, (esif_string)value->buf_ptr, data_len, offset) != ESIF_OK) {
data_len = 0;
return ESIF_E_NOT_FOUND;
}
// Decrypt?
if (keypair->flags & ESIF_SERVICE_CONFIG_ENCRYPT) {
UInt32 byte;
for (byte = 0; byte < data_len; byte++)
((UInt8*)(value->buf_ptr))[byte] = ~((UInt8*)(value->buf_ptr))[byte];
}
} else {
esif_ccb_memcpy(value->buf_ptr, buf_ptr, data_len);
}
value->data_len = data_len;
rc = ESIF_OK;
}
}
// Write to Log
DataVault_WriteLog(self, "GET", (esif_string)self->name, path, 0, value);
return rc;
}
