$search
00001 /* 00002 * AscTec Autopilot Telemetry 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 <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 00037 #include "asctec_autopilot/crc16.h" 00038 #include "asctec_autopilot/telemetry.h" 00039 00040 namespace asctec 00041 { 00042 Telemetry::Telemetry (ros::NodeHandle nh): nh_(nh) 00043 { 00044 requestCount_ = 0; 00045 pollingEnabled_ = false; 00046 requestPackets_ = 0; 00047 memset (requestInterval_, 0, REQUEST_TYPES * sizeof (uint8_t)); 00048 memset (requestOffset_, 0, REQUEST_TYPES * sizeof (uint8_t)); 00049 REQUEST_BITMASK[RequestTypes::LL_STATUS] = 0x0001; 00050 REQUEST_BITMASK[RequestTypes::IMU_RAWDATA] = 0x0002; 00051 REQUEST_BITMASK[RequestTypes::IMU_CALCDATA] = 0x0004; 00052 REQUEST_BITMASK[RequestTypes::RC_DATA] = 0x0008; 00053 REQUEST_BITMASK[RequestTypes::CONTROLLER_OUTPUT] = 0x0010; 00054 REQUEST_BITMASK[RequestTypes::GPS_DATA] = 0x0080; 00055 REQUEST_BITMASK[RequestTypes::WAYPOINT] = 0x0100; 00056 REQUEST_BITMASK[RequestTypes::GPS_DATA_ADVANCED] = 0x0200; 00057 REQUEST_BITMASK[RequestTypes::CAM_DATA] = 0x0800; 00058 estop_ = false; 00059 00060 // initialize pointers 00061 00062 LLStatus_ = boost::make_shared<asctec_msgs::LLStatus> (); 00063 IMURawData_ = boost::make_shared<asctec_msgs::IMURawData> (); 00064 IMUCalcData_ = boost::make_shared<asctec_msgs::IMUCalcData> (); 00065 RCData_ = boost::make_shared<asctec_msgs::RCData> (); 00066 ControllerOutput_ = boost::make_shared<asctec_msgs::ControllerOutput>(); 00067 GPSData_ = boost::make_shared<asctec_msgs::GPSData> (); 00068 GPSDataAdvanced_ = boost::make_shared<asctec_msgs::GPSDataAdvanced> (); 00069 } 00070 Telemetry::~Telemetry () 00071 { 00072 } 00073 00074 void Telemetry::buildRequest () 00075 { 00076 //ROS_DEBUG ("Telemetry::buildRequest()"); 00077 // Clear previous packet request 00078 requestPackets_ ^= requestPackets_; 00079 for (int i = 0; i < REQUEST_TYPES; i++) 00080 { 00081 if (requestInterval_[i] != 0 && ((requestCount_ - requestOffset_[i]) % requestInterval_[i] == 0) && 00082 (requestPublisher_[i].getNumSubscribers () > 0)) 00083 requestPackets_ |= REQUEST_BITMASK[i]; 00084 } 00085 } 00086 void Telemetry::publishPackets () 00087 { 00088 for (int i = 0; i < REQUEST_TYPES; i++) 00089 { 00090 if (requestInterval_[i] != 0 && ((requestCount_ - requestOffset_[i]) % requestInterval_[i] == 0)) 00091 { 00092 switch (i) 00093 { 00094 case RequestTypes::LL_STATUS: 00095 copyLL_STATUS (); 00096 LLStatus_->header.stamp = timestamps_[RequestTypes::LL_STATUS]; 00097 //dumpLL_STATUS (); 00098 requestPublisher_[i].publish (LLStatus_); 00099 break; 00100 case RequestTypes::IMU_RAWDATA: 00101 copyIMU_RAWDATA (); 00102 IMURawData_->header.stamp = timestamps_[RequestTypes::IMU_RAWDATA]; 00103 //dumpIMU_RAWDATA(); 00104 requestPublisher_[i].publish (IMURawData_); 00105 break; 00106 case RequestTypes::IMU_CALCDATA: 00107 copyIMU_CALCDATA (); 00108 IMUCalcData_->header.stamp = timestamps_[RequestTypes::IMU_CALCDATA]; 00109 //dumpIMU_CALCDATA(); 00110 requestPublisher_[i].publish (IMUCalcData_); 00111 break; 00112 case RequestTypes::GPS_DATA: 00113 copyGPS_DATA (); 00114 GPSData_->header.stamp = timestamps_[RequestTypes::GPS_DATA]; 00115 requestPublisher_[i].publish (GPSData_); 00116 break; 00117 case RequestTypes::RC_DATA: 00118 copyRC_DATA (); 00119 RCData_->header.stamp = timestamps_[RequestTypes::RC_DATA]; 00120 requestPublisher_[i].publish (RCData_); 00121 break; 00122 case RequestTypes::CONTROLLER_OUTPUT: 00123 copyCONTROLLER_OUTPUT (); 00124 ControllerOutput_->header.stamp = timestamps_[RequestTypes::CONTROLLER_OUTPUT]; 00125 requestPublisher_[i].publish (ControllerOutput_); 00126 break; 00127 case RequestTypes::GPS_DATA_ADVANCED: 00128 copyGPS_DATA_ADVANCED (); 00129 GPSDataAdvanced_->header.stamp = timestamps_[RequestTypes::GPS_DATA_ADVANCED]; 00130 //dumpGPS_DATA_ADVANCED(); 00131 requestPublisher_[i].publish (GPSDataAdvanced_); 00132 break; 00133 default: 00134 ROS_DEBUG ("Unable to publish unknown type"); 00135 } 00136 } 00137 } 00138 } 00139 00140 void Telemetry::enablePolling (RequestType msg, uint8_t interval, uint8_t offset) 00141 { 00142 switch (msg) 00143 { 00144 case RequestTypes::LL_STATUS: 00145 requestPublisher_[msg] = nh_.advertise < asctec_msgs::LLStatus > (requestToString (msg).c_str (), 10); 00146 break; 00147 case RequestTypes::IMU_RAWDATA: 00148 requestPublisher_[msg] = nh_.advertise < asctec_msgs::IMURawData > (requestToString (msg).c_str (), 10); 00149 break; 00150 case RequestTypes::IMU_CALCDATA: 00151 requestPublisher_[msg] = nh_.advertise < asctec_msgs::IMUCalcData > (requestToString (msg).c_str (), 10); 00152 break; 00153 case RequestTypes::RC_DATA: 00154 requestPublisher_[msg] = nh_.advertise < asctec_msgs::RCData > (requestToString (msg).c_str (), 10); 00155 break; 00156 case RequestTypes::GPS_DATA: 00157 requestPublisher_[msg] = nh_.advertise < asctec_msgs::GPSData > (requestToString (msg).c_str (), 10); 00158 break; 00159 case RequestTypes::GPS_DATA_ADVANCED: 00160 requestPublisher_[msg] = nh_.advertise < asctec_msgs::GPSDataAdvanced > (requestToString (msg).c_str (), 10); 00161 break; 00162 case RequestTypes::WAYPOINT: 00163 // to be filled in 00164 break; 00165 case RequestTypes::CAM_DATA: 00166 // to be filled in 00167 break; 00168 case RequestTypes::CONTROLLER_OUTPUT: 00169 requestPublisher_[msg] = nh_.advertise < asctec_msgs::ControllerOutput > (requestToString (msg).c_str (), 10); 00170 break; 00171 } 00172 00173 ROS_INFO ("Publishing %s data", requestToString (msg).c_str()); 00174 ROS_DEBUG ("Telemetry::enablePolling()"); 00175 requestInterval_[msg] = interval; 00176 requestOffset_[msg] = offset; 00177 pollingEnabled_ = true; 00178 } 00179 00180 void Telemetry::enableControl (Telemetry * telemetry_, uint8_t interval, uint8_t offset) 00181 { 00182 controlSubscriber_ = nh_.subscribe("CTRL_INPUT", 1, &Telemetry::copyCTRL_INPUT, telemetry_, ros::TransportHints().tcpNoDelay()); 00183 ROS_INFO("Listening to %s data on topic: %s", "CTRL_INPUT","CTRL_INPUT"); 00184 ROS_DEBUG ("Telemetry::enableControl()"); 00185 estopSubscriber_ = nh_.subscribe("ESTOP", 1, &Telemetry::estopCallback, telemetry_, ros::TransportHints().tcpNoDelay()); 00186 controlInterval_ = interval; 00187 controlOffset_ = offset; 00188 controlEnabled_ = true; 00189 } 00190 void Telemetry::estopCallback(const std_msgs::Bool msg) 00191 { 00192 static bool info_printed = false; 00193 if (msg.data) { 00194 estop_ = true; 00195 if (!info_printed) { 00196 ROS_WARN("Heard e-stop command!"); 00197 info_printed = true; 00198 } 00199 } else { 00200 ROS_WARN("Got e-stop message, but value was false"); 00201 } 00202 } 00203 00204 std::string Telemetry::requestToString (RequestTypes::RequestType t) 00205 { 00206 switch (t) 00207 { 00208 case RequestTypes::LL_STATUS: 00209 { 00210 return "LL_STATUS"; 00211 } 00212 case RequestTypes::IMU_RAWDATA: 00213 { 00214 return "IMU_RAWDATA"; 00215 } 00216 case RequestTypes::IMU_CALCDATA: 00217 { 00218 return "IMU_CALCDATA"; 00219 } 00220 case RequestTypes::RC_DATA: 00221 { 00222 return "RC_DATA"; 00223 } 00224 case RequestTypes::CONTROLLER_OUTPUT: 00225 { 00226 return "CONTROLLER_OUTPUT"; 00227 } 00228 case RequestTypes::GPS_DATA: 00229 { 00230 return "GPS_DATA"; 00231 } 00232 case RequestTypes::GPS_DATA_ADVANCED: 00233 { 00234 return "GPS_DATA_ADVANCED"; 00235 } 00236 case RequestTypes::WAYPOINT: 00237 { 00238 return "WAYPOINT"; 00239 } 00240 case RequestTypes::CAM_DATA: 00241 { 00242 return "CAM_DATA"; 00243 } 00244 } 00245 return "Unknown"; 00246 } 00247 00248 void Telemetry::dumpLL_STATUS () 00249 { 00250 ROS_INFO("LL_STATUS"); 00251 ROS_INFO("--------------------------------"); 00252 ROS_INFO("battery_voltage_1:%d",LL_STATUS_.battery_voltage_1); 00253 ROS_INFO("battery_voltage_2:%d",LL_STATUS_.battery_voltage_2); 00254 ROS_INFO("status:%d",LL_STATUS_.status); 00255 ROS_INFO("cpu_load:%d",LL_STATUS_.cpu_load); 00256 ROS_INFO("compass_enabled:%d",LL_STATUS_.compass_enabled); 00257 ROS_INFO("chksum_error:%d",LL_STATUS_.chksum_error); 00258 ROS_INFO("flying:%d",LL_STATUS_.flying); 00259 ROS_INFO("motors_on:%d",LL_STATUS_.motors_on); 00260 ROS_INFO("flightMode:%d",LL_STATUS_.flightMode); 00261 ROS_INFO("up_time:%d",LL_STATUS_.up_time); 00262 if (LL_STATUS_.flightMode == 97) 00263 ROS_INFO ("---------- SERIAL LINK ACTIVE !!! --------"); 00264 } 00265 void Telemetry::dumpIMU_RAWDATA () 00266 { 00267 ROS_INFO ("IMU_RAWDATA"); 00268 ROS_INFO ("--------------------------------"); 00269 ROS_INFO ("pressure:%d", IMU_RAWDATA_.pressure); 00270 ROS_INFO ("gyro_x:%d", IMU_RAWDATA_.gyro_x); 00271 ROS_INFO ("gyro_y:%d", IMU_RAWDATA_.gyro_y); 00272 ROS_INFO ("gyro_z:%d", IMU_RAWDATA_.gyro_z); 00273 ROS_INFO ("mag_x:%d", IMU_RAWDATA_.mag_x); 00274 ROS_INFO ("mag_y:%d", IMU_RAWDATA_.mag_y); 00275 ROS_INFO ("mag_z:%d", IMU_RAWDATA_.mag_z); 00276 ROS_INFO ("acc_x:%d", IMU_RAWDATA_.acc_x); 00277 ROS_INFO ("acc_y:%d", IMU_RAWDATA_.acc_y); 00278 ROS_INFO ("acc_z:%d", IMU_RAWDATA_.acc_z); 00279 ROS_INFO ("temp_gyro:%d", IMU_RAWDATA_.temp_gyro); 00280 ROS_INFO ("temp_ADC:%d", IMU_RAWDATA_.temp_ADC); 00281 } 00282 void Telemetry::dumpIMU_CALCDATA () 00283 { 00284 ROS_INFO ("IMU_CALCDATA"); 00285 ROS_INFO ("--------------------------------"); 00286 ROS_INFO ("angle_nick:%d", IMU_CALCDATA_.angle_nick); 00287 ROS_INFO ("angle_roll:%d", IMU_CALCDATA_.angle_roll); 00288 ROS_INFO ("angle_yaw:%d", IMU_CALCDATA_.angle_yaw); 00289 ROS_INFO ("angvel_nick:%d", IMU_CALCDATA_.angvel_nick); 00290 ROS_INFO ("angvel_roll:%d", IMU_CALCDATA_.angvel_roll); 00291 ROS_INFO ("angvel_yaw:%d", IMU_CALCDATA_.angvel_yaw); 00292 ROS_INFO ("acc_x_calib:%d", IMU_CALCDATA_.acc_x_calib); 00293 ROS_INFO ("acc_y_calib:%d", IMU_CALCDATA_.acc_y_calib); 00294 ROS_INFO ("acc_z_calib:%d", IMU_CALCDATA_.acc_z_calib); 00295 ROS_INFO ("acc_x:%d", IMU_CALCDATA_.acc_x); 00296 ROS_INFO ("acc_y:%d", IMU_CALCDATA_.acc_y); 00297 ROS_INFO ("acc_z:%d", IMU_CALCDATA_.acc_z); 00298 ROS_INFO ("acc_angle_nick:%d", IMU_CALCDATA_.acc_angle_nick); 00299 ROS_INFO ("acc_angle_roll:%d", IMU_CALCDATA_.acc_angle_roll); 00300 ROS_INFO ("acc_absolute_value:%d", IMU_CALCDATA_.acc_absolute_value); 00301 ROS_INFO ("Hx:%d", IMU_CALCDATA_.Hx); 00302 ROS_INFO ("Hy:%d", IMU_CALCDATA_.Hy); 00303 ROS_INFO ("Hz:%d", IMU_CALCDATA_.Hz); 00304 ROS_INFO ("mag_heading:%d", IMU_CALCDATA_.mag_heading); 00305 ROS_INFO ("speed_x:%d", IMU_CALCDATA_.speed_x); 00306 ROS_INFO ("speed_y:%d", IMU_CALCDATA_.speed_y); 00307 ROS_INFO ("speed_z:%d", IMU_CALCDATA_.speed_z); 00308 ROS_INFO ("height:%d", IMU_CALCDATA_.height); 00309 ROS_INFO ("dheight:%d", IMU_CALCDATA_.dheight); 00310 ROS_INFO ("dheight_reference:%d", IMU_CALCDATA_.dheight_reference); 00311 ROS_INFO ("height_reference:%d", IMU_CALCDATA_.height_reference); 00312 } 00313 void Telemetry::dumpRC_DATA () 00314 { 00315 ROS_INFO ("RC_DATA"); 00316 ROS_INFO ("--------------------------------"); 00317 ROS_INFO ("channels_in: %d %d %d %d %d %d %d %d", RC_DATA_.channels_in[0], RC_DATA_.channels_in[1], 00318 RC_DATA_.channels_in[2], RC_DATA_.channels_in[3], RC_DATA_.channels_in[4], RC_DATA_.channels_in[5], 00319 RC_DATA_.channels_in[6], RC_DATA_.channels_in[7]); 00320 ROS_INFO ("channels_out: %d %d %d %d %d %d %d %d", RC_DATA_.channels_out[0], RC_DATA_.channels_out[1], 00321 RC_DATA_.channels_out[2], RC_DATA_.channels_out[3], RC_DATA_.channels_out[4], RC_DATA_.channels_out[5], 00322 RC_DATA_.channels_out[6], RC_DATA_.channels_out[7]); 00323 ROS_INFO ("lock:%d", RC_DATA_.lock); 00324 } 00325 void Telemetry::dumpCONTROLLER_OUTPUT () 00326 { 00327 ROS_INFO ("CONTROLLER_OUTPUT"); 00328 ROS_INFO ("--------------------------------"); 00329 ROS_INFO ("nick:%d", CONTROLLER_OUTPUT_.nick); 00330 ROS_INFO ("roll:%d", CONTROLLER_OUTPUT_.roll); 00331 ROS_INFO ("yaw:%d", CONTROLLER_OUTPUT_.yaw); 00332 ROS_INFO ("thrust:%d", CONTROLLER_OUTPUT_.thrust); 00333 } 00334 void Telemetry::dumpGPS_DATA () 00335 { 00336 ROS_INFO ("GPS_DATA"); 00337 ROS_INFO ("--------------------------------"); 00338 ROS_INFO ("latitude:%d", GPS_DATA_.latitude); 00339 ROS_INFO ("longitude:%d", GPS_DATA_.longitude); 00340 ROS_INFO ("height:%d", GPS_DATA_.height); 00341 ROS_INFO ("speed_x:%d", GPS_DATA_.speed_x); 00342 ROS_INFO ("speed_y:%d", GPS_DATA_.speed_y); 00343 ROS_INFO ("heading:%d", GPS_DATA_.heading); 00344 ROS_INFO ("horizontal_accuracy:%d", GPS_DATA_.horizontal_accuracy); 00345 ROS_INFO ("vertical_accuracy:%d", GPS_DATA_.vertical_accuracy); 00346 ROS_INFO ("speed_accuracy:%d", GPS_DATA_.speed_accuracy); 00347 ROS_INFO ("numSV:%d", GPS_DATA_.numSV); 00348 ROS_INFO ("status:%d", GPS_DATA_.status); 00349 } 00350 void Telemetry::dumpGPS_DATA_ADVANCED () 00351 { 00352 ROS_INFO ("GPS_DATA_ADVANCED"); 00353 ROS_INFO ("--------------------------------"); 00354 ROS_INFO ("latitude:%d", GPS_DATA_ADVANCED_.latitude); 00355 ROS_INFO ("longitude:%d", GPS_DATA_ADVANCED_.longitude); 00356 ROS_INFO ("height:%d", GPS_DATA_ADVANCED_.height); 00357 ROS_INFO ("speed_x:%d", GPS_DATA_ADVANCED_.speed_x); 00358 ROS_INFO ("speed_y:%d", GPS_DATA_ADVANCED_.speed_y); 00359 ROS_INFO ("heading:%d", GPS_DATA_ADVANCED_.heading); 00360 ROS_INFO ("horizontal_accuracy:%d", GPS_DATA_ADVANCED_.horizontal_accuracy); 00361 ROS_INFO ("vertical_accuracy:%d", GPS_DATA_ADVANCED_.vertical_accuracy); 00362 ROS_INFO ("speed_accuracy:%d", GPS_DATA_ADVANCED_.speed_accuracy); 00363 ROS_INFO ("numSV:%d", GPS_DATA_ADVANCED_.numSV); 00364 ROS_INFO ("status:%d", GPS_DATA_ADVANCED_.status); 00365 ROS_INFO ("latitude_best_estimate:%d", GPS_DATA_ADVANCED_.latitude_best_estimate); 00366 ROS_INFO ("longitude_best_estimate:%d", GPS_DATA_ADVANCED_.longitude_best_estimate); 00367 ROS_INFO ("speed_x_best_estimate:%d", GPS_DATA_ADVANCED_.speed_x_best_estimate); 00368 ROS_INFO ("speed_y_best_estimate:%d", GPS_DATA_ADVANCED_.speed_y_best_estimate); 00369 } 00370 void Telemetry::dumpCTRL_INPUT () 00371 { 00372 ROS_INFO ("CTRL_INPUT"); 00373 ROS_INFO ("--------------------------------"); 00374 ROS_INFO ("pitch:%d", CTRL_INPUT_.pitch); 00375 ROS_INFO ("roll:%d", CTRL_INPUT_.roll); 00376 ROS_INFO ("yaw:%d", CTRL_INPUT_.yaw); 00377 ROS_INFO ("thrust:%d", CTRL_INPUT_.thrust); 00378 ROS_INFO ("ctrl:%d", CTRL_INPUT_.ctrl); 00379 ROS_INFO ("chksum:%d", CTRL_INPUT_.chksum); 00380 } 00381 void Telemetry::copyLL_STATUS () 00382 { 00383 LLStatus_->battery_voltage_1 = LL_STATUS_.battery_voltage_1; 00384 LLStatus_->battery_voltage_2 = LL_STATUS_.battery_voltage_2; 00385 LLStatus_->status = LL_STATUS_.status; 00386 LLStatus_->cpu_load = LL_STATUS_.cpu_load; 00387 LLStatus_->compass_enabled = LL_STATUS_.compass_enabled; 00388 LLStatus_->chksum_error = LL_STATUS_.chksum_error; 00389 LLStatus_->flying = LL_STATUS_.flying; 00390 LLStatus_->motors_on = LL_STATUS_.motors_on; 00391 LLStatus_->flightMode = LL_STATUS_.flightMode; 00392 LLStatus_->up_time = LL_STATUS_.up_time; 00393 } 00394 void Telemetry::copyIMU_RAWDATA () 00395 { 00396 IMURawData_->pressure = IMU_RAWDATA_.pressure; 00397 IMURawData_->gyro_x = IMU_RAWDATA_.gyro_x; 00398 IMURawData_->gyro_y = IMU_RAWDATA_.gyro_y; 00399 IMURawData_->gyro_z = IMU_RAWDATA_.gyro_z; 00400 IMURawData_->mag_x = IMU_RAWDATA_.mag_x; 00401 IMURawData_->mag_y = IMU_RAWDATA_.mag_y; 00402 IMURawData_->mag_z = IMU_RAWDATA_.mag_z; 00403 IMURawData_->acc_x = IMU_RAWDATA_.acc_x; 00404 IMURawData_->acc_y = IMU_RAWDATA_.acc_y; 00405 IMURawData_->acc_z = IMU_RAWDATA_.acc_z; 00406 IMURawData_->temp_gyro = IMU_RAWDATA_.temp_gyro; 00407 IMURawData_->temp_ADC = IMU_RAWDATA_.temp_ADC; 00408 } 00409 void Telemetry::copyIMU_CALCDATA () 00410 { 00411 IMUCalcData_->angle_nick = IMU_CALCDATA_.angle_nick; 00412 IMUCalcData_->angle_roll = IMU_CALCDATA_.angle_roll; 00413 IMUCalcData_->angle_yaw = IMU_CALCDATA_.angle_yaw; 00414 IMUCalcData_->angvel_nick = IMU_CALCDATA_.angvel_nick; 00415 IMUCalcData_->angvel_roll = IMU_CALCDATA_.angvel_roll; 00416 IMUCalcData_->angvel_yaw = IMU_CALCDATA_.angvel_yaw; 00417 IMUCalcData_->acc_x_calib = IMU_CALCDATA_.acc_x_calib; 00418 IMUCalcData_->acc_y_calib = IMU_CALCDATA_.acc_y_calib; 00419 IMUCalcData_->acc_z_calib = IMU_CALCDATA_.acc_z_calib; 00420 IMUCalcData_->acc_x = IMU_CALCDATA_.acc_x; 00421 IMUCalcData_->acc_y = IMU_CALCDATA_.acc_y; 00422 IMUCalcData_->acc_z = IMU_CALCDATA_.acc_z; 00423 IMUCalcData_->acc_angle_nick = IMU_CALCDATA_.acc_angle_nick; 00424 IMUCalcData_->acc_angle_roll = IMU_CALCDATA_.acc_angle_roll; 00425 IMUCalcData_->acc_absolute_value = IMU_CALCDATA_.acc_absolute_value; 00426 IMUCalcData_->Hx = IMU_CALCDATA_.Hx; 00427 IMUCalcData_->Hy = IMU_CALCDATA_.Hy; 00428 IMUCalcData_->Hz = IMU_CALCDATA_.Hz; 00429 IMUCalcData_->mag_heading = IMU_CALCDATA_.mag_heading; 00430 IMUCalcData_->speed_x = IMU_CALCDATA_.speed_x; 00431 IMUCalcData_->speed_y = IMU_CALCDATA_.speed_y; 00432 IMUCalcData_->speed_z = IMU_CALCDATA_.speed_z; 00433 IMUCalcData_->height = IMU_CALCDATA_.height; 00434 IMUCalcData_->dheight = IMU_CALCDATA_.dheight; 00435 IMUCalcData_->dheight_reference = IMU_CALCDATA_.dheight_reference; 00436 IMUCalcData_->height_reference = IMU_CALCDATA_.height_reference; 00437 } 00438 void Telemetry::copyRC_DATA () 00439 { 00440 for (int i = 0; i < 8; i++) 00441 { 00442 RCData_->channels_in[i] = RC_DATA_.channels_in[i]; 00443 RCData_->channels_out[i] = RC_DATA_.channels_out[i]; 00444 } 00445 RCData_->lock = RC_DATA_.lock; 00446 } 00447 00448 void Telemetry::copyCONTROLLER_OUTPUT () 00449 { 00450 ControllerOutput_->nick = CONTROLLER_OUTPUT_.nick; 00451 ControllerOutput_->roll = CONTROLLER_OUTPUT_.roll; 00452 ControllerOutput_->yaw = CONTROLLER_OUTPUT_.yaw; 00453 ControllerOutput_->thrust = CONTROLLER_OUTPUT_.thrust; 00454 } 00455 00456 void Telemetry::copyGPS_DATA () 00457 { 00458 GPSData_->latitude = GPS_DATA_.latitude; 00459 GPSData_->longitude = GPS_DATA_.longitude; 00460 GPSData_->height = GPS_DATA_.height; 00461 GPSData_->speed_x = GPS_DATA_.speed_x; 00462 GPSData_->speed_y = GPS_DATA_.speed_y; 00463 GPSData_->heading = GPS_DATA_.heading; 00464 GPSData_->horizontal_accuracy = GPS_DATA_.horizontal_accuracy; 00465 GPSData_->vertical_accuracy = GPS_DATA_.vertical_accuracy; 00466 GPSData_->speed_accuracy = GPS_DATA_.speed_accuracy; 00467 GPSData_->numSV = GPS_DATA_.numSV; 00468 GPSData_->status = GPS_DATA_.status; 00469 } 00470 00471 void Telemetry::copyGPS_DATA_ADVANCED () 00472 { 00473 GPSDataAdvanced_->latitude = GPS_DATA_ADVANCED_.latitude; 00474 GPSDataAdvanced_->longitude = GPS_DATA_ADVANCED_.longitude; 00475 GPSDataAdvanced_->height = GPS_DATA_ADVANCED_.height; 00476 GPSDataAdvanced_->speed_x = GPS_DATA_ADVANCED_.speed_x; 00477 GPSDataAdvanced_->speed_y = GPS_DATA_ADVANCED_.speed_y; 00478 GPSDataAdvanced_->heading = GPS_DATA_ADVANCED_.heading; 00479 GPSDataAdvanced_->horizontal_accuracy = GPS_DATA_ADVANCED_.horizontal_accuracy; 00480 GPSDataAdvanced_->vertical_accuracy = GPS_DATA_ADVANCED_.vertical_accuracy; 00481 GPSDataAdvanced_->speed_accuracy = GPS_DATA_ADVANCED_.speed_accuracy; 00482 GPSDataAdvanced_->numSV = GPS_DATA_ADVANCED_.numSV; 00483 GPSDataAdvanced_->status = GPS_DATA_ADVANCED_.status; 00484 GPSDataAdvanced_->latitude_best_estimate = GPS_DATA_ADVANCED_.latitude_best_estimate; 00485 GPSDataAdvanced_->longitude_best_estimate = GPS_DATA_ADVANCED_.longitude_best_estimate; 00486 GPSDataAdvanced_->speed_x_best_estimate = GPS_DATA_ADVANCED_.speed_x_best_estimate; 00487 GPSDataAdvanced_->speed_y_best_estimate = GPS_DATA_ADVANCED_.speed_y_best_estimate; 00488 } 00489 void Telemetry::copyCTRL_INPUT(asctec_msgs::CtrlInput msg){ 00490 CTRL_INPUT_.pitch = msg.pitch; 00491 CTRL_INPUT_.roll = msg.roll; 00492 CTRL_INPUT_.yaw = msg.yaw; 00493 CTRL_INPUT_.thrust = msg.thrust; 00494 CTRL_INPUT_.ctrl = msg.ctrl; 00495 CTRL_INPUT_.chksum = msg.chksum; 00496 //dumpCTRL_INPUT(); 00497 } 00498 }