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();
}
/**
* Set serial parameters for the specified serial port.
*
* @param serial Previously initialized serial port structure.
* @param[in] baudrate The baudrate to set.
* @param[in] bits The number of data bits to use (5, 6, 7 or 8).
* @param[in] parity The parity setting to use (0 = none, 1 = even, 2 = odd).
* @param[in] stopbits The number of stop bits to use (1 or 2).
* @param[in] flowcontrol The flow control settings to use (0 = none,
* 1 = RTS/CTS, 2 = XON/XOFF).
* @param[in] rts Status of RTS line (0 or 1; required by some interfaces).
* @param[in] dtr Status of DTR line (0 or 1; required by some interfaces).
*
* @retval SR_OK Success.
* @retval SR_ERR Failure.
*
* @private
*/
SR_PRIV int serial_set_params(struct sr_serial_dev_inst *serial, int baudrate,
int bits, int parity, int stopbits,
int flowcontrol, int rts, int dtr)
{
int ret;
char *error;
struct sp_port_config *config;
if (!serial) {
sr_dbg("Invalid serial port.");
return SR_ERR;
}
if (!serial->data) {
sr_dbg("Cannot configure unopened serial port %s.", serial->port);
return SR_ERR;
}
sr_spew("Setting serial parameters on port %s.", serial->port);
sp_new_config(&config);
sp_set_config_baudrate(config, baudrate);
sp_set_config_bits(config, bits);
switch (parity) {
case 0:
sp_set_config_parity(config, SP_PARITY_NONE);
break;
case 1:
sp_set_config_parity(config, SP_PARITY_EVEN);
break;
case 2:
sp_set_config_parity(config, SP_PARITY_ODD);
break;
default:
return SR_ERR_ARG;
}
sp_set_config_stopbits(config, stopbits);
sp_set_config_rts(config, flowcontrol == 1 ? SP_RTS_FLOW_CONTROL : rts);
sp_set_config_cts(config, flowcontrol == 1 ? SP_CTS_FLOW_CONTROL : SP_CTS_IGNORE);
sp_set_config_dtr(config, dtr);
sp_set_config_dsr(config, SP_DSR_IGNORE);
sp_set_config_xon_xoff(config, flowcontrol == 2 ? SP_XONXOFF_INOUT : SP_XONXOFF_DISABLED);
ret = sp_set_config(serial->data, config);
sp_free_config(config);
switch (ret) {
case SP_ERR_ARG:
sr_err("Invalid arguments for setting serial port parameters.");
return SR_ERR_ARG;
case SP_ERR_FAIL:
error = sp_last_error_message();
sr_err("Error setting serial port parameters (%d): %s.",
sp_last_error_code(), error);
sp_free_error_message(error);
return SR_ERR;
}
return SR_OK;
}
int get_serial_adapter(struct sp_port **my_port, struct sp_port_config *my_port_config) {
int ret;
// Get pointer to port
char port_name[] = "/dev/ttyAMA0";
ret = sp_get_port_by_name(port_name, my_port);
if (ret != SP_OK) {
printf("Failed to get port: %s. Exiting now.\n",port_name);
return 0;
}
// Open port
ret = sp_open((*my_port), SP_MODE_READ_WRITE);
if (ret == SP_OK) {
printf("Successfully opened port!\n");
} else {
printf("Failed to open port :(\n");
sp_free_port(*my_port);
return 0;
}
// Initialize port config
ret = sp_new_config(&my_port_config);
if (ret != SP_OK) {
printf("Failed to initialize port config.\n");
sp_free_port(*my_port);
return 0;
}
return 1;
}
int serial_set_params(struct serial_device_t *serial,
int baudrate,
int bits,
int parity,
int stopbits,
int flowcontrol,
int rts,
int dtr)
{
if (!serial) {
log_error("serial_set_params: invalid serial port device.");
return -DEVICE_CONN_ERROR;
}
if (!serial->data) {
log_error("serial_set_params: cannot configure unopened serial port %s.",
serial->port);
return -DEVICE_CONN_ERROR;
}
log_debug("serial_set_params: setting serial parameters on port %s.",
serial->port);
struct sp_port_config *config;
sp_new_config(&config);
sp_set_config_baudrate(config, baudrate);
sp_set_config_bits(config, bits);
sp_set_config_parity(config, parity);
sp_set_config_stopbits(config, stopbits);
sp_set_config_rts(config, flowcontrol == 1 ? SP_RTS_FLOW_CONTROL : rts);
sp_set_config_cts(config, flowcontrol == 1 ? SP_CTS_FLOW_CONTROL : SP_CTS_IGNORE);
sp_set_config_dtr(config, dtr);
sp_set_config_dsr(config, SP_DSR_IGNORE);
sp_set_config_xon_xoff(config, flowcontrol == 2 ? SP_XONXOFF_INOUT : SP_XONXOFF_DISABLED);
int ret = sp_set_config(serial->data, config);
sp_free_config(config);
char *error;
switch (ret) {
case SP_ERR_ARG:
log_error("serial_set_params: invalid serial port parameters.");
return -DEVICE_CONF_ERROR;
case SP_ERR_FAIL:
error = sp_last_error_message();
log_error("serial_set_params: error setting serial port parameters (%d): %s.",
sp_last_error_code(), error);
sp_free_error_message(error);
return -DEVICE_CONN_ERROR;
}
return NO_ERROR;
}
/** @private */
SR_PRIV int serial_timeout(struct sr_serial_dev_inst *port, int num_bytes)
{
struct sp_port_config *config;
int timeout_ms, bits, baud, tmp;
/* Default to 1s. */
timeout_ms = 1000;
if (sp_new_config(&config) < 0)
return timeout_ms;
bits = baud = 0;
do {
if (sp_get_config(port->data, config) < 0)
break;
/* Start bit. */
bits = 1;
if (sp_get_config_bits(config, &tmp) < 0)
break;
bits += tmp;
if (sp_get_config_stopbits(config, &tmp) < 0)
break;
bits += tmp;
if (sp_get_config_baudrate(config, &tmp) < 0)
break;
baud = tmp;
} while (FALSE);
if (bits && baud) {
/* Throw in 10ms for misc OS overhead. */
timeout_ms = 10;
timeout_ms += ((1000.0 / baud) * bits) * num_bytes;
}
sp_free_config(config);
return timeout_ms;
}
int Arduino_connect(const char *serialPortName, int baudrate)
{
struct sp_port_config *serialPortConfiguration;
enum sp_return r;
if(serialPort) sp_free_port(serialPort);
// Find the specified serial port
if(sp_get_port_by_name(serialPortName, &serialPort) != SP_OK)
{
fprintf(stderr, "Cannot find the serial port\n");
return 0;
}
// Open the serial port
if(sp_open(serialPort, SP_MODE_READ_WRITE) != SP_OK)
{
fprintf(stderr, "Cannot open the serial port\n");
return 0;
}
// Configure the serial port
sp_new_config(&serialPortConfiguration);
sp_set_config_baudrate(serialPortConfiguration, 9600);
sp_set_config_parity(serialPortConfiguration, SP_PARITY_NONE);
sp_set_config_bits(serialPortConfiguration, 8);
sp_set_config_stopbits(serialPortConfiguration, 1);
sp_set_config_flowcontrol(serialPortConfiguration, SP_FLOWCONTROL_NONE);
if(sp_set_config(serialPort, serialPortConfiguration) != SP_OK)
{
fprintf(stderr, "Cannot configure the serial port\n");
return 0;
}
sp_free_config(serialPortConfiguration);
return 1;
}
int navilink_open_sp_port(struct sp_port* port, NavilinkDevice* device)
{
enum sp_return result = sp_open(port, SP_MODE_READ_WRITE);
if (result != SP_OK) {
CATCH_LIBSERIAL_ERROR(device);
goto error_cleanup_port;
}
struct sp_port_config* config = NULL;
result = sp_new_config(&config);
if (result != SP_OK) {
CATCH_LIBSERIAL_ERROR(device);
goto error_clean_config;
}
// Set the config
sp_set_baudrate(port, 115200);
sp_set_bits(port, 8);
sp_set_parity(port, SP_PARITY_NONE);
sp_set_stopbits(port, 1);
sp_set_flowcontrol(port, SP_FLOWCONTROL_NONE);
sp_set_config(port, config);
// Allocate events buffer
struct sp_event_set* event_set = NULL;
sp_new_event_set(&event_set);
sp_add_port_events(event_set, port, SP_EVENT_TX_READY);
//Wait for the port to be ready
result = sp_wait(event_set, 5000);
if (result != SP_OK) {
CATCH_LIBSERIAL_ERROR(device);
goto error_clean_event_set;
}
device->serial_port = port;
device->event_set = event_set;
//Check that this is a Navilink device
int res = navilink_check_protocol(device);
if (res < 0) {
goto error_clean_event_set;
}
// Retrieve the informations about the device
res = navilink_query_information(device, &device->informations);
if (res < 0) {
goto error_clean_event_set;
}
// Retrieve the firmware version
res = navilink_query_firmware_version(device, &device->firmware_version);
if (res < 0) {
goto error_clean_event_set;
}
return 0;
error_clean_event_set:
sp_free_event_set(event_set);
error_clean_config:
sp_free_config(config);
error_cleanup_port:
sp_free_port(port);
return -1;
}
int main() {
printf("Started!\n");
/*
// Initialize GPIO
if (gpioInitialise() < 0) {
printf("pigpio initialization failed\n");
} else {
printf("pigpio initialization succeeded\n");
}
//gpioTerminate();
// Setup GPIO pin 18 and send rest signal
#define GPIO_IMU_RST 18
#define GPIO_LEVEL_HIGH 1
#define GPIO_LEVEL_LOW 0
if (gpioSetMode(GPIO_IMU_RST, PI_OUTPUT) < 0) {
printf("GPIO 18 setup failed\n");
gpioTerminate();
} else {
printf("GPIO 18 setup succeeded\n");
}
if (gpioWrite(GPIO_IMU_RST, GPIO_LEVEL_HIGH) < 0) {
printf("GPIO RST write high failed\n");
gpioTerminate();
} else {
printf("GPIO RST write high succeeded\n");
}
// Sleep 650 ms to allow BNO055 to fully reset
time_sleep(0.65);
*/
// - Get serial port adapter and config object
int ret;
struct sp_port *serial_adapter;
struct sp_port_config *serial_config;
ret = sp_new_config(&serial_config);
ret = get_serial_adapter(&serial_adapter, serial_config);
if (ret != 1) {
printf("Failed to get serial adapter :(\n");
sp_free_port(serial_adapter);
}
Point3D measurement_vector;
ret = read_linear_acceleration_vector(serial_adapter, &measurement_vector);
if (ret == 1) {
printf("Successfully read linear acceleration.\n");
printf("Measurement - x: %f - y: %f - z: %f\n",measurement_vector.x,measurement_vector.y,measurement_vector.z);
} else {
printf("Failed to read linear acceleration.\n");
}
// - Measure average sensor read time over N reads
int n, N = 50;
clock_t start_time = clock(), time_diff;
for (n = 0; n<N; n++) {
ret = sleep(1);
ret = read_linear_acceleration_vector(serial_adapter, &measurement_vector);
if (ret != 1) {
printf("Failed to read linear acceleration in turn %d.\n",n);
}
}
time_diff = clock() - start_time;
float msec = (float)time_diff * 1000 / CLOCKS_PER_SEC;
printf("Time per read (averaged over %d reads): %f (ms)\n",N,msec/N);
// - Clean up program
gpioTerminate();
sp_free_port(serial_adapter);
printf("Reached end!\n");
return 0;
}
