00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
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
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
00077
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
00098 requestPublisher_[i].publish (LLStatus_);
00099 break;
00100 case RequestTypes::IMU_RAWDATA:
00101 copyIMU_RAWDATA ();
00102 IMURawData_->header.stamp = timestamps_[RequestTypes::IMU_RAWDATA];
00103
00104 requestPublisher_[i].publish (IMURawData_);
00105 break;
00106 case RequestTypes::IMU_CALCDATA:
00107 copyIMU_CALCDATA ();
00108 IMUCalcData_->header.stamp = timestamps_[RequestTypes::IMU_CALCDATA];
00109
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
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
00164 break;
00165 case RequestTypes::CAM_DATA:
00166
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
00497 }
00498 }