int
main(int argc, char **argv)
{
char *uart_name = (char*)"/dev/ttyUSB0";
int baudrate = 57600;
parse_commandline(argc, argv, uart_name, baudrate);
Serial_Port my_serial(uart_name, baudrate);
LPmy_serial = &my_serial;
Autopilot_Interface autopilot_interface(LPmy_serial);
LPautopilot_interface = &autopilot_interface;
LPmy_serial->start();
LPautopilot_interface->start();
while(1);
LPautopilot_interface->stop();
LPmy_serial->stop();
return 0;
}
// ------------------------------------------------------------------------------
// TOP
// ------------------------------------------------------------------------------
int
top (int argc, char **argv)
{
// --------------------------------------------------------------------------
// PARSE THE COMMANDS
// --------------------------------------------------------------------------
// Default input arguments
char *uart_name = (char*)"/dev/ttyUSB0";
int baudrate = 57600;
// do the parse, will throw an int if it fails
parse_commandline(argc, argv, uart_name, baudrate);
// --------------------------------------------------------------------------
// PORT and THREAD STARTUP
// --------------------------------------------------------------------------
/*
* Instantiate a serial port object
*
* This object handles the opening and closing of the offboard computer's
* serial port over which it will communicate to an autopilot. It has
* methods to read and write a mavlink_message_t object. To help with read
* and write in the context of pthreading, it gaurds port operations with a
* pthread mutex lock.
*
*/
Serial_Port serial_port(uart_name, baudrate);
/*
* Instantiate an autopilot interface object
*
* This starts two threads for read and write over MAVlink. The read thread
* listens for any MAVlink message and pushes it to the current_messages
* attribute. The write thread at the moment only streams a position target
* in the local NED frame (mavlink_set_position_target_local_ned_t), which
* is changed by using the method update_setpoint(). Sending these messages
* are only half the requirement to get response from the autopilot, a signal
* to enter "offboard_control" mode is sent by using the enable_offboard_control()
* method. Signal the exit of this mode with disable_offboard_control(). It's
* important that one way or another this program signals offboard mode exit,
* otherwise the vehicle will go into failsafe.
*
*/
Autopilot_Interface autopilot_interface(&serial_port);
/*
* Setup interrupt signal handler
*
* Responds to early exits signaled with Ctrl-C. The handler will command
* to exit offboard mode if required, and close threads and the port.
* The handler in this example needs references to the above objects.
*
*/
serial_port_quit = &serial_port;
autopilot_interface_quit = &autopilot_interface;
signal(SIGINT,quit_handler);
/*
* Start the port and autopilot_interface
* This is where the port is opened, and read and write threads are started.
*/
serial_port.start();
autopilot_interface.start();
// --------------------------------------------------------------------------
// RUN COMMANDS
// --------------------------------------------------------------------------
/*
* Now we can implement the algorithm we want on top of the autopilot interface
*/
commands(autopilot_interface);
// --------------------------------------------------------------------------
// THREAD and PORT SHUTDOWN
// --------------------------------------------------------------------------
/*
* Now that we are done we can stop the threads and close the port
*/
autopilot_interface.stop();
serial_port.stop();
// --------------------------------------------------------------------------
// DONE
// --------------------------------------------------------------------------
// woot!
return 0;
}
// ------------------------------------------------------------------------------
// TOP
// ------------------------------------------------------------------------------
int
top (int argc, char **argv)
{
int i;
struct timespec wr_period,rd_period;
// --------------------------------------------------------------------------
// PARSE THE COMMANDS
// --------------------------------------------------------------------------
// Default input arguments
char *uart_name = (char*)"/dev/ttyUSB0";
int baudrate = 921600;
char *qgc_ip = (char*)"127.0.0.1";
char *mtl_ip = (char*)"10.30.3.217";
//char *mtl_ip = (char*)"127.0.0.1";
unsigned int r_port_qgc = 14551;
unsigned int w_port_qgc = 14550;
unsigned int r_port_mtl = 49000;
unsigned int w_port_mtl = 49006;
// do the parse, will throw an int if it fails
parse_commandline(argc, argv, uart_name, baudrate);
// --------------------------------------------------------------------------
// PORT and THREAD STARTUP
// --------------------------------------------------------------------------
/*
* Instantiate a serial port object
*
* This object handles the opening and closing of the offboard computer's
* serial port over which it will communicate to an autopilot. It has
* methods to read and write a mavlink_message_t object. To help with read
* and write in the context of pthreading, it gaurds port operations with a
* pthread mutex lock.
*
*/
Serial_Port serial_port(uart_name, baudrate);
/*
* Instantiate a UDP port object
*
* This object handles the opening and closing of the offboard computer's
* serial port over which it will communicate to Ground Station.
*
*/
Udp_Port udp_port_QGC(qgc_ip,r_port_qgc,w_port_qgc);
Udp_Port udp_port_MTL(mtl_ip,r_port_mtl,w_port_mtl);
/*
* Instantiate an autopilot interface object
*
* This starts two threads for read and write over MAVlink. The read thread
* listens for any MAVlink message and pushes it to the current_messages
* attribute. The write thread at the moment only streams a position target
* in the local NED frame (mavlink_set_position_target_local_ned_t), which
* is changed by using the method update_setpoint(). Sending these messages
* are only half the requirement to get response from the autopilot, a signal
* to enter "offboard_control" mode is sent by using the enable_offboard_control()
* method. Signal the exit of this mode with disable_offboard_control(). It's
* important that one way or another this program signals offboard mode exit,
* otherwise the vehicle will go into failsafe.
*
*/
Autopilot_Interface autopilot_interface(&serial_port, &udp_port_MTL, &udp_port_QGC);
wr_period.tv_sec = 0;
wr_period.tv_nsec = 4 * 1000000;
rd_period.tv_sec = 0;
rd_period.tv_nsec = 4 * 1000000;
autopilot_interface.setWritePeriod(wr_period);
autopilot_interface.setReadPeriod(rd_period);
/*
* Setup interrupt signal handler
*
* Responds to early exits signaled with Ctrl-C. The handler will command
* to exit offboard mode if required, and close threads and the port.
* The handler in this example needs references to the above objects.
*
*/
serial_port_quit = &serial_port;
autopilot_interface_quit = &autopilot_interface;
signal(SIGINT,quit_handler);
/*
* Start the port and autopilot_interface
* This is where the port is opened, and read and write threads are started.
*/
serial_port.start();
autopilot_interface.start();
// --------------------------------------------------------------------------
// RUN COMMANDS
// --------------------------------------------------------------------------
/*
* Now we can implement the algorithm we want on top of the autopilot interface
*/
//commands(autopilot_interface);
// --------------------------------------------------------------------------
// SET HIL SIMULATION
// --------------------------------------------------------------------------
autopilot_interface.start_hil();
// --------------------------------------------------------------------------
// THREAD and PORT SHUTDOWN
// --------------------------------------------------------------------------
/*
* Now that we are done we can stop the threads and close the port
*/
for(;;)
{
sleep(20);
}
// --------------------------------------------------------------------------
// TERMINATING
// --------------------------------------------------------------------------
//sleep(4);
//autopilot_interface.stop_hil();
autopilot_interface.stop();
serial_port.stop();
// woot!
return 0;
}
