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; }