Go to the documentation of this file.00001 #include "msg_print.hpp"
00002 #include <iomanip>
00003
00004 typedef unsigned int uint;
00005
00006 std::ostream& operator<<(std::ostream& s, const msp::msg::ApiVersion& api_version) {
00007 s << "#Api Version:" << std::endl;
00008 s << "API: " << api_version.major << "." << api_version.minor << std::endl;
00009 s << "Protocol: " << api_version.protocol << std::endl;
00010 return s;
00011 }
00012
00013 std::ostream& operator<<(std::ostream& s, const msp::msg::FcVariant& fc_variant) {
00014 s << "#FC variant:" << std::endl;
00015 s << "Identifier: " << fc_variant.identifier << std::endl;
00016 return s;
00017 }
00018
00019 std::ostream& operator<<(std::ostream& s, const msp::msg::FcVersion& fc_version) {
00020 s << "#FC version:" << std::endl;
00021 s << "Version: " << fc_version.major << "." << fc_version.minor << "." << fc_version.patch_level << std::endl;
00022 return s;
00023 }
00024
00025 std::ostream& operator<<(std::ostream& s, const msp::msg::BoardInfo& board_info) {
00026 s << "#Board Info:" << std::endl;
00027 s << "Identifier: " << board_info.identifier << std::endl;
00028 s << "Version: " << board_info.version << std::endl;
00029 s << "Type: " << uint(board_info.type) << std::endl;
00030 return s;
00031 }
00032
00033 std::ostream& operator<<(std::ostream& s, const msp::msg::BuildInfo& build_info) {
00034 s << "#Build Info:" << std::endl;
00035 s << "Date: " << build_info.buildDate << std::endl;
00036 s << "Time: " << build_info.buildTime << std::endl;
00037 s << "Git revision: " << build_info.shortGitRevision << std::endl;
00038 return s;
00039 }
00040
00041 std::ostream& operator<<(std::ostream& s, const msp::msg::Feature& feature) {
00042 s << "#Features:" << std::endl;
00043 for(const std::string &f : feature.features) {
00044 s << f << std::endl;
00045 }
00046 return s;
00047 }
00048
00049 std::ostream& operator<<(std::ostream& s, const msp::msg::RxMap& rx_map) {
00050 s << "#Channel mapping:" << std::endl;
00051 for(uint i(0); i<rx_map.map.size(); i++) {
00052 s << i << ": " << uint(rx_map.map[i]) << std::endl;
00053 }
00054 return s;
00055 }
00056
00057 std::ostream& operator<<(std::ostream& s, const msp::msg::Ident& ident) {
00058 std::string type;
00059 switch(ident.type) {
00060 case msp::msg::MultiType::TRI:
00061 type = "Tricopter";
00062 break;
00063 case msp::msg::MultiType::QUADP:
00064 type = "Quadrocopter Plus";
00065 break;
00066 case msp::msg::MultiType::QUADX:
00067 type = "Quadrocopter X";
00068 break;
00069 case msp::msg::MultiType::BI:
00070 type = "BI-copter";
00071 break;
00072 default:
00073 type = "UNDEFINED";
00074 break;
00075 }
00076
00077 s << "#Ident:" << std::endl;
00078
00079 s << "MultiWii Version: "<<ident.version << std::endl
00080 << "MSP Version: "<<ident.msp_version << std::endl
00081 << "Type: " << type << std::endl
00082 << "Capabilities:" << std::endl;
00083
00084 s << " Bind: ";
00085 ident.hasBind() ? s<<"ON" : s<< "OFF";
00086 s << std::endl;
00087
00088 s << " DynBal: ";
00089 ident.hasDynBal() ? s<<"ON" : s<< "OFF";
00090 s << std::endl;
00091
00092 s << " Flap: ";
00093 ident.hasFlap() ? s<<"ON" : s<< "OFF";
00094 s << std::endl;
00095
00096 return s;
00097 }
00098
00099 std::ostream& operator<<(std::ostream& s, const msp::msg::Status& status) {
00100 s << "#Status:" << std::endl;
00101 s << "Cycle time: " << status.time<< " us" << std::endl;
00102 s << "I2C errors: " << status.errors<< std::endl;
00103 s << "Sensors:" << std::endl;
00104
00105 s << " Accelerometer: ";
00106 status.hasAccelerometer() ? s<<"ON" : s<< "OFF";
00107 s << std::endl;
00108
00109 s << " Barometer: ";
00110 status.hasBarometer() ? s<<"ON" : s<< "OFF";
00111 s << std::endl;
00112
00113 s << " Magnetometer: ";
00114 status.hasMagnetometer() ? s<<"ON" : s<< "OFF";
00115 s << std::endl;
00116
00117 s << " GPS: ";
00118 status.hasGPS() ? s<<"ON" : s<< "OFF";
00119 s << std::endl;
00120
00121 s << " Sonar: ";
00122 status.hasSonar() ? s<<"ON" : s<< "OFF";
00123 s << std::endl;
00124
00125 s << "Active Boxes (by ID):";
00126 for(const uint box_id : status.active_box_id) {
00127 s << " " << box_id;
00128 }
00129 s << std::endl;
00130
00131 return s;
00132 }
00133
00134 std::ostream& operator<<(std::ostream& s, const msp::msg::ImuRaw& imu) {
00135 s << "#Imu:" << std::endl;
00136 s << "Linear acceleration: " << imu.acc[0] << ", " << imu.acc[1] << ", " << imu.acc[2] << std::endl;
00137 s << "Angular velocity: " << imu.gyro[0] << ", " << imu.gyro[1] << ", " << imu.gyro[2] << std::endl;
00138 s << "Magnetometer: " << imu.magn[0] << ", " << imu.magn[1] << ", " << imu.magn[2] << std::endl;
00139 return s;
00140 }
00141
00142 std::ostream& operator<<(std::ostream& s, const msp::msg::ImuSI& imu) {
00143 s << "#Imu:" << std::endl;
00144 s << "Linear acceleration: " << imu.acc[0] << ", " << imu.acc[1] << ", " << imu.acc[2] << " m/s²" << std::endl;
00145 s << "Angular velocity: " << imu.gyro[0] << ", " << imu.gyro[1] << ", " << imu.gyro[2] << " deg/s" << std::endl;
00146 s << "Magnetometer: " << imu.magn[0] << ", " << imu.magn[1] << ", " << imu.magn[2] << " uT" << std::endl;
00147 return s;
00148 }
00149
00150 std::ostream& operator<<(std::ostream& s, const msp::msg::Servo& servo) {
00151 s << "#Servo:" << std::endl;
00152 s << servo.servo[0] << " " << servo.servo[1] << " " << servo.servo[2] << " " << servo.servo[3] << std::endl;
00153 s << servo.servo[4] << " " << servo.servo[5] << " " << servo.servo[6] << " " << servo.servo[7] << std::endl;
00154 return s;
00155 }
00156
00157 std::ostream& operator<<(std::ostream& s, const msp::msg::Motor& motor) {
00158 s << "#Motor:" << std::endl;
00159 s << motor.motor[0] << " " << motor.motor[1] << " " << motor.motor[2] << " " << motor.motor[3] << std::endl;
00160 s << motor.motor[4] << " " << motor.motor[5] << " " << motor.motor[6] << " " << motor.motor[7] << std::endl;
00161 return s;
00162 }
00163
00164 std::ostream& operator<<(std::ostream& s, const msp::msg::Rc& rc) {
00165 s << "#Rc channels (" << rc.channels.size() << ") :" << std::endl;
00166 for(const uint16_t c : rc.channels) { s << c << " "; }
00167 s << std::endl;
00168 return s;
00169 }
00170
00171 std::ostream& operator<<(std::ostream& s, const msp::msg::Attitude& attitude) {
00172 s << "#Attitude:" << std::endl;
00173 s << "Ang : " << attitude.ang_x << ", " << attitude.ang_y << " deg" << std::endl;
00174 s << "Heading: " << attitude.heading << " deg" << std::endl;
00175 return s;
00176 }
00177
00178 std::ostream& operator<<(std::ostream& s, const msp::msg::Altitude& altitude) {
00179 s << "#Altitude:" << std::endl;
00180 s << "Altitude: " << altitude.altitude << " m, var: " << altitude.vario << " m/s" << std::endl;
00181 return s;
00182 }
00183
00184 std::ostream& operator<<(std::ostream& s, const msp::msg::Analog& analog) {
00185 s << "#Analog:" << std::endl;
00186 s << "Battery Voltage: " << analog.vbat << " V" << std::endl;
00187 s << "Current: " << analog.amperage << " A" << std::endl;
00188 s << "Power consumption: " << analog.powerMeterSum << " Ah" << std::endl;
00189 s << "RSSI: " << analog.rssi << std::endl;
00190 return s;
00191 }
00192
00193 std::ostream& operator<<(std::ostream& s, const msp::msg::RcTuning& rc_tuning) {
00194 s << "#Rc Tuning:" << std::endl;
00195 s << "Rc Rate: " << rc_tuning.RC_RATE << std::endl;
00196 s << "Rc Expo: " << rc_tuning.RC_EXPO << std::endl;
00197 s << "Roll/Pitch Rate: " << rc_tuning.RollPitchRate << std::endl;
00198 s << "Yaw Rate: " << rc_tuning.YawRate << std::endl;
00199
00200 s << "Dynamic Throttle PID: " << rc_tuning.DynThrPID << std::endl;
00201 s << "Throttle MID: " << rc_tuning.Throttle_MID << std::endl;
00202 s << "Throttle Expo: " << rc_tuning.Throttle_EXPO << std::endl;
00203
00204 return s;
00205 }
00206
00207 std::ostream& operator<<(std::ostream& s, const msp::msg::Pid& pid) {
00208 s << std::setprecision(3);
00209 s << "#PID:" << std::endl;
00210 s << "Name P | I | D |" << std::endl;
00211 s << "----------------|-------|-------|" << std::endl;
00212 s << "Roll: " << pid.roll.P << "\t| " << pid.roll.I << "\t| " << pid.roll.D << std::endl;
00213 s << "Pitch: " << pid.pitch.P << "\t| " << pid.pitch.I << "\t| " << pid.pitch.D << std::endl;
00214 s << "Yaw: " << pid.yaw.P << "\t| " << pid.yaw.I << "\t| " << pid.yaw.D << std::endl;
00215 s << "Altitude: " << pid.alt.P << "\t| " << pid.alt.I << "\t| " << pid.alt.D << std::endl;
00216
00217 s << "Position: " << pid.pos.P << "\t| " << pid.pos.I << "\t| " << pid.pos.D << std::endl;
00218 s << "PositionR: " << pid.posr.P << "\t| " << pid.posr.I << "\t| " << pid.posr.D << std::endl;
00219 s << "NavR: " << pid.navr.P << "\t| " << pid.navr.I << "\t| " << pid.navr.D << std::endl;
00220 s << "Level: " << pid.level.P << "\t| " << pid.level.I << "\t| " << pid.level.D << std::endl;
00221 s << "Magn: " << pid.mag.P << "\t| " << pid.mag.I << "\t| " << pid.mag.D << std::endl;
00222 s << "Vel: " << pid.vel.P << "\t| " << pid.vel.I << "\t| " << pid.vel.D << std::endl;
00223
00224 return s;
00225 }
00226
00227 std::ostream& operator<<(std::ostream& s, const msp::msg::Box& box) {
00228 s << "#Box:" << std::endl;
00229 for(uint ibox(0); ibox<box.box_pattern.size(); ibox++) {
00230 s << ibox << " ";
00231 for(uint iaux(0); iaux<box.box_pattern[ibox].size(); iaux++) {
00232 s << "aux" << iaux+1 << ": ";
00233 if(box.box_pattern[ibox][iaux].count(msp::msg::SwitchPosition::LOW))
00234 s << "L";
00235 else
00236 s << "_";
00237 if(box.box_pattern[ibox][iaux].count(msp::msg::SwitchPosition::MID))
00238 s << "M";
00239 else
00240 s << "_";
00241 if(box.box_pattern[ibox][iaux].count(msp::msg::SwitchPosition::HIGH))
00242 s << "H";
00243 else
00244 s << "_";
00245 s << ", ";
00246 }
00247 s << std::endl;
00248 }
00249
00250 return s;
00251 }
00252
00253 std::ostream& operator<<(std::ostream& s, const msp::msg::Misc& misc) {
00254 s << "#Miscellaneous:" << std::endl;
00255 s << "Power Trigger: " << misc.powerTrigger << std::endl;
00256 s << "Min Throttle: " << misc.minThrottle << std::endl;
00257 s << "Max Throttle: " << misc.maxThrottle << std::endl;
00258 s << "Failsafe Throttle: " << misc.failsafeThrottle << std::endl;
00259
00260 s << "Arm Counter: " << misc.arm << std::endl;
00261 s << "Lifetime: " << misc.lifetime << std::endl;
00262
00263 s << "Magnetic Declination: " << misc.mag_declination << " deg" << std::endl;
00264 s << "Battery Voltage Scale: " << misc.vbatScale << " V" << std::endl;
00265 s << "Battery Warning Level 1: " << misc.vbatLevelWarn1 << " V" << std::endl;
00266 s << "Battery Warning Level 2: " << misc.vbatLevelWarn2 << " V" << std::endl;
00267 s << "Battery Critical Level: " << misc.vbatLevelCrit << " V" << std::endl;
00268
00269 return s;
00270 }
00271
00272 std::ostream& operator<<(std::ostream& s, const msp::msg::MotorPins& pin) {
00273 s << "#Motor pins:" << std::endl;
00274 for(uint imotor(0); imotor<msp::msg::N_MOTOR; imotor++) {
00275 s << "Motor " << imotor << ": pin " << (uint)pin.pwm_pin[imotor] << std::endl;
00276 }
00277
00278 return s;
00279 }
00280
00281 std::ostream& operator<<(std::ostream& s, const msp::msg::BoxNames& box_names) {
00282 s << "#Box names:" << std::endl;
00283 for(uint ibox(0); ibox<box_names.box_names.size(); ibox++) {
00284 s << ibox << ": " << box_names.box_names[ibox] << std::endl;
00285 }
00286 return s;
00287 }
00288
00289 std::ostream& operator<<(std::ostream& s, const msp::msg::PidNames& pid_names) {
00290 s << "#PID names:" << std::endl;
00291 for(uint ipid(0); ipid<pid_names.pid_names.size(); ipid++) {
00292 s << ipid << ": " << pid_names.pid_names[ipid] << std::endl;
00293 }
00294 return s;
00295 }
00296
00297 std::ostream& operator<<(std::ostream& s, const msp::msg::BoxIds& box_ids) {
00298 s << "#Box IDs:" << std::endl;
00299 for(uint ibox(0); ibox<box_ids.box_ids.size(); ibox++) {
00300 s << ibox << ": " << (uint)box_ids.box_ids[ibox] << std::endl;
00301 }
00302 return s;
00303 }
00304
00305 std::ostream& operator<<(std::ostream& s, const msp::msg::ServoConf& servo_conf) {
00306 s << "#Servo conf:" << std::endl;
00307 s << "Nr. | [min | middle | max] (rate)" << std::endl;
00308 for(uint iservo(0); iservo<msp::msg::N_SERVO; iservo++) {
00309 const msp::msg::ServoConfRange servo = servo_conf.servo_conf[iservo];
00310 s << iservo << ": | "<< "["<< servo.min <<" | "<< servo.middle <<" | "<< servo.max <<"] ("<< (uint)servo.rate <<")" << std::endl;
00311 }
00312 return s;
00313 }
00314
00315 std::ostream& operator<<(std::ostream& s, const msp::msg::Debug& debug) {
00316 s << "#Debug:" << std::endl;
00317 s << "debug1: " << debug.debug1 << std::endl;
00318 s << "debug2: " << debug.debug2 << std::endl;
00319 s << "debug3: " << debug.debug3 << std::endl;
00320 s << "debug4: " << debug.debug4 << std::endl;
00321 return s;
00322 }