int navilink_write_device(NavilinkDevice* device, uint8_t packet_type, const uint8_t* payload, size_t payload_length)
{
assert(device != NULL);
uint8_t* buff = (void*)(&device->buffer[0]);
memset(buff, 0, NAVILINK_MAX_PACKET_SIZE);
int bytes = navilink_create_packet(buff, packet_type, payload, payload_length);
if (bytes < 0) {
navilink_set_current_error(device, bytes, NULL);
return -1;
}
#if (__DEBUG_CMD)
printf("WRITE:");
DumpHex(buff, bytes);
#endif
enum sp_return result = sp_blocking_write(device->serial_port, &device->buffer[0], bytes, 1000);
if (result < 0) {
goto manage_serial_error;
}
result = sp_drain(device->serial_port);
if (result < 0) {
goto manage_serial_error;
}
return 0;
manage_serial_error:
CATCH_LIBSERIAL_ERROR(device);
return -1;
}
static int _serial_write(struct serial_device_t *serial,
const void *buf, size_t count,
int nonblocking, unsigned int timeout_ms)
{
if (!serial) {
log_error("serial_write: invalid serial port device.");
return -DEVICE_CONN_ERROR;
}
if (!serial->data) {
log_error("serial_write: cannot use unopened serial port %s.", serial->port);
return -DEVICE_CONN_ERROR;
}
int ret;
if (nonblocking) {
ret = sp_nonblocking_write(serial->data, buf, count);
} else {
ret = sp_blocking_write(serial->data, buf, count, timeout_ms);
}
char *error;
switch (ret) {
case SP_ERR_ARG:
log_error("serial_write: invalid serial port parameters.");
return -DEVICE_CONF_ERROR;
case SP_ERR_FAIL:
error = sp_last_error_message();
log_error("serial_write: write error (%d): %s.", sp_last_error_code(), error);
sp_free_error_message(error);
return -DEVICE_CONN_ERROR;
}
return ret;
}
uart_input::~uart_input()
{
std::array<std::uint8_t, 31> buffer = { 0 };
sp_blocking_write(port, (void*)buffer.data(), buffer.size(), 0);
sp_drain(port);
sp_close(port);
sp_free_port(port);
}
void FlotillaDock::cmd_enumerate(void){
std::this_thread::sleep_for(std::chrono::microseconds(100000));
sp_blocking_write(port, "e\r", 2, 0);
std::ostringstream msg;
msg << GetTimestamp() << "Enumerating Dock" << std::endl; // , serial " << serial << "..." << std::endl;
std::cout << msg.str();
}
void FlotillaDock::tick(){
if (state != Connected) return;
int channel_index;
mutex.lock();
while (command_queue.size() > 0){
sp_blocking_write(port, command_queue.front().c_str(), command_queue.front().length(), 0);
command_queue.pop();
std::this_thread::sleep_for(std::chrono::microseconds(100000));
}
mutex.unlock();
for (channel_index = 0; channel_index < MAX_CHANNELS; channel_index++){
if (module[channel_index].state != ModuleConnected) continue;
std::string update;
if (module[channel_index].get_next_update(update)){
std::ostringstream stream;
stream << "s " << (channel_index + 1) << " " << update;
update = stream.str();
#ifdef DEBUG_TRANSPORT
std::ostringstream msg;
msg << GetTimestamp() << "Sending to dock: " << update << std::endl;
std::cout << msg.str();
#endif
while (sp_output_waiting(port) > 0) {};
sp_blocking_write(port, update.c_str(), update.length(), 0);
sp_blocking_write(port, "\r", 1, 0);
std::this_thread::sleep_for(std::chrono::microseconds(10000));
}
}
while (sp_input_waiting(port) > 0){
process_command(sp_readline(port));
}
}
/**
* Write a number of bytes to the specified serial port.
*
* @param serial Previously initialized serial port structure.
* @param buf Buffer containing the bytes to write.
* @param count Number of bytes to write.
*
* @return The number of bytes written, or a negative error code upon failure.
*/
SR_PRIV int serial_write(struct sr_serial_dev_inst *serial,
const void *buf, size_t count)
{
ssize_t ret;
char *error;
if (!serial) {
sr_dbg("Invalid serial port.");
return SR_ERR;
}
if (serial->fd == -1) {
sr_dbg("Cannot use unopened serial port %s (fd %d).",
serial->port, serial->fd);
return SR_ERR;
}
if (serial->nonblocking)
ret = sp_nonblocking_write(serial->data, buf, count);
else
ret = sp_blocking_write(serial->data, buf, count, 0);
switch (ret) {
case SP_ERR_ARG:
sr_err("Attempted serial port write with invalid arguments.");
return SR_ERR_ARG;
case SP_ERR_FAIL:
error = sp_last_error_message();
sr_err("Write error: %s.", error);
sp_free_error_message(error);
return SR_ERR;
}
sr_spew("Wrote %d/%d bytes (fd %d).", ret, count, serial->fd);
return ret;
}
static int _serial_write(struct sr_serial_dev_inst *serial,
const void *buf, size_t count, int nonblocking, unsigned int timeout_ms)
{
ssize_t ret;
char *error;
if (!serial) {
sr_dbg("Invalid serial port.");
return SR_ERR;
}
if (!serial->data) {
sr_dbg("Cannot use unopened serial port %s.", serial->port);
return SR_ERR;
}
if (nonblocking)
ret = sp_nonblocking_write(serial->data, buf, count);
else
ret = sp_blocking_write(serial->data, buf, count, timeout_ms);
switch (ret) {
case SP_ERR_ARG:
sr_err("Attempted serial port write with invalid arguments.");
return SR_ERR_ARG;
case SP_ERR_FAIL:
error = sp_last_error_message();
sr_err("Write error (%d): %s.", sp_last_error_code(), error);
sp_free_error_message(error);
return SR_ERR;
}
sr_spew("Wrote %zd/%zu bytes.", ret, count);
return ret;
}
int serial_send(struct sp_port *my_port, const void *command, size_t write_bytes_count, void *response) {
int ret, read_count=6;
if (DO_BLOCKING_READ_WRITE) {
unsigned int timeout = READ_WRITE_TIMEOUT_SEC*1000;
ret = sp_blocking_write(my_port, command, write_bytes_count, timeout);
if (ret <= 0) {
printf("Failed to write serial bytes.\n");
return 0;
}
ret = sp_blocking_read(my_port, response, (size_t)read_count, timeout);
} else {
int fd;
ret = sp_get_port_handle(my_port,&fd);
if (ret != SP_OK) {
printf("Failed to get port handle for nonblocking read/write.\n");
}
fd_set fds;
FD_ZERO(&fds);
FD_SET(fd,&fds);
struct timespec ts;
ts.tv_sec = READ_WRITE_TIMEOUT_SEC;
ret = pselect(fd+1, NULL, &fds, NULL, &ts, NULL);
if (ret < 0 && errno != EINTR) {
printf("Error in pselect for write.\n");
return 0;
} else if (ret == 0) {
printf("Timeout while waiting to write.\n");
return 0;
}
ret = sp_nonblocking_write(my_port, command, write_bytes_count);
FD_ZERO(&fds);
FD_SET(fd,&fds);
ts.tv_sec = READ_WRITE_TIMEOUT_SEC;
ret = pselect(fd+1, &fds, NULL, NULL, &ts, NULL);
if (ret < 0 && errno != EINTR) {
printf("Error in pselect for write.\n");
return 0;
} else if (ret == 0) {
printf("Timeout while waiting to write.\n");
return 0;
}
ret = sp_nonblocking_read(my_port, response, (size_t)read_count);
}
if (ret < read_count) {
printf("Successful write to serial, but failed on serial read.\n");
return 0;
} else {
return ret;
}
}
bool FlotillaDock::get_version_info(){
int x;
version = "";
//serial = "";
user = "";
name = "";
/*
I've added a small delay here to counter a bug where the dock seems to hate receiving
subsequent commands and doesn't respond to the second command at all.
Can be tested simply by sending v\rv\r in one transaction, this *should* return the version
information twice, but the dock is dropping/ignoring the second request?
Update: Fixed the dock in many ways to sideline module commands using an alternate parsing method
however, rapid subsequent system commands like v ( version ), d ( debug ) will still make for
an uphappy dock!
*/
while (sp_output_waiting(port) > 0);
std::this_thread::sleep_for(std::chrono::microseconds(100000));
sp_blocking_write(port, "v\r", 2, 0);
if (!sp_wait_for(port, "# Flotilla ready to set sail..")) {
return false;
}
std::ostringstream msg;
msg << GetTimestamp() << "Dock Connected" << std::endl;
std::cout << msg.str();
msg.str(""); msg.clear();
for (x = 0; x < 4; x++){
std::string line = sp_readline(port);
std::string::size_type position;
if (line.length() > 0 && line.at(0) == '#'){
if ((position = line.find("Version: ")) != std::string::npos){
version = line.substr(position + 9);
std::ostringstream report;
report << GetTimestamp() << "Version: " << version << std::endl;
std::cout << report.str();
}
if ((position = line.find("Serial: ")) != std::string::npos){
serial = line.substr(position + 8);
std::ostringstream report;
report << GetTimestamp() << "Serial: " << serial << std::endl;
std::cout << report.str();
}
if ((position = line.find("User: "******"User: "******"Dock: ")) != std::string::npos){
name = line.substr(position + 6);
std::ostringstream report;
report << GetTimestamp() << "Name: " << name << std::endl;
std::cout << report.str();
}
}
line.clear();
}
/*
The serial number is a hexadecimal representation of 11 bytes,
so a valid serial string will *always* be 22 characters long.
*/
if (version.length() > 0 && serial.length() == 22){
return true;
}
return false;
}
