asctec_autopilot: asctec_autopilot: autopilot.cpp Source File

00001 /*
00002  *  AscTec Autopilot Interface
00003  *  Copyright (C) 2010, CCNY Robotics Lab
00004  *  William Morris <morris@ee.ccny.cuny.edu>
00005  *  Ivan Dryanovski <ivan.dryanovski@gmail.com>
00006  *  Steven Bellens <steven.bellens@mech.kuleuven.be>
00007  *  Patrick Bouffard <bouffard@eecs.berkeley.edu>
00008  *
00009  *  http://robotics.ccny.cuny.edu
00010  *
00011  *  This program is free software: you can redistribute it and/or modify
00012  *  it under the terms of the GNU General Public License as published by
00013  *  the Free Software Foundation, either version 3 of the License, or
00014  *  (at your option) any later version.
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU General Public License for more details.
00020  *
00021  *  You should have received a copy of the GNU General Public License
00022  *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
00023  */
00024 
00025 #include "asctec_autopilot/autopilot.h"
00026 
00027 namespace asctec
00028 {
00029   AutoPilot::AutoPilot(ros::NodeHandle nh, ros::NodeHandle nh_private): 
00030     nh_(nh), 
00031     nh_private_(nh_private),
00032     diag_updater_()
00033   {
00034     ROS_INFO ("Creating AutoPilot Interface");
00035 
00036     // **** get parameters
00037 
00038     if (!nh_private_.getParam ("freq", freq_))
00039       freq_ = 50.0;
00040     if (!nh_private_.getParam ("port", port_))
00041       port_ = "/dev/ttyUSB0";
00042     if (!nh_private_.getParam ("speed", speed_))
00043       speed_ = 57600;
00044 
00045     if (!nh_private_.getParam ("enable_LL_STATUS", enable_LL_STATUS_))
00046       enable_LL_STATUS_ = false;
00047     if (!nh_private_.getParam ("enable_IMU_RAWDATA", enable_IMU_RAWDATA_))
00048       enable_IMU_RAWDATA_ = false;
00049     if (!nh_private_.getParam ("enable_IMU_CALCDATA", enable_IMU_CALCDATA_))
00050       enable_IMU_CALCDATA_ = false;
00051     if (!nh_private_.getParam ("enable_RC_DATA", enable_RC_DATA_))
00052       enable_RC_DATA_ = false;
00053     if (!nh_private_.getParam ("enable_CONTROLLER_OUTPUT", enable_CONTROLLER_OUTPUT_))
00054       enable_CONTROLLER_OUTPUT_ = false;
00055     if (!nh_private_.getParam ("enable_GPS_DATA", enable_GPS_DATA_))
00056       enable_GPS_DATA_ = false;
00057     if (!nh_private_.getParam ("enable_GPS_DATA_ADVANCED", enable_GPS_DATA_ADVANCED_))
00058       enable_GPS_DATA_ADVANCED_ = false;
00059     if (!nh_private_.getParam ("enable_CONTROL", enable_CONTROL_))
00060       enable_CONTROL_ = false;
00061 
00062     if (!nh_private_.getParam ("interval_LL_STATUS", interval_LL_STATUS_))
00063       interval_LL_STATUS_ = 1;
00064     if (!nh_private_.getParam ("interval_IMU_RAWDATA", interval_IMU_RAWDATA_))
00065       interval_IMU_RAWDATA_ = 1;
00066     if (!nh_private_.getParam ("interval_IMU_CALCDATA", interval_IMU_CALCDATA_))
00067       interval_IMU_CALCDATA_ = 1;
00068     if (!nh_private_.getParam ("interval_RC_DATA", interval_RC_DATA_))
00069       interval_RC_DATA_ = 1;
00070     if (!nh_private_.getParam ("interval_CONTROLLER_OUTPUT", interval_CONTROLLER_OUTPUT_))
00071       interval_CONTROLLER_OUTPUT_ = 1;
00072     if (!nh_private_.getParam ("interval_GPS_DATA", interval_GPS_DATA_))
00073       interval_GPS_DATA_ = 1;
00074     if (!nh_private_.getParam ("interval_GPS_DATA_ADVANCED", interval_GPS_DATA_ADVANCED_))
00075       interval_GPS_DATA_ADVANCED_ = 1;
00076     if (!nh_private_.getParam ("interval_CONTROL", interval_CONTROL_))
00077       interval_CONTROL_ = 1;
00078 
00079     if (!nh_private_.getParam ("offset_LL_STATUS", offset_LL_STATUS_))
00080       offset_LL_STATUS_ = 0;
00081     if (!nh_private_.getParam ("offset_IMU_RAWDATA", offset_IMU_RAWDATA_))
00082       offset_IMU_RAWDATA_ = 0;
00083     if (!nh_private_.getParam ("offset_IMU_CALCDATA", offset_IMU_CALCDATA_))
00084       offset_IMU_CALCDATA_ = 0;
00085     if (!nh_private_.getParam ("offset_RC_DATA", offset_RC_DATA_))
00086       offset_RC_DATA_ = 0;
00087     if (!nh_private_.getParam ("offset_CONTROLLER_OUTPUT", offset_CONTROLLER_OUTPUT_))
00088       offset_CONTROLLER_OUTPUT_ = 0;
00089     if (!nh_private_.getParam ("offset_GPS_DATA", offset_GPS_DATA_))
00090       offset_GPS_DATA_ = 0;
00091     if (!nh_private_.getParam ("offset_GPS_DATA_ADVANCED", offset_GPS_DATA_ADVANCED_))
00092       offset_GPS_DATA_ADVANCED_ = 0;
00093     if (!nh_private_.getParam ("offset_CONTROL", offset_CONTROL_))
00094       offset_CONTROL_ = 0;
00095 
00096     if (freq_ <= 0.0)
00097       ROS_FATAL ("Invalid frequency param");
00098 
00099     ros::Duration d (1.0 / freq_);
00100 
00101     // **** set up intefaces
00102 
00103     serialInterface_ = new asctec::SerialInterface(port_, speed_);
00104     serialInterface_->serialport_bytes_rx_ = 0;
00105     serialInterface_->serialport_bytes_tx_ = 0;
00106 
00107     ros::NodeHandle nh_rawdata(nh_, asctec::ROS_NAMESPACE);    // publish to "asctec" namespace
00108     telemetry_ = new asctec::Telemetry(nh_rawdata);
00109 
00110     // Diagnostics
00111     diag_updater_.add("AscTec Autopilot Status", this, &AutoPilot::diagnostics);
00112     diag_updater_.setHardwareID("none");
00113     diag_updater_.force_update();
00114 
00115     // **** enable polling
00116     if(enable_LL_STATUS_ == true)
00117       telemetry_->enablePolling (asctec::RequestTypes::LL_STATUS, interval_LL_STATUS_, offset_LL_STATUS_);
00118     if(enable_RC_DATA_)
00119       telemetry_->enablePolling (asctec::RequestTypes::RC_DATA, interval_RC_DATA_, offset_RC_DATA_);
00120     if(enable_CONTROLLER_OUTPUT_)
00121       telemetry_->enablePolling (asctec::RequestTypes::CONTROLLER_OUTPUT, interval_CONTROLLER_OUTPUT_, offset_CONTROLLER_OUTPUT_);
00122     if(enable_IMU_RAWDATA_)
00123       telemetry_->enablePolling(asctec::RequestTypes::IMU_RAWDATA, interval_IMU_RAWDATA_, offset_IMU_RAWDATA_);
00124     if(enable_IMU_CALCDATA_)
00125       telemetry_->enablePolling (asctec::RequestTypes::IMU_CALCDATA, interval_IMU_CALCDATA_, offset_IMU_CALCDATA_);
00126     if(enable_GPS_DATA_)
00127       telemetry_->enablePolling (asctec::RequestTypes::GPS_DATA, interval_GPS_DATA_, offset_GPS_DATA_);
00128     if(enable_GPS_DATA_ADVANCED_)
00129       telemetry_->enablePolling (asctec::RequestTypes::GPS_DATA_ADVANCED, interval_GPS_DATA_ADVANCED_,  offset_GPS_DATA_ADVANCED_);
00130 
00131     // **** enable control
00132     if(enable_CONTROL_ == true)
00133     {
00134       ROS_INFO("Control Enabled");
00135       telemetry_->enableControl(telemetry_, interval_CONTROL_, offset_CONTROL_);
00136     }
00137     else
00138     {
00139       ROS_INFO("Control Disabled");
00140     }
00141     timer_ = nh_private_.createTimer (d, &AutoPilot::spin, this);
00142   }
00143 
00144   AutoPilot::~AutoPilot ()
00145   {
00146     ROS_INFO ("Destroying AutoPilot Interface");
00147 
00148     delete telemetry_;
00149     delete serialInterface_;
00150   }
00151 
00152   void AutoPilot::spin(const ros::TimerEvent & e)
00153   {
00154     //ROS_INFO ("spin()");
00155     //ROS_INFO ("RX: %03.3f Bps",float(serialInterface_->serialport_bytes_rx_)/1000*freq_);
00156     //ROS_INFO ("TX: %03.3f Bps",float(serialInterface_->serialport_bytes_tx_)/1000*freq_);
00157     //serialInterface_->serialport_bytes_rx_ = 0;
00158     //serialInterface_->serialport_bytes_tx_ = 0;
00159     telemetry_->publishPackets();
00160     telemetry_->controlCount_++;
00161     if (telemetry_->estop_)
00162     {
00163       serialInterface_->sendEstop(telemetry_);
00164     }
00165     else
00166     {
00167       serialInterface_->sendControl(telemetry_);
00168     }
00169     telemetry_->buildRequest();
00170     telemetry_->requestCount_++;
00171     if (telemetry_->requestPackets_.count() > 0)
00172     {
00173       serialInterface_->getPackets(telemetry_);
00174     }
00175     last_spin_time_ = e.profile.last_duration.toSec();
00176     diag_updater_.update();
00177   }
00178 
00179   void AutoPilot::diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat)
00180   {
00181     if (telemetry_->estop_)
00182     {
00183       stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "E-STOP");
00184     }
00185     else
00186     {
00187       stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "OK");
00188     }
00189     stat.add("Serial Bytes TX", serialInterface_->serialport_bytes_tx_);
00190     stat.add("Serial Bytes RX", serialInterface_->serialport_bytes_rx_);
00191     stat.add("Last spin() duration [s]", last_spin_time_);
00192   }
00193 
00194 }