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();
}
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;
}
/**
* Open the specified serial port.
*
* @param serial Previously initialized serial port structure.
* @param flags Flags to use when opening the serial port. Possible flags
* include SERIAL_RDWR, SERIAL_RDONLY, SERIAL_NONBLOCK.
*
* If the serial structure contains a serialcomm string, it will be
* passed to serial_set_paramstr() after the port is opened.
*
* @return SR_OK on success, SR_ERR on failure.
*/
SR_PRIV int serial_open(struct sr_serial_dev_inst *serial, int flags)
{
int ret;
char *error;
int sp_flags = 0;
if (!serial) {
sr_dbg("Invalid serial port.");
return SR_ERR;
}
sr_spew("Opening serial port '%s' (flags %d).", serial->port, flags);
sp_get_port_by_name(serial->port, &serial->data);
if (flags & SERIAL_RDWR)
sp_flags = (SP_MODE_READ | SP_MODE_WRITE);
else if (flags & SERIAL_RDONLY)
sp_flags = SP_MODE_READ;
serial->nonblocking = (flags & SERIAL_NONBLOCK) ? 1 : 0;
ret = sp_open(serial->data, sp_flags);
switch (ret) {
case SP_ERR_ARG:
sr_err("Attempt to open serial port with invalid parameters.");
return SR_ERR_ARG;
case SP_ERR_FAIL:
error = sp_last_error_message();
sr_err("Error opening port: %s.", error);
sp_free_error_message(error);
return SR_ERR;
}
#ifndef _WIN32
sp_get_port_handle(serial->data, &serial->fd);
#endif
if (serial->serialcomm)
return serial_set_paramstr(serial, serial->serialcomm);
else
return SR_OK;
}
int navilink_open_device_from_name(const char* serial_port_name, NavilinkDevice* device)
{
assert(serial_port_name != NULL);
assert(serial_port_name != NULL);
struct sp_port* port = NULL;
enum sp_return result = sp_get_port_by_name(serial_port_name, &port);
if (result < 0) {
goto serial_error;
}
result = navilink_open_sp_port(port, device);
if (result < 0) {
goto serial_error;
}
return 0;
serial_error:
CATCH_LIBSERIAL_ERROR(device);
return -1;
}
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 serial_open(struct serial_device_t *serial, int flags)
{
if (!serial) {
log_error("serial_open: invalid serial port device.");
return -DEVICE_CONN_ERROR;
}
log_info("serial_open: opening serial port '%s' (flags %d).",
serial->port, flags);
sp_get_port_by_name(serial->port, &serial->data);
int ret = sp_open(serial->data, flags);
char *error;
switch (ret) {
case SP_ERR_ARG:
log_error("serial_open: invalid serial port parameters.");
return -DEVICE_CONF_ERROR;
case SP_ERR_FAIL:
error = sp_last_error_message();
log_error("serial_open: error opening serial port (%d): %s.",
sp_last_error_code(), error);
sp_free_error_message(error);
return -DEVICE_CONN_ERROR;
}
return NO_ERROR;
}
/**
* Open the specified serial port.
*
* @param serial Previously initialized serial port structure.
* @param flags Flags to use when opening the serial port. Possible flags
* include SERIAL_RDWR, SERIAL_RDONLY, SERIAL_NONBLOCK.
*
* If the serial structure contains a serialcomm string, it will be
* passed to serial_set_paramstr() after the port is opened.
*
* @return SR_OK on success, SR_ERR on failure.
*/
SR_PRIV int serial_open(struct sr_serial_dev_inst *serial, int flags)
{
int ret;
char *error;
if (!serial) {
sr_dbg("Invalid serial port.");
return SR_ERR;
}
sr_spew("Opening serial port '%s' (flags %d).", serial->port, flags);
sp_get_port_by_name(serial->port, &serial->data);
ret = sp_open(serial->data, flags);
switch (ret) {
case SP_ERR_ARG:
sr_err("Attempt to open serial port with invalid parameters.");
return SR_ERR_ARG;
case SP_ERR_FAIL:
error = sp_last_error_message();
sr_err("Error opening port: %s.", error);
sp_free_error_message(error);
return SR_ERR;
}
#ifndef _WIN32
serial->fd = serial->data->fd;
#endif
if (serial->serialcomm)
return serial_set_paramstr(serial, serial->serialcomm);
else
return SR_OK;
}
int main(int argc, char **argv)
{
int opt;
int result = 0;
sbp_state_t s;
if (argc <= 1) {
usage(argv[0]);
exit(EXIT_FAILURE);
}
while ((opt = getopt(argc, argv, "p:")) != -1) {
switch (opt) {
case 'p':
serial_port_name = (char *)calloc(strlen(optarg) + 1, sizeof(char));
if (!serial_port_name) {
fprintf(stderr, "Cannot allocate memory!\n");
exit(EXIT_FAILURE);
}
strcpy(serial_port_name, optarg);
break;
case 'h':
usage(argv[0]);
exit(EXIT_FAILURE);
}
}
if (!serial_port_name) {
fprintf(stderr, "Please supply the serial port path where the Piksi is " \
"connected!\n");
exit(EXIT_FAILURE);
}
result = sp_get_port_by_name(serial_port_name, &piksi_port);
if (result != SP_OK) {
fprintf(stderr, "Cannot find provided serial port!\n");
exit(EXIT_FAILURE);
}
result = sp_open(piksi_port, SP_MODE_READ);
if (result != SP_OK) {
fprintf(stderr, "Cannot open %s for reading!\n", serial_port_name);
exit(EXIT_FAILURE);
}
setup_port();
sbp_state_init(&s);
#if 0
/* Register a node and callback, and associate them with a specific message ID. */
sbp_register_callback(&s, SBP_MSG_HEARTBEAT, &hearbeat_callback, NULL,
&heartbeat_callback_node);
sbp_register_callback(&s, SBP_MSG_GPS_TIME, &sbp_gps_time_callback,
NULL, &gps_time_node);
sbp_register_callback(&s, SBP_MSG_POS_LLH, &sbp_pos_llh_callback,
NULL, &pos_llh_node);
sbp_register_callback(&s, SBP_MSG_BASELINE_NED, &sbp_baseline_ned_callback,
NULL, &baseline_ned_node);
sbp_register_callback(&s, SBP_MSG_VEL_NED, &sbp_vel_ned_callback,
NULL, &vel_ned_node);
sbp_register_callback(&s, SBP_MSG_DOPS, &sbp_dops_callback,
NULL, &dops_node);
/* Use sprintf to right justify floating point prints. */
char rj[30];
/* Only want 1 call to SH_SendString as semihosting is quite slow.
* sprintf everything to this array and then print using array. */
char str[1000];
int str_i;
#endif
while (1) {
// sbp_process(&s, &piksi_port_read);
sleep(1);
fprintf(stdout, "hello\n");
#if 0
str_i = 0;
memset(str, 0, sizeof(str));
str_i += sprintf(str + str_i, "\n\n\n\n");
/* Print GPS time. */
str_i += sprintf(str + str_i, "GPS Time:\n");
str_i += sprintf(str + str_i, "\tWeek\t\t: %6d\n", (int)gps_time.wn);
sprintf(rj, "%6.2f", ((float)gps_time.tow) / 1e3);
str_i += sprintf(str + str_i, "\tSeconds\t: %9s\n", rj);
str_i += sprintf(str + str_i, "\n");
/* Print absolute position. */
str_i += sprintf(str + str_i, "Absolute Position:\n");
sprintf(rj, "%4.10lf", pos_llh.lat);
str_i += sprintf(str + str_i, "\tLatitude\t: %17s\n", rj);
sprintf(rj, "%4.10lf", pos_llh.lon);
str_i += sprintf(str + str_i, "\tLongitude\t: %17s\n", rj);
sprintf(rj, "%4.10lf", pos_llh.height);
str_i += sprintf(str + str_i, "\tHeight\t: %17s\n", rj);
str_i += sprintf(str + str_i, "\tSatellites\t: %02d\n", pos_llh.n_sats);
str_i += sprintf(str + str_i, "\n");
/* Print NED (North/East/Down) baseline (position vector from base to rover). */
str_i += sprintf(str + str_i, "Baseline (mm):\n");
str_i += sprintf(str + str_i, "\tNorth\t\t: %6d\n", (int)baseline_ned.n);
str_i += sprintf(str + str_i, "\tEast\t\t: %6d\n", (int)baseline_ned.e);
str_i += sprintf(str + str_i, "\tDown\t\t: %6d\n", (int)baseline_ned.d);
str_i += sprintf(str + str_i, "\n");
/* Print NED velocity. */
str_i += sprintf(str + str_i, "Velocity (mm/s):\n");
str_i += sprintf(str + str_i, "\tNorth\t\t: %6d\n", (int)vel_ned.n);
str_i += sprintf(str + str_i, "\tEast\t\t: %6d\n", (int)vel_ned.e);
str_i += sprintf(str + str_i, "\tDown\t\t: %6d\n", (int)vel_ned.d);
str_i += sprintf(str + str_i, "\n");
/* Print Dilution of Precision metrics. */
str_i += sprintf(str + str_i, "Dilution of Precision:\n");
sprintf(rj, "%4.2f", ((float)dops.gdop / 100));
str_i += sprintf(str + str_i, "\tGDOP\t\t: %7s\n", rj);
sprintf(rj, "%4.2f", ((float)dops.hdop / 100));
str_i += sprintf(str + str_i, "\tHDOP\t\t: %7s\n", rj);
sprintf(rj, "%4.2f", ((float)dops.pdop / 100));
str_i += sprintf(str + str_i, "\tPDOP\t\t: %7s\n", rj);
sprintf(rj, "%4.2f", ((float)dops.tdop / 100));
str_i += sprintf(str + str_i, "\tTDOP\t\t: %7s\n", rj);
sprintf(rj, "%4.2f", ((float)dops.vdop / 100));
str_i += sprintf(str + str_i, "\tVDOP\t\t: %7s\n", rj);
str_i += sprintf(str + str_i, "\n");
#endif
}
result = sp_close(piksi_port);
if (result != SP_OK) {
fprintf(stderr, "Cannot close %s properly!\n", serial_port_name);
}
sp_free_port(piksi_port);
free(serial_port_name);
return 0;
}
