mirror of
https://github.com/OpenSolo/OpenSolo.git
synced 2025-04-29 22:24:32 +02:00
372 lines
10 KiB
C++
372 lines
10 KiB
C++
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <iostream>
|
|
#include <iomanip>
|
|
#include <stdint.h>
|
|
#include <fcntl.h>
|
|
#include <unistd.h>
|
|
#include <sys/types.h>
|
|
#include <sys/time.h>
|
|
#include <netdb.h>
|
|
#include <time.h>
|
|
#include <termios.h>
|
|
#include <sys/errno.h>
|
|
#include <sys/ioctl.h>
|
|
#include <list>
|
|
#include <sys/mman.h>
|
|
#include <fstream>
|
|
#include <poll.h>
|
|
#include "util.h"
|
|
#include "../mavlink/c_library/ardupilotmega/mavlink.h"
|
|
#include "../mavlink/c_library/common/mavlink.h"
|
|
#include <sstream>
|
|
#include <signal.h>
|
|
#include <pthread.h>
|
|
#include <semaphore.h>
|
|
#include <dirent.h>
|
|
|
|
using namespace std;
|
|
|
|
/***********************************************************************
|
|
Serial port name
|
|
***********************************************************************/
|
|
string serialPortName;
|
|
|
|
/***********************************************************************
|
|
File descriptors
|
|
***********************************************************************/
|
|
int serial_fd;
|
|
|
|
/***********************************************************************
|
|
Threading variables
|
|
***********************************************************************/
|
|
pthread_mutex_t mutex_msg;
|
|
pthread_mutex_t mutex_msgtype;
|
|
mavlink_message_t latest_msg;
|
|
sem_t sem_msg;
|
|
int msg_type = 0;
|
|
|
|
/***********************************************************************
|
|
Serial tx/rx buffer size
|
|
***********************************************************************/
|
|
#define BUFSIZE 256
|
|
|
|
/***********************************************************************
|
|
Function: int serial_setup(int baud)
|
|
|
|
Description: The serial port initialization function. This function
|
|
initializes the serial port over which DSM data is sent to
|
|
the pixhawk. A return of 0 indicates an error.
|
|
***********************************************************************/
|
|
int serial_setup(int baud)
|
|
{
|
|
struct termios options;
|
|
|
|
serial_fd = open(serialPortName.c_str(), O_RDWR | O_NOCTTY);
|
|
|
|
if(serial_fd < 0)
|
|
{
|
|
cerr << "Unable to open serial port " << serialPortName.c_str() << endl;
|
|
return 0;
|
|
}
|
|
|
|
tcflush(serial_fd, TCIOFLUSH);
|
|
|
|
//Configure port for 8N1 transmission
|
|
tcgetattr(serial_fd, &options); //Gets the current options for the port
|
|
//Set the output baud rate
|
|
switch(baud)
|
|
{
|
|
case 57600: cfsetspeed(&options, B57600); break;
|
|
case 115200: cfsetspeed(&options, B115200); break;
|
|
case 1500000: cfsetspeed(&options, B1500000); break;
|
|
default:
|
|
cerr << "Unsupported baud rate" << endl;
|
|
return 0;
|
|
}
|
|
options.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
|
|
options.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST);
|
|
options.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
|
|
options.c_cflag &= ~(CSIZE | PARENB);
|
|
options.c_cflag |= CS8;
|
|
options.c_cflag |= CRTSCTS;
|
|
options.c_cc[VMIN] = 17;
|
|
options.c_cc[VTIME] = 0;
|
|
|
|
tcsetattr(serial_fd, TCSANOW, &options); //Set the new options for the port "NOW"
|
|
|
|
//sleep(1);
|
|
tcflush(serial_fd, TCIOFLUSH);
|
|
|
|
cout << "Opened serial port " << serialPortName.c_str() << endl;
|
|
|
|
return 1;
|
|
}
|
|
|
|
void connectUSB(void)
|
|
{
|
|
system("echo 19 >> /sys/class/gpio/export");
|
|
system("echo 21 >> /sys/class/gpio/export");
|
|
system("echo out >> /sys/class/gpio/gpio19/direction");
|
|
system("echo out >> /sys/class/gpio/gpio21/direction");
|
|
system("echo 1 >> /sys/class/gpio/gpio21/value");
|
|
system("echo 0 >> /sys/class/gpio/gpio19/value");
|
|
|
|
}
|
|
|
|
void disconnectUSB(void)
|
|
{
|
|
system("echo 0 >> /sys/class/gpio/gpio21/value");
|
|
system("echo 0 >> /sys/class/gpio/gpio19/value");
|
|
system("echo 19 >> /sys/class/gpio/unexport");
|
|
system("echo 21 >> /sys/class/gpio/unexport");
|
|
}
|
|
|
|
/***********************************************************************
|
|
Function: int mavlink_task()
|
|
|
|
Description: Thread task to pull mavlink data from the serial port
|
|
and dump it into a mutex-protected mavlink message.
|
|
|
|
When log data begins streaming, dumps log data into the
|
|
log file on the filesystem.
|
|
***********************************************************************/
|
|
void *mavlink_task(void*)
|
|
{
|
|
mavlink_message_t _msg;
|
|
mavlink_status_t mavlink_status;
|
|
int read_len;
|
|
int i;
|
|
char buf[BUFSIZE];
|
|
int msgtolookfor;
|
|
|
|
while(1)
|
|
{
|
|
read_len = read(serial_fd, buf, BUFSIZE);
|
|
|
|
if (read_len < 0)
|
|
{
|
|
if (errno != EAGAIN)
|
|
cerr << "Read err: " << errno << endl;
|
|
}
|
|
|
|
if(read_len > 0)
|
|
{
|
|
for(i=0; i<read_len; ++i)
|
|
{
|
|
if(mavlink_parse_char(MAVLINK_COMM_0, buf[i], &_msg, &mavlink_status))
|
|
{
|
|
//If this is the type of message we're waiting for, put it on
|
|
//the latest_msg or handle its log data.
|
|
pthread_mutex_lock(&mutex_msgtype);
|
|
msgtolookfor = msg_type;
|
|
pthread_mutex_unlock(&mutex_msgtype);
|
|
|
|
if(_msg.msgid == msgtolookfor)
|
|
{
|
|
pthread_mutex_lock(&mutex_msg);
|
|
memcpy(&latest_msg, &_msg, sizeof(mavlink_message_t));
|
|
pthread_mutex_unlock(&mutex_msg);
|
|
sem_post(&sem_msg);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
pthread_exit(NULL);
|
|
}
|
|
|
|
/***********************************************************************
|
|
Function: int wait_for_message()
|
|
|
|
Description: Blocks until a particular mavlink message type is received.
|
|
A return value of 0 indicates succes, -1 for a timeout.
|
|
Setting timeout_s to 0 blocks indefinitely.
|
|
***********************************************************************/
|
|
int wait_for_message(int _msg_type, mavlink_message_t *msg, int timeout_s)
|
|
{
|
|
struct timespec timeout;
|
|
int ret;
|
|
|
|
//Tell the mavlink thread which message to look for
|
|
pthread_mutex_lock(&mutex_msgtype);
|
|
msg_type = _msg_type;
|
|
pthread_mutex_unlock(&mutex_msgtype);
|
|
|
|
clock_gettime(CLOCK_REALTIME, &timeout);
|
|
timeout.tv_sec += timeout_s;
|
|
|
|
if(timeout_s == 0)
|
|
sem_wait(&sem_msg);
|
|
else
|
|
{
|
|
ret = sem_timedwait(&sem_msg, &timeout);
|
|
|
|
if(ret < 0)
|
|
{
|
|
if (errno == ETIMEDOUT)
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
pthread_mutex_lock(&mutex_msg);
|
|
memcpy(msg, &latest_msg, sizeof(mavlink_message_t));
|
|
pthread_mutex_unlock(&mutex_msg);
|
|
|
|
return 0;
|
|
}
|
|
|
|
int open_USB_serial(void)
|
|
{
|
|
char usbdev_string[256] = "usb-3D_Robotics_PX4_FMU";
|
|
struct dirent *direntry;
|
|
bool foundDevice = false;
|
|
DIR *dir;
|
|
|
|
connectUSB();
|
|
sleep(1);
|
|
|
|
dir = opendir("/dev/serial/by-id/");
|
|
if (dir == NULL) {
|
|
cerr << "open /dev/serial/by-id failed" << endl;
|
|
return -1;
|
|
}
|
|
|
|
while((direntry = readdir(dir)))
|
|
{
|
|
if(!strncmp(direntry->d_name, usbdev_string, 23))
|
|
{
|
|
foundDevice = true;
|
|
strcpy(usbdev_string,direntry->d_name);
|
|
break;
|
|
}
|
|
}
|
|
|
|
if(!foundDevice)
|
|
{
|
|
cerr << "Unable to find USB device" << endl;
|
|
return -1;
|
|
}
|
|
|
|
closedir(dir);
|
|
|
|
//Serial port setup
|
|
serialPortName = "/dev/serial/by-id/";
|
|
serialPortName.append(usbdev_string);
|
|
if(!serial_setup(115200))
|
|
{
|
|
cerr << "Unable to initialize the USB serial port" << endl;
|
|
return -1;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/***********************************************************************
|
|
Function: void request_baudrate()
|
|
|
|
Description: Request a particular log file from the pixhawk.
|
|
***********************************************************************/
|
|
void request_baudrate(void)
|
|
{
|
|
char buf[BUFSIZE];
|
|
mavlink_message_t msg;
|
|
int msg_len;
|
|
float baud;
|
|
int tries;
|
|
bool gotParam=false;
|
|
|
|
mavlink_msg_param_request_read_pack(1, 1, &msg, 1, 1, "SERIAL1_BAUD", -1);
|
|
msg_len = mavlink_msg_to_send_buffer((uint8_t*)buf,&msg);
|
|
|
|
if(write(serial_fd, buf, msg_len) != msg_len)
|
|
{
|
|
cerr << "Serial port write error." << endl;
|
|
disconnectUSB();
|
|
exit(1);
|
|
}
|
|
|
|
for(tries=0; tries<3; ++tries)
|
|
{
|
|
cout << "Requesting SERIAL1_BAUD..." << endl;
|
|
|
|
if(wait_for_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg, 3) >= 0)
|
|
{
|
|
baud = mavlink_msg_param_value_get_param_value(&msg);
|
|
cout << "Got param value " << baud << endl;
|
|
disconnectUSB();
|
|
exit(0);
|
|
}
|
|
}
|
|
|
|
if(!gotParam)
|
|
{
|
|
cout << "Unable to get SERIAL1_BAUD_VALUE" << endl;
|
|
disconnectUSB();
|
|
exit(1);
|
|
}
|
|
}
|
|
|
|
/**********************************************************************
|
|
Function: int main(void)
|
|
|
|
Description: The main function. Initializes and runs the serial and
|
|
UDP threads.
|
|
***********************************************************************/
|
|
int main(int argc, char *argv[])
|
|
{
|
|
int baudrate;
|
|
pthread_t mavlink_ctx;
|
|
mavlink_message_t msg;
|
|
|
|
//Cout cleanup
|
|
std::cout.precision(2);
|
|
std::cout.setf( ios::fixed, ios::floatfield );
|
|
|
|
cout << "Pixhawk baudrate checker" << endl;
|
|
|
|
//Serial port setup
|
|
serialPortName = "/dev/ttymxc1";
|
|
baudrate=115200;//57600;
|
|
if(!serial_setup(baudrate))
|
|
{
|
|
cerr << "Unable to initialize the serial send" << endl;
|
|
return -1;
|
|
}
|
|
|
|
//Threading initialization
|
|
pthread_mutex_init(&mutex_msg, NULL);
|
|
sem_init(&sem_msg, 0, 0);
|
|
|
|
//Start the mavlink rx thread
|
|
pthread_create(&mavlink_ctx, NULL, mavlink_task, NULL);
|
|
|
|
cout << "Waiting for a hearbeat..." << endl;
|
|
|
|
//Wait for a heartbeat on the telem port
|
|
if(wait_for_message(MAVLINK_MSG_ID_HEARTBEAT, &msg, 5) < 0)
|
|
{
|
|
cout << "No heartbeat received, requesting baudrate from USB" << endl;
|
|
pthread_cancel(mavlink_ctx);
|
|
close(serial_fd);
|
|
if (open_USB_serial() < 0)
|
|
return -1;
|
|
}
|
|
else
|
|
{
|
|
cout << "Got a heartbeat, exiting" << endl;
|
|
return 0;
|
|
}
|
|
|
|
pthread_create(&mavlink_ctx, NULL, mavlink_task, NULL);
|
|
|
|
//Send a param request to the pixhawk
|
|
request_baudrate();
|
|
|
|
//Wait for the mavlink thread to end
|
|
pthread_join(mavlink_ctx, NULL);
|
|
|
|
return 0;
|
|
}
|