hl_interface.cpp
Go to the documentation of this file.
00001 /*
00002 
00003 Copyright (c) 2011, Markus Achtelik, ASL, ETH Zurich, Switzerland
00004 You can contact the author at <markus dot achtelik at mavt dot ethz dot ch>
00005 
00006 All rights reserved.
00007 
00008 Redistribution and use in source and binary forms, with or without
00009 modification, are permitted provided that the following conditions are met:
00010 * Redistributions of source code must retain the above copyright
00011 notice, this list of conditions and the following disclaimer.
00012 * Redistributions in binary form must reproduce the above copyright
00013 notice, this list of conditions and the following disclaimer in the
00014 documentation and/or other materials provided with the distribution.
00015 * Neither the name of ETHZ-ASL nor the
00016 names of its contributors may be used to endorse or promote products
00017 derived from this software without specific prior written permission.
00018 
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00020 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022 DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY
00023 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00024 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00028 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 
00030 */
00031 
00032 #include "hl_interface.h"
00033 #include "helper.h"
00034 #include <asctec_hl_comm/MotorSpeed.h>
00035 
00036 HLInterface::HLInterface(ros::NodeHandle & nh, CommPtr & comm) :
00037   nh_(nh), pnh_("~/fcu"), comm_(comm), gps_status_(sensor_msgs::NavSatStatus::STATUS_NO_FIX), gps_satellites_used_(0),
00038       diag_updater_(), diag_imu_freq_(diagnostic_updater::FrequencyStatusParam(&diag_imu_freq_min_,
00039                                                                                &diag_imu_freq_max_, 0, 5))
00040 {
00041   pnh_.param("frame_id", frame_id_, std::string("fcu"));
00042   pnh_.param("k_stick", k_stick_, 25);
00043   pnh_.param("k_stick_yaw", k_stick_yaw_, 120);
00044   pnh_.param("stddev_angular_velocity", angular_velocity_variance_, 0.013); // taken from experiments
00045   pnh_.param("stddev_linear_acceleration", linear_acceleration_variance_, 0.083); // taken from experiments
00046   angular_velocity_variance_ *= angular_velocity_variance_;
00047   linear_acceleration_variance_ *= linear_acceleration_variance_;
00048 
00049   imu_pub_ = nh_.advertise<asctec_hl_comm::mav_imu> ("imu_custom", 1);
00050   imu_ros_pub_ = nh_.advertise<sensor_msgs::Imu> ("imu", 1);
00051   motors_pub_ = nh_.advertise<asctec_hl_comm::MotorSpeed> ("motor_speed", 1);
00052   gps_pub_ = nh_.advertise<sensor_msgs::NavSatFix> ("gps", 1);
00053   gps_custom_pub_ = nh_.advertise<asctec_hl_comm::GpsCustom> ("gps_custom", 1);
00054   rc_pub_ = nh_.advertise<asctec_hl_comm::mav_rcdata> ("rcdata", 1);
00055   status_pub_ = nh_.advertise<asctec_hl_comm::mav_status> ("status", 1);
00056   mag_pub_ = nh_.advertise<geometry_msgs::Vector3Stamped> ("mag", 1);
00057 
00058   control_sub_ = nh_.subscribe("control", 1, &HLInterface::controlCmdCallback, this);
00059 
00060   motor_srv_ = nh_.advertiseService("motor_control", &HLInterface::cbMotors, this);
00061   crtl_srv_ = nh_.advertiseService("control", &HLInterface::cbCtrl, this);
00062 
00063   config_ = asctec_hl_interface::HLInterfaceConfig::__getDefault__();
00064 
00065   // bring up dynamic reconfigure
00066   reconf_srv_ = new ReconfigureServer(pnh_);
00067   ReconfigureServer::CallbackType f = boost::bind(&HLInterface::cbConfig, this, _1, _2);
00068   reconf_srv_->setCallback(f);
00069 
00070   diag_updater_.add("AscTec AutoPilot status", this, &HLInterface::diagnostic);
00071   diag_updater_.setHardwareID("none");
00072   diag_updater_.add(diag_imu_freq_);
00073   diag_updater_.force_update();
00074 
00075   // register callbacks for packets from serial port
00076   comm_->registerCallback(HLI_PACKET_ID_IMU, &HLInterface::processImuData, this);
00077   comm_->registerCallback(HLI_PACKET_ID_RC, &HLInterface::processRcData, this);
00078   comm_->registerCallback(HLI_PACKET_ID_GPS, &HLInterface::processGpsData, this);
00079   comm_->registerCallback(HLI_PACKET_ID_STATUS, &HLInterface::processStatusData, this);
00080   comm_->registerCallback(HLI_PACKET_ID_TIMESYNC, &HLInterface::processTimeSyncData, this);
00081   comm_->registerCallback(HLI_PACKET_ID_MAG, &HLInterface::processMagData, this);
00082 }
00083 
00084 HLInterface::~HLInterface()
00085 {
00086   delete reconf_srv_;
00087 }
00088 
00089 void HLInterface::processImuData(uint8_t * buf, uint32_t bufLength)
00090 {
00091   HLI_IMU* data = (HLI_IMU*)buf;
00092   static int seq = 0;
00093   diag_imu_freq_.tick();
00094 
00095   double roll = helper::asctecAttitudeToSI(data->ang_roll);
00096   double pitch = helper::asctecAttitudeToSI(data->ang_pitch);
00097   double yaw = helper::asctecAttitudeToSI(data->ang_yaw);
00098 
00099   if (yaw > M_PI)
00100     yaw -= 2 * M_PI;
00101 
00102   double height = data->height * 0.001;
00103   double differential_height = data->differential_height * 0.001;
00104 
00105   uint32_t subs_imu = imu_pub_.getNumSubscribers();
00106   uint32_t subs_imu_ros = imu_ros_pub_.getNumSubscribers();
00107 
00108   if (subs_imu > 0 || subs_imu_ros > 0)
00109   {
00110     geometry_msgs::Quaternion q;
00111     helper::angle2quaternion(roll, pitch, yaw, &q.w, &q.x, &q.y, &q.z);
00112 
00113     if (subs_imu > 0)
00114     {
00115       asctec_hl_comm::mav_imuPtr msg(new asctec_hl_comm::mav_imu);
00116 
00117       msg->header.stamp = ros::Time(data->timestamp * 1.0e-6);
00118       msg->header.seq = seq;
00119       msg->header.frame_id = frame_id_;
00120       msg->acceleration.x = helper::asctecAccToSI(data->acc_x);
00121       msg->acceleration.y = helper::asctecAccToSI(data->acc_y);
00122       msg->acceleration.z = helper::asctecAccToSI(data->acc_z);
00123       msg->angular_velocity.x = helper::asctecOmegaToSI(data->ang_vel_roll);
00124       msg->angular_velocity.y = helper::asctecOmegaToSI(data->ang_vel_pitch);
00125       msg->angular_velocity.z = helper::asctecOmegaToSI(data->ang_vel_yaw);
00126       msg->differential_height = differential_height;
00127       msg->height = height;
00128       msg->orientation = q;
00129 
00130       imu_pub_.publish(msg);
00131     }
00132     if (subs_imu_ros > 0)
00133     {
00134       sensor_msgs::ImuPtr msg(new sensor_msgs::Imu);
00135 
00136       msg->header.stamp = ros::Time(data->timestamp * 1.0e-6);
00137       msg->header.seq = seq;
00138       msg->header.frame_id = frame_id_;
00139       msg->linear_acceleration.x = helper::asctecAccToSI(data->acc_x);
00140       msg->linear_acceleration.y = helper::asctecAccToSI(data->acc_y);
00141       msg->linear_acceleration.z = helper::asctecAccToSI(data->acc_z);
00142       msg->angular_velocity.x = helper::asctecOmegaToSI(data->ang_vel_roll);
00143       msg->angular_velocity.y = helper::asctecOmegaToSI(data->ang_vel_pitch);
00144       msg->angular_velocity.z = helper::asctecOmegaToSI(data->ang_vel_yaw);
00145       msg->orientation = q;
00146       helper::setDiagonalCovariance(msg->angular_velocity_covariance, angular_velocity_variance_);
00147       helper::setDiagonalCovariance(msg->linear_acceleration_covariance, linear_acceleration_variance_);
00148 
00149       imu_ros_pub_.publish(msg);
00150     }
00151   }
00152 
00153   asctec_hl_comm::MotorSpeedPtr msg_motors (new asctec_hl_comm::MotorSpeed);
00154   msg_motors->header.stamp = ros::Time(data->timestamp * 1.0e-6);
00155   msg_motors->header.seq = seq;
00156   msg_motors->header.frame_id = frame_id_;
00157   for(int i = 0; i<6; ++i)
00158     msg_motors->motor_speed[i] = data->motors[i];
00159 
00160   motors_pub_.publish(msg_motors);
00161 
00162   seq++;
00163 }
00164 
00165 void HLInterface::processGpsData(uint8_t * buf, uint32_t bufLength)
00166 {
00167   HLI_GPS* data = (HLI_GPS*)buf;
00168   static int seq = 0;
00169   sensor_msgs::NavSatFixPtr gps_fix(new sensor_msgs::NavSatFix);
00170   asctec_hl_comm::GpsCustomPtr gps_custom(new asctec_hl_comm::GpsCustom);
00171 
00172   gps_fix->header.stamp = ros::Time(((double)data->timestamp) * 1.0e-6);
00173   gps_fix->header.seq = seq;
00174   gps_fix->header.frame_id = frame_id_;
00175   gps_fix->latitude = static_cast<double> (data->latitude) * 1.0e-7;
00176   gps_fix->longitude = static_cast<double> (data->longitude) * 1.0e-7;
00177   gps_fix->altitude = static_cast<double> (data->height) * 1.0e-3;
00178 
00179   // TODO: check covariance
00180   double var_h = static_cast<double> (data->horizontalAccuracy) * 1.0e-3 / 3.0; // accuracy, 3 sigma bound ???
00181   double var_v = static_cast<double> (data->verticalAccuracy) * 1.0e-3 / 3.0;
00182   var_h *= var_h;
00183   var_v *= var_v;
00184 
00185   gps_fix->position_covariance[0] = var_h;
00186   gps_fix->position_covariance[4] = var_h;
00187   gps_fix->position_covariance[8] = var_v;
00188   gps_fix->position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_APPROXIMATED;
00189 
00190   gps_fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
00191 
00192   if (data->status & 0x01)
00193     gps_fix->status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
00194   else
00195     gps_fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
00196 
00197   gps_custom->header = gps_fix->header;
00198   gps_custom->status = gps_fix->status;
00199   gps_custom->longitude = gps_fix->longitude;
00200   gps_custom->latitude = gps_fix->latitude;
00201   gps_custom->altitude = gps_fix->altitude;
00202   gps_custom->position_covariance = gps_fix->position_covariance;
00203   gps_custom->position_covariance_type = gps_fix->position_covariance_type;
00204 
00205   gps_custom->velocity_x = static_cast<double> (data->speedX) * 1.0e-3;
00206   gps_custom->velocity_y = static_cast<double> (data->speedY) * 1.0e-3;
00207 
00208   gps_custom->pressure_height = static_cast<double>(data->pressure_height) * 1.0e-3;
00209 
00210   // TODO: check covariance
00211   double var_vel = static_cast<double> (data->speedAccuracy) * 1.0e-3 / 3.0; // accuracy, 3 sigma bound ???
00212   var_vel *= var_vel;
00213   gps_custom->velocity_covariance[0] = gps_custom->velocity_covariance[3] = var_vel;
00214 
00215   // copy this for the other status message
00216   gps_status_ = gps_fix->status.status;
00217   gps_satellites_used_ = data->numSatellites;
00218 
00219   seq++;
00220   gps_pub_.publish(gps_fix);
00221   gps_custom_pub_.publish(gps_custom);
00222 }
00223 
00224 void HLInterface::processRcData(uint8_t * buf, uint32_t bufLength)
00225 {
00226   HLI_RCDATA* data = (HLI_RCDATA*)buf;
00227   static int seq = 0;
00228   asctec_hl_comm::mav_rcdataPtr msg(new asctec_hl_comm::mav_rcdata);
00229 
00230   msg->header.stamp = ros::Time(data->timestamp * 1.0e-6);
00231   msg->header.frame_id = frame_id_;
00232   msg->header.seq = seq;
00233 
00234   size_t n_channels = std::min(msg->channel.size(), size_t(HLI_NUMBER_RC_CHANNELS));
00235 
00236   for (size_t i = 0; i < n_channels; i++)
00237     msg->channel[i] = data->channel[i];
00238 
00239   seq++;
00240   rc_pub_.publish(msg);
00241 }
00242 
00243 void HLInterface::processStatusData(uint8_t * buf, uint32_t bufLength)
00244 {
00245   HLI_STATUS* data = (HLI_STATUS*)buf;
00246   static int seq = 0;
00247   asctec_hl_comm::mav_statusPtr msg(new asctec_hl_comm::mav_status);
00248 
00249   msg->header.stamp = ros::Time(data->timestamp * 1.0e-6);
00250   msg->header.frame_id = frame_id_;
00251   msg->header.seq = seq;
00252 
00253   msg->battery_voltage = data->battery_voltage * 0.001;
00254   msg->flight_time = data->flight_time;
00255   msg->debug1 = data->debug1;
00256   msg->debug2 = data->debug2;
00257   msg->cpu_load = data->cpu_load;
00258 
00259   switch (data->motors)
00260   {
00261     case -1:
00262       msg->motor_status = "off";
00263       break;
00264     case 0:
00265       msg->motor_status = "stopping";
00266       break;
00267     case 1:
00268       msg->motor_status = "starting";
00269       break;
00270     case 2:
00271       msg->motor_status = "running";
00272       break;
00273     default:
00274       msg->motor_status = "";
00275       break;
00276   }
00277 
00278   if ((data->flight_mode & 0x0F) == HLI_FLIGHTMODE_ACC)
00279     msg->flight_mode_ll = "Acc";
00280   else if ((data->flight_mode & 0x0F) == HLI_FLIGHTMODE_HEIGHT)
00281     msg->flight_mode_ll = "Height";
00282   else if ((data->flight_mode & 0x0F) == HLI_FLIGHTMODE_GPS)
00283     msg->flight_mode_ll = "GPS";
00284   else
00285     msg->flight_mode_ll = "unknown";
00286 
00287   msg->serial_interface_enabled = data->flight_mode & HLI_SERIALINTERFACE_ENABLED_BIT;
00288   msg->serial_interface_active = data->flight_mode & HLI_SERIALINTERFACE_ACTIVE_BIT;
00289 
00290   switch (gps_status_)
00291   {
00292     case sensor_msgs::NavSatStatus::STATUS_NO_FIX:
00293       msg->gps_status = "GPS no fix";
00294       break;
00295     case sensor_msgs::NavSatStatus::STATUS_FIX:
00296       msg->gps_status = "GPS fix";
00297       break;
00298     case sensor_msgs::NavSatStatus::STATUS_SBAS_FIX:
00299       msg->gps_status = "SBAS fix";
00300       break;
00301     case sensor_msgs::NavSatStatus::STATUS_GBAS_FIX:
00302       msg->gps_status = "GBAS fix";
00303       break;
00304     default:
00305       msg->gps_status = "";
00306       break;
00307   }
00308 
00309   msg->gps_num_satellites = gps_satellites_used_;
00310 
00311   msg->have_SSDK_parameters = data->have_SSDK_parameters == 1;
00312 
00313   switch (data->state_estimation)
00314   {
00315     case HLI_MODE_STATE_ESTIMATION_EXT:
00316       msg->state_estimation = asctec_hl_interface::HLInterface_STATE_EST_EXTERN;
00317       break;
00318     case HLI_MODE_STATE_ESTIMATION_HL_EKF:
00319       msg->state_estimation = asctec_hl_interface::HLInterface_STATE_EST_HIGHLEVEL_EKF;
00320       break;
00321     case HLI_MODE_STATE_ESTIMATION_HL_SSDK:
00322       msg->state_estimation = asctec_hl_interface::HLInterface_STATE_EST_HIGHLEVEL_SSDK;
00323       break;
00324     case HLI_MODE_STATE_ESTIMATION_OFF:
00325       msg->state_estimation = asctec_hl_interface::HLInterface_STATE_EST_OFF;
00326       break;
00327     default:
00328       msg->state_estimation = "unknown";
00329       break;
00330   }
00331 
00332   switch (data->position_control)
00333   {
00334     case HLI_MODE_POSCTRL_HL:
00335       msg->position_control = asctec_hl_interface::HLInterface_POSCTRL_HIGHLEVEL;
00336       break;
00337     case HLI_MODE_POSCTRL_LL:
00338       msg->position_control = asctec_hl_interface::HLInterface_POSCTRL_GPS;
00339       break;
00340     case HLI_MODE_POSCTRL_OFF:
00341       msg->position_control = asctec_hl_interface::HLInterface_POSCTRL_OFF;
00342       break;
00343     default:
00344       msg->position_control = "unknown";
00345       break;
00346   }
00347 
00348   msg->rx_packets = comm_->getRxPackets();
00349   msg->rx_packets_good = comm_->getRxPacketsGood();
00350   msg->tx_packets = data->rx_packets;
00351   msg->tx_packets_good = data->rx_packets_good;
00352 
00353   msg->timesync_offset = (float)data->timesync_offset * 1e-6;
00354 
00355   seq++;
00356   status_ = *msg;
00357   status_pub_.publish(msg);
00358 
00359   diag_updater_.update();
00360 }
00361 
00362 
00363 void HLInterface::diagnostic(diagnostic_updater::DiagnosticStatusWrapper & stat)
00364 {
00365   unsigned char summary_level = diagnostic_msgs::DiagnosticStatus::OK;
00366   std::string summary_message = "OK";
00367 
00368   // TODO: are these thresholds ok?
00369 
00370   if (status_.battery_voltage < (config_.battery_warning - 0.5))
00371   {
00372     summary_level = diagnostic_msgs::DiagnosticStatus::ERROR;
00373     summary_message = "battery voltage critical - land now !!!";
00374     ROS_ERROR_STREAM_THROTTLE(1, "" << summary_message << ": " << status_.battery_voltage << " V");
00375   }
00376   else if (status_.battery_voltage < config_.battery_warning)
00377   {
00378     summary_level = diagnostic_msgs::DiagnosticStatus::WARN;
00379     summary_message = "battery voltage low";
00380     ROS_WARN_STREAM_THROTTLE(1, "" << summary_message << ": " << status_.battery_voltage << " V");
00381   }
00382 
00383   stat.summary(summary_level, summary_message);
00384   stat.add("Battery voltage [V]", status_.battery_voltage);
00385   stat.add("Flight time [s]", status_.flight_time);
00386   stat.add("HLP cpu load [%]", status_.cpu_load);
00387   stat.add("Position control mode", status_.position_control);
00388   stat.add("State estimation mode", status_.state_estimation);
00389   stat.add("GPS status", status_.gps_status);
00390   stat.add("GPS # satellites", status_.gps_num_satellites);
00391   stat.add("Flight mode LLP", status_.flight_mode_ll);
00392   stat.add("Motor status", status_.motor_status);
00393   stat.add("Successful TX packets [%]", (float)status_.tx_packets_good/(float)status_.tx_packets*100.0);
00394   stat.add("Successful RX packets [%]", (float)status_.rx_packets_good/(float)status_.rx_packets*100.0);
00395   stat.add("Serial interface enabled", static_cast<bool>(status_.serial_interface_enabled));
00396   stat.add("Serial interface active", static_cast<bool>(status_.serial_interface_active));
00397   stat.add("Have SSDK parameters", static_cast<bool>(status_.have_SSDK_parameters));
00398   stat.add("Timesync offset [ms]", status_.timesync_offset*1000.0);
00399   stat.add("Debug 1", status_.debug1);
00400   stat.add("Debug 2", status_.debug2);
00401 }
00402 
00403 void HLInterface::processTimeSyncData(uint8_t * buf, uint32_t bufLength)
00404 {
00405   HLI_TIMESYNC data = *(HLI_TIMESYNC*)buf;
00406 
00407   data.ts1 = (uint64_t)(ros::Time::now().toSec() * 1.0e6);
00408 
00409   comm_->sendPacket(HLI_PACKET_ID_TIMESYNC, data);
00410 
00411   // warn if imu time is too far away from pc time. At low baudrates, the IMU will take longer to sync.
00412   ROS_WARN_STREAM_COND(std::abs(status_.timesync_offset) > 0.002, "imu time is off by "<<status_.timesync_offset * 1e3 <<" ms");
00413 }
00414 
00415 void HLInterface::processMagData(uint8_t * buf, uint32_t bufLength)
00416 {
00417   HLI_MAG* data = (HLI_MAG*)buf;
00418   static int seq = 0;
00419   geometry_msgs::Vector3StampedPtr msg(new geometry_msgs::Vector3Stamped);
00420 
00421   msg->header.stamp = ros::Time(data->timestamp * 1.0e-6);
00422   msg->header.frame_id = frame_id_;
00423   msg->header.seq = seq;
00424 
00425   msg->vector.x = data->x;
00426   msg->vector.y = data->y;
00427   msg->vector.z = data->z;
00428 
00429   seq++;
00430   mag_pub_.publish(msg);
00431 }
00432 
00433 bool HLInterface::cbMotors(asctec_hl_comm::mav_ctrl_motors::Request &req,
00434                            asctec_hl_comm::mav_ctrl_motors::Response &resp)
00435 {
00436   HLI_MOTORS data;
00437 
00438   if (req.startMotors)
00439   {
00440     data.motors = 1;
00441     ROS_INFO("request to start motors");
00442   }
00443   else
00444   {
00445     data.motors = 0;
00446     ROS_INFO("request to stop motors");
00447   }
00448 
00449   // make sure packet arrives
00450   bool success = false;
00451   for (int i = 0; i < 5; i++)
00452   {
00453     success = comm_->sendPacketAck(HLI_PACKET_ID_MOTORS, data, 0.5);
00454     if (success)
00455       break;
00456   }
00457 
00458   if(!success)
00459   {
00460     ROS_WARN("unable to send motor on/off command");
00461     resp.motorsRunning = status_.motor_status == "running";
00462     return false;
00463   }
00464 
00465   ros::Duration d(0.1);
00466   for (int i = 0; i < 10; i++)
00467   {
00468     if (req.startMotors && status_.motor_status == "running")
00469     {
00470       resp.motorsRunning = true;
00471       return true;
00472     }
00473     else if (!req.startMotors && status_.motor_status == "off")
00474     {
00475       resp.motorsRunning = false;
00476       return true;
00477     }
00478     d.sleep();
00479   }
00480 
00481   ROS_WARN("switching motors %s timed out", req.startMotors?"on":"off");
00482   return false;
00483 }
00484 
00485 void HLInterface::controlCmdCallback(const asctec_hl_comm::mav_ctrlConstPtr & msg)
00486 {
00487   sendControlCmd(*msg);
00488 }
00489 
00490 bool HLInterface::cbCtrl(asctec_hl_comm::MavCtrlSrv::Request & req, asctec_hl_comm::MavCtrlSrv::Response & resp)
00491 {
00492   sendControlCmd(req.ctrl, &resp.ctrl_result);
00493   return resp.ctrl_result.type != -1;
00494 }
00495 
00496 void HLInterface::sendControlCmd(const asctec_hl_comm::mav_ctrl & ctrl, asctec_hl_comm::mav_ctrl * ctrl_result)
00497 {
00498   bool validCommand = false;
00499 
00500   if (ctrl.type == asctec_hl_comm::mav_ctrl::acceleration)
00501   {
00502     if (config_.position_control == asctec_hl_interface::HLInterface_POSCTRL_OFF)
00503     {
00504       sendAccCommandLL(ctrl, ctrl_result);
00505       validCommand = true;
00506     }
00507     else
00508     {
00509       ROS_WARN_STREAM_THROTTLE(2,
00510           "GPS/Highlevel position control must be turned off. "
00511           "Set \"position_control\" parameter to \"off\"");
00512       if(ctrl_result != NULL)
00513         ctrl_result->type = -1;
00514     }
00515   }
00516 
00517   else if (ctrl.type == asctec_hl_comm::mav_ctrl::velocity_body)
00518   {
00519     if (config_.position_control == asctec_hl_interface::HLInterface_POSCTRL_GPS)
00520     {
00521       sendVelCommandLL(ctrl, ctrl_result);
00522       validCommand = true;
00523     }
00524     else if (config_.position_control == asctec_hl_interface::HLInterface_POSCTRL_HIGHLEVEL)
00525     {
00526       sendVelCommandHL(ctrl, ctrl_result);
00527       validCommand = true;
00528     }
00529     else
00530     {
00531       ROS_WARN_STREAM_THROTTLE(2,
00532           "Higlevel or Lowlevel processor position control has not "
00533           "been chosen. Set \"position_control\" parameter to "
00534           "\"HighLevel\" or \"GPS\" ! sending nothing to mav !");
00535       if(ctrl_result != NULL)
00536         ctrl_result->type = -1;
00537     }
00538   }
00539   else if (ctrl.type == asctec_hl_comm::mav_ctrl::velocity)
00540     {
00541       if (config_.position_control == asctec_hl_interface::HLInterface_POSCTRL_HIGHLEVEL)
00542       {
00543         sendVelCommandHL(ctrl, ctrl_result);
00544         validCommand = true;
00545       }
00546       else
00547       {
00548         ROS_WARN_STREAM_THROTTLE(2,
00549             "Higlevel or Lowlevel processor position control has not "
00550             "been chosen. Set \"position_control\" parameter to "
00551             "\"HighLevel\" ! sending nothing to mav !");
00552         if(ctrl_result != NULL)
00553           ctrl_result->type = -1;
00554       }
00555     }
00556   else if (ctrl.type == asctec_hl_comm::mav_ctrl::position || ctrl.type == asctec_hl_comm::mav_ctrl::position_body)
00557   {
00558     // allow to "inherit" max velocity from parameters
00559     asctec_hl_comm::mav_ctrl ctrl_msg = ctrl;
00560     if(ctrl_msg.v_max_xy == -1)
00561       ctrl_msg.v_max_xy = config_.max_velocity_xy;
00562 
00563     if(ctrl_msg.v_max_z == -1)
00564       ctrl_msg.v_max_z = config_.max_velocity_z;
00565 
00566     if (config_.position_control == asctec_hl_interface::HLInterface_POSCTRL_HIGHLEVEL &&
00567         config_.state_estimation != asctec_hl_interface::HLInterface_STATE_EST_OFF)
00568     {
00569       sendPosCommandHL(ctrl_msg, ctrl_result);
00570       validCommand = true;
00571     }
00572     else
00573     {
00574       ROS_WARN_STREAM_THROTTLE(2,
00575           "Higlevel processor position control was not chosen and/or no state "
00576           "estimation was selected. Set the \"position_control\" parameter to "
00577           "\"HighLevel\" and the \"state_estimation\" parameter to anything but \"off\"! sending nothing to mav !");
00578       if(ctrl_result != NULL)
00579         ctrl_result->type = -1;
00580     }
00581   }
00582 
00583   // was the controlmode specified properly?
00584   if (!validCommand)
00585   {
00586     ROS_WARN_STREAM_THROTTLE(2,"Control type was not specified ... not sending anything to the mav");
00587   }
00588 }
00589 
00590 void HLInterface::sendAccCommandLL(const asctec_hl_comm::mav_ctrl & msg, asctec_hl_comm::mav_ctrl * ctrl_result)
00591 {
00592   HLI_CMD_LL ctrlLL;
00593 
00594   // spin-directions positive according to right hand rule around axis
00595   ctrlLL.x = helper::clamp<short>(-2047, 2047, (short)(msg.x * 180.0 / M_PI * 1000.0 / (float)k_stick_)); // cmd=real_angle*1000/K_stick
00596   ctrlLL.y = helper::clamp<short>(-2047, 2047, (short)(msg.y * 180.0 / M_PI * 1000.0 / (float)k_stick_)); // dito
00597 
00598   // cmd=real_anglular_velocity*1000/k_stick_yaw,
00599   // cmd is limited to +- 1700 such that it's not possible to switch the motors with a bad command
00600   ctrlLL.yaw = helper::clamp<short>(-1700, 1700, (short)(msg.yaw * 180.0 / M_PI * 1000.0 / (float)k_stick_yaw_));
00601 
00602   // catch wrong thrust command ;-)
00603   if (msg.z > 1.0)
00604   {
00605     ROS_ERROR(
00606         "I just prevented you from giving full thrust..."
00607         "\nset the input range correct!!! [0 ... 1.0] ");
00608 
00609     if(ctrl_result != NULL)
00610       ctrl_result->type = -1;
00611 
00612     return;
00613   }
00614   else
00615   {
00616     ctrlLL.z = helper::clamp<short>(0, 4096, (short)(msg.z * 4096.0));
00617   }
00618 
00619   if(ctrl_result != NULL)
00620   {
00621     *ctrl_result = msg;
00622     ctrl_result->x = static_cast<float>(ctrlLL.x * k_stick_) / 180.0 * M_PI * 1e-3;
00623     ctrl_result->y = static_cast<float>(ctrlLL.y * k_stick_) / 180.0 * M_PI * 1e-3;
00624     ctrl_result->z = static_cast<float>(ctrlLL.z) / 4096.0;
00625     ctrl_result->yaw = static_cast<float>(ctrlLL.yaw * k_stick_yaw_) / 180.0 * M_PI * 1e-3;
00626   }
00627 
00628 //  ROS_INFO_STREAM("sending command: x:"<<ctrlLL.x<<" y:"<<ctrlLL.y<<" yaw:"<<ctrlLL.yaw<<" z:"<<ctrlLL.z<<" ctrl:"<<enable_ctrl_);
00629   comm_->sendPacket(HLI_PACKET_ID_CONTROL_LL, ctrlLL);
00630 }
00631 
00632 void HLInterface::sendVelCommandLL(const asctec_hl_comm::mav_ctrl & msg, asctec_hl_comm::mav_ctrl * ctrl_result)
00633 {
00634   HLI_CMD_LL ctrlLL;
00635 
00636   ctrlLL.x = helper::clamp<short>(-2047, 2047, (short)(msg.x / config_.max_velocity_xy * 2047.0));
00637   ctrlLL.y = helper::clamp<short>(-2047, 2047, (short)(msg.y / config_.max_velocity_xy * 2047.0));
00638   ctrlLL.yaw = helper::clamp<short>(-2047, 2047, (short)(msg.yaw / config_.max_velocity_yaw* 2047.0));
00639   ctrlLL.z = helper::clamp<short>(-2047, 2047, (short)(msg.z / config_.max_velocity_z * 2047.0)) + 2047; // "zero" is still 2047!
00640 
00641   if (ctrl_result != NULL)
00642   {
00643     *ctrl_result = msg;
00644     ctrl_result->x = static_cast<float> (ctrlLL.x) / 2047.0 * config_.max_velocity_xy;
00645     ctrl_result->y = static_cast<float> (ctrlLL.y) / 2047.0 * config_.max_velocity_xy;
00646     ctrl_result->z = static_cast<float> (ctrlLL.z - 2047) / 2047.0 * config_.max_velocity_z;
00647     ctrl_result->yaw = static_cast<float> (ctrlLL.yaw) / 2047.0 * config_.max_velocity_yaw;
00648   }
00649 
00650 //  ROS_INFO_STREAM("sending command: x:"<<ctrlLL.x<<" y:"<<ctrlLL.y<<" yaw:"<<ctrlLL.yaw<<" z:"<<ctrlLL.z<<" ctrl:"<<enable_ctrl_);
00651   comm_->sendPacket(HLI_PACKET_ID_CONTROL_LL, ctrlLL);
00652 }
00653 
00654 void HLInterface::sendVelCommandHL(const asctec_hl_comm::mav_ctrl & msg, asctec_hl_comm::mav_ctrl * ctrl_result)
00655 {
00656   HLI_CMD_HL ctrlHL;
00657 
00658   ctrlHL.vX = (short)(helper::clamp<float>(-config_.max_velocity_xy, config_.max_velocity_xy, msg.x) * 1000.0);
00659   ctrlHL.vY = (short)(helper::clamp<float>(-config_.max_velocity_xy, config_.max_velocity_xy, msg.y) * 1000.0);
00660   ctrlHL.vZ = (short)(helper::clamp<float>(-config_.max_velocity_z, config_.max_velocity_z, msg.z) * 1000.0);
00661   ctrlHL.vYaw = (short)(helper::clamp<float>(-config_.max_velocity_yaw, config_.max_velocity_yaw, msg.yaw) * 180.0 / M_PI * 1000.0);
00662 
00663   ctrlHL.x = 0;
00664   ctrlHL.y = 0;
00665   ctrlHL.z = 0;
00666   ctrlHL.heading = 0;
00667 
00668   ctrlHL.bitfield = 1;
00669   if(msg.type == asctec_hl_comm::mav_ctrl::velocity_body)
00670     ctrlHL.bitfield |= EXT_POSITION_CMD_BODYFIXED;
00671 
00672   if (ctrl_result != NULL)
00673   {
00674     *ctrl_result = msg;
00675     ctrl_result->x = static_cast<float> (ctrlHL.vX) * 1e-3;
00676     ctrl_result->y = static_cast<float> (ctrlHL.vY) * 1e-3;
00677     ctrl_result->z = static_cast<float> (ctrlHL.vZ) * 1e-3;
00678     ctrl_result->yaw = static_cast<float> (ctrlHL.vYaw) / 180.0e3 * M_PI;
00679   }
00680 
00681   comm_->sendPacket(HLI_PACKET_ID_CONTROL_HL, ctrlHL);
00682 }
00683 void HLInterface::sendPosCommandHL(const asctec_hl_comm::mav_ctrl & msg, asctec_hl_comm::mav_ctrl * ctrl_result)
00684 {
00685   HLI_CMD_HL ctrlHL;
00686   static unsigned int seq = 1; // <-- set to one, otherwise first packet doesn't get through
00687 
00688   if (std::abs(msg.yaw) > M_PI)
00689   {
00690     ROS_WARN("yaw has to be in [-pi ... pi], got %f instead", msg.yaw);
00691     if (ctrl_result != NULL)
00692       ctrl_result->type = -1;
00693     return;
00694   }
00695 
00696   ctrlHL.seq = seq;
00697   ctrlHL.vX = 0;
00698   ctrlHL.vX = 0;
00699   ctrlHL.vZ = 0;
00700   //                    ctrlHL.vHeading = 0;
00701 
00702   ctrlHL.x = static_cast<int>(helper::clamp<float>(config_.min_pos_x, config_.max_pos_x, msg.x) * 1000.0);
00703   ctrlHL.y = static_cast<int>(helper::clamp<float>(config_.min_pos_y, config_.max_pos_y, msg.y) * 1000.0);
00704   ctrlHL.z = static_cast<int>(helper::clamp<float>(config_.min_pos_z, config_.max_pos_z, msg.z) * 1000.0);
00705   // asctec uses 0...360° * 1000, we -pi...+pi
00706   ctrlHL.heading = helper::yaw2asctec(msg.yaw);
00707 
00708   ctrlHL.vMaxXY = static_cast<short>(std::min<float>(config_.max_velocity_xy, msg.v_max_xy)*1000);
00709   ctrlHL.vMaxZ = static_cast<short>(std::min<float>(config_.max_velocity_z, msg.v_max_z)*1000);
00710 
00711   ctrlHL.bitfield = 0;
00712 
00713   if(msg.type == asctec_hl_comm::mav_ctrl::position_body)
00714     ctrlHL.bitfield |= EXT_POSITION_CMD_BODYFIXED;
00715 
00716   if (ctrl_result != NULL)
00717   {
00718     *ctrl_result = msg;
00719     ctrl_result->x = static_cast<float> (ctrlHL.x) * 1e-3;
00720     ctrl_result->y = static_cast<float> (ctrlHL.y) * 1e-3;
00721     ctrl_result->z = static_cast<float> (ctrlHL.z) * 1e-3;
00722     ctrl_result->yaw = static_cast<float> (ctrlHL.heading) / 180.0e3 * M_PI;
00723     if (ctrl_result->yaw > M_PI)
00724         ctrl_result->yaw -= 2 * M_PI;
00725   }
00726 
00727   comm_->sendPacket(HLI_PACKET_ID_CONTROL_HL, ctrlHL);
00728   seq++;
00729 }
00730 
00731 void HLInterface::cbConfig(asctec_hl_interface::HLInterfaceConfig & config, uint32_t level)
00732 {
00733 
00734   if (level & asctec_hl_interface::HLInterface_HLI_CONFIG)
00735   {
00745     enable_ctrl_ = 0;
00746     if (config.enable_x)
00747       enable_ctrl_ |= 1;
00748     if (config.enable_y)
00749       enable_ctrl_ |= 1 << 1;
00750     if (config.enable_yaw)
00751       enable_ctrl_ |= 1 << 2;
00752     if (config.enable_z)
00753       enable_ctrl_ |= 1 << 3;
00754     if (config.position_control == asctec_hl_interface::HLInterface_POSCTRL_GPS)
00755     {
00756       enable_ctrl_ |= 1 << 4;
00757       enable_ctrl_ |= 1 << 5;
00758     }
00759 
00760     HLI_CONFIG cfg;
00761     cfg.position_control_axis_enable = enable_ctrl_;
00762 
00763     if (config.position_control == asctec_hl_interface::HLInterface_POSCTRL_HIGHLEVEL)
00764       cfg.mode_position_control = HLI_MODE_POSCTRL_HL;
00765     else if (config.position_control == asctec_hl_interface::HLInterface_POSCTRL_GPS)
00766       cfg.mode_position_control = HLI_MODE_POSCTRL_LL;
00767     else
00768       cfg.mode_position_control = HLI_MODE_POSCTRL_OFF;
00769 
00770     if (config.state_estimation == asctec_hl_interface::HLInterface_STATE_EST_EXTERN)
00771       cfg.mode_state_estimation = HLI_MODE_STATE_ESTIMATION_EXT;
00772     else if (config.state_estimation == asctec_hl_interface::HLInterface_STATE_EST_HIGHLEVEL_SSDK)
00773       cfg.mode_state_estimation = HLI_MODE_STATE_ESTIMATION_HL_SSDK;
00774     else if (config.state_estimation == asctec_hl_interface::HLInterface_STATE_EST_HIGHLEVEL_EKF)
00775       cfg.mode_state_estimation = HLI_MODE_STATE_ESTIMATION_HL_EKF;
00776     else
00777       cfg.mode_state_estimation = HLI_MODE_STATE_ESTIMATION_OFF;
00778 
00779     cfg.battery_warning_voltage = static_cast<uint16_t>(config.battery_warning * 1000); // convert to mV
00780 
00781     if(!comm_->sendPacketAck(HLI_PACKET_ID_CONFIG, cfg)){
00782       config.enable_x = config_.enable_x;
00783       config.enable_y = config_.enable_y;
00784       config.enable_z = config_.enable_z;
00785       config.enable_yaw = config_.enable_yaw;
00786       config.position_control = config_.position_control;
00787       config.state_estimation = config_.state_estimation;
00788       config.battery_warning = config_.battery_warning;
00789     }
00790   }
00791 
00792   if (level & asctec_hl_interface::HLInterface_PACKET_RATE)
00793   {
00794     HLI_SUBSCRIPTION ps;
00795     ps.imu = helper::rateToPeriod(config.packet_rate_imu);
00796     ps.rcdata = helper::rateToPeriod(config.packet_rate_rc);
00797     ps.gps = helper::rateToPeriod(config.packet_rate_gps);
00798     ps.ssdk_debug = helper::rateToPeriod(config.packet_rate_ssdk_debug);
00799     ps.ekf_state = helper::rateToPeriod(config.packet_rate_ekf_state);
00800     ps.mag = helper::rateToPeriod(config.packet_rate_mag);
00801 
00802     diag_imu_freq_min_ = 0.95 * config.packet_rate_imu;
00803     diag_imu_freq_max_ = 1.05 * config.packet_rate_imu;
00804 
00805     if(!comm_->sendPacketAck(HLI_PACKET_ID_SUBSCRIPTION, ps)){
00806       config.packet_rate_imu = config_.packet_rate_imu;
00807       config.packet_rate_rc = config_.packet_rate_rc;
00808       config.packet_rate_gps = config_.packet_rate_gps;
00809       config.packet_rate_ssdk_debug = config_.packet_rate_ssdk_debug;
00810       config.packet_rate_ekf_state = config_.packet_rate_ekf_state;
00811       config.packet_rate_mag = config_.packet_rate_mag;
00812     }
00813   }
00814 
00815   config_ = config;
00816 }
00817 
00818 


asctec_hl_interface
Author(s): Markus Achtelik, Michael Achtelik, Stephan Weiss, Laurent Kneip
autogenerated on Sun Oct 5 2014 22:21:01