uart_input::uart_input()
{
conf = Global.uart_conf;
if (sp_get_port_by_name(conf.port.c_str(), &port) != SP_OK)
throw std::runtime_error("uart: cannot find specified port");
if (sp_open(port, SP_MODE_READ_WRITE) != SP_OK)
throw std::runtime_error("uart: cannot open port");
sp_port_config *config;
if (sp_new_config(&config) != SP_OK ||
sp_set_config_baudrate(config, conf.baud) != SP_OK ||
sp_set_config_flowcontrol(config, SP_FLOWCONTROL_NONE) != SP_OK ||
sp_set_config_bits(config, 8) != SP_OK ||
sp_set_config_stopbits(config, 1) != SP_OK ||
sp_set_config_parity(config, SP_PARITY_NONE) != SP_OK ||
sp_set_config(port, config) != SP_OK)
throw std::runtime_error("uart: cannot set config");
sp_free_config(config);
if (sp_flush(port, SP_BUF_BOTH) != SP_OK)
throw std::runtime_error("uart: cannot flush");
old_packet.fill(0);
last_update = std::chrono::high_resolution_clock::now();
}
/**
* Flush serial port buffers.
*
* @param serial Previously initialized serial port structure.
*
* @retval SR_OK Success.
* @retval SR_ERR Failure.
*
* @private
*/
SR_PRIV int serial_flush(struct sr_serial_dev_inst *serial)
{
int ret;
char *error;
if (!serial) {
sr_dbg("Invalid serial port.");
return SR_ERR;
}
if (!serial->data) {
sr_dbg("Cannot flush unopened serial port %s.", serial->port);
return SR_ERR;
}
sr_spew("Flushing serial port %s.", serial->port);
ret = sp_flush(serial->data, SP_BUF_BOTH);
switch (ret) {
case SP_ERR_ARG:
sr_err("Attempt to flush an invalid serial port.");
return SR_ERR_ARG;
case SP_ERR_FAIL:
error = sp_last_error_message();
sr_err("Error flushing port (%d): %s.",
sp_last_error_code(), error);
sp_free_error_message(error);
return SR_ERR;
}
return SR_OK;
}
void FlotillaDock::disconnect(void){
if (state == Disconnected) return;
state = Disconnected;
int x;
for (x = 0; x < MAX_CHANNELS; x++){
if (module[x].state == ModuleConnected){
module[x].disconnect();
queue_module_event(x);
}
}
sp_flush(port, SP_BUF_OUTPUT);
sp_flush(port, SP_BUF_INPUT);
sp_close(port);
sp_free_port(port);
std::ostringstream msg;
msg << GetTimestamp() << "Dock Disconnected" << std::endl; // , serial " << serial << std::endl;
std::cout << msg.str();
}
// TODO: encapsulate robust serial processing, including retries and timeouts
int read_sensor_vector(struct sp_port *my_port, char address, Point3D *result) {
int i, ret, expected_response_size;
expected_response_size = (sizeof(result)-1)*2;
// Initialize intermediate serial response buffer
char response[expected_response_size];
float temp_result[3];
// Construct standard UART read packet (described in BNO055 data sheet: https://cdn-shop.adafruit.com/datasheets/BST_BNO055_DS000_12.pdf)
char command[] = {0xAA, 0x01, address & 0xFF, expected_response_size & 0xFF};
// First, flush the input buffer
ret = sp_flush(my_port,SP_BUF_INPUT);
// Second, send data read command to BNO055
ret = serial_send(my_port, command, sizeof(command), response);
if (ret != expected_response_size) {
printf("Linear acceleration read returned wrong size response. Expected %d, Got %d.\n",expected_response_size,ret);
return 0;
}
// Verify register read success
if (response[0] != 0xBB) {
printf("Register read error.\n");
return 0;
}
// - Unpack serial response into result array
// - First two bytes are metadata, unpack remaining N bytes of data
for (i = 2; i<sizeof(result)+2-1; i++) {
temp_result[i] = ((response[i*2+1] << 8) | response[i*2]) & 0xFFFF;
if (temp_result[i] > 32767) {
temp_result[i] = temp_result[i] - 65536;
}
}
// Set fields of result
result->x = temp_result[0];
result->y = temp_result[1];
result->z = temp_result[2];
return 1;
}
