00001 /* 00002 * AscTec Autopilot Interface 00003 * Copyright (C) 2010, CCNY Robotics Lab 00004 * Ivan Dryanovski <ivan.dryanovski@gmail.com> 00005 * William Morris <morris@ee.ccny.cuny.edu> 00006 * http://robotics.ccny.cuny.edu 00007 * 00008 * This program is free software: you can redistribute it and/or modify 00009 * it under the terms of the GNU General Public License as published by 00010 * the Free Software Foundation, either version 3 of the License, or 00011 * (at your option) any later version. 00012 * 00013 * This program is distributed in the hope that it will be useful, 00014 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00015 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00016 * GNU General Public License for more details. 00017 * 00018 * You should have received a copy of the GNU General Public License 00019 * along with this program. If not, see <http://www.gnu.org/licenses/>. 00020 */ 00021 00022 #ifndef ASCTEC_AUTOPILOT_AUTOPILOT_H 00023 #define ASCTEC_AUTOPILOT_AUTOPILOT_H 00024 00025 #include <stdio.h> 00026 #include <sys/termios.h> 00027 #include <sys/ioctl.h> 00028 #include <cstring> 00029 #include <unistd.h> 00030 #include <cstdlib> 00031 #include <time.h> 00032 #include <errno.h> 00033 #include <bitset> 00034 00035 #include <ros/ros.h> 00036 #include <diagnostic_updater/diagnostic_updater.h> 00037 #include <asctec_msgs/common.h> // for namespace and topic names 00038 00039 #include "asctec_autopilot/crc16.h" 00040 #include "asctec_autopilot/telemetry.h" 00041 #include "asctec_autopilot/serialinterface.h" 00042 00043 //const std::string rawdata_namespace_ = "asctec"; 00044 00045 namespace asctec 00046 { 00047 class AutoPilot 00048 { 00049 private: 00050 00051 ros::Timer timer_; 00052 ros::NodeHandle nh_; 00053 ros::NodeHandle nh_private_; 00054 00055 double freq_; 00056 std::string port_; 00057 int speed_; 00058 bool enable_LL_STATUS_; 00059 int interval_LL_STATUS_; 00060 int offset_LL_STATUS_; 00061 bool enable_IMU_RAWDATA_; 00062 int interval_IMU_RAWDATA_; 00063 int offset_IMU_RAWDATA_; 00064 bool enable_IMU_CALCDATA_; 00065 int interval_IMU_CALCDATA_; 00066 int offset_IMU_CALCDATA_; 00067 bool enable_RC_DATA_; 00068 int interval_RC_DATA_; 00069 int offset_RC_DATA_; 00070 bool enable_CONTROLLER_OUTPUT_; 00071 int interval_CONTROLLER_OUTPUT_; 00072 int offset_CONTROLLER_OUTPUT_; 00073 bool enable_GPS_DATA_; 00074 int interval_GPS_DATA_; 00075 int offset_GPS_DATA_; 00076 bool enable_GPS_DATA_ADVANCED_; 00077 int interval_GPS_DATA_ADVANCED_; 00078 int offset_GPS_DATA_ADVANCED_; 00079 bool enable_CONTROL_; 00080 int interval_CONTROL_; 00081 int offset_CONTROL_; 00082 00083 SerialInterface* serialInterface_; 00084 Telemetry* telemetry_; 00085 00086 // Diagnostics 00087 diagnostic_updater::Updater diag_updater_; 00088 double last_spin_time_; 00089 00090 void diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat); 00091 00092 public: 00093 00094 AutoPilot (ros::NodeHandle nh, ros::NodeHandle nh_private); 00095 virtual ~AutoPilot(); 00096 00097 void enablePolling (uint16_t request, uint16_t interval); 00098 void spin (const ros::TimerEvent & e); 00099 }; // end class AutoPilot 00100 } //end namespace asctec_autopilot 00101 00102 #endif