diff --git a/src/LogCompressor.cc b/src/LogCompressor.cc index 0d4ebdb9f603042e2ce36fceabe4d684df77706d..84acff29396852c96cb435835d1b6d09c3e52436 100644 --- a/src/LogCompressor.cc +++ b/src/LogCompressor.cc @@ -214,7 +214,7 @@ void LogCompressor::run() } } - if (index % (dataLines/100) == 0) emit logProcessingStatusChanged(tr("Log compressor: Processed %1% of %2 lines").arg(index/(float)dataLines*100, 0, 'f', 2).arg(dataLines)); + if (dataLines > 100) if (index % (dataLines/100) == 0) emit logProcessingStatusChanged(tr("Log compressor: Processed %1% of %2 lines").arg(index/(float)dataLines*100, 0, 'f', 2).arg(dataLines)); if (!failed) { diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index bb3b9452c384cdb2129de607eaebfb0104aefe53..a8043d8ceb42076240856bec2ae54217230022ec 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -375,13 +375,20 @@ void MAVLinkSimulationLink::mainloop() { rate10hzCounter = 1; - + float lastX = x; + float lastY = y; + float lastZ = z; + float hackDt = 0.1f; // 100 ms // Move X Position x = 12.0*sin(((double)circleCounter)/100.0); y = 5.0*cos(((double)circleCounter)/100.0); z = 1.8 + 1.2*sin(((double)circleCounter)/60.0); + float xSpeed = (x - lastX)/hackDt; + float ySpeed = (y - lastY)/hackDt; + float zSpeed = (z - lastZ)/hackDt; + circleCounter++; @@ -402,7 +409,7 @@ void MAVLinkSimulationLink::mainloop() streampointer += bufferlength; // Send back new position - mavlink_msg_local_position_pack(systemId, componentId, &ret, 0, x, y, -fabs(z), 0, 0, 0); + mavlink_msg_local_position_pack(systemId, componentId, &ret, 0, x, y, -fabs(z), xSpeed, ySpeed, zSpeed); bufferlength = mavlink_msg_to_send_buffer(buffer, &ret); //add data into datastream memcpy(stream+streampointer,buffer, bufferlength); @@ -416,14 +423,14 @@ void MAVLinkSimulationLink::mainloop() // streampointer += bufferlength; // GLOBAL POSITION - mavlink_msg_global_position_int_pack(systemId, componentId, &ret, (473780.28137103+(x))*1E3, (85489.9892510421+(y))*1E3, (z+550.0)*1000.0, 0*100.0, 0*100.0, 0*100.0); + mavlink_msg_global_position_int_pack(systemId, componentId, &ret, (473780.28137103+(x))*1E3, (85489.9892510421+(y))*1E3, (z+550.0)*1000.0, xSpeed*15.0*100.0, ySpeed*15.0*100.0, zSpeed*15*100.0); bufferlength = mavlink_msg_to_send_buffer(buffer, &ret); //add data into datastream memcpy(stream+streampointer,buffer, bufferlength); streampointer += bufferlength; // GLOBAL POSITION VEHICLE 2 - mavlink_msg_global_position_int_pack(54, componentId, &ret, (473780.28137103+(x+0.002))*1E3, (85489.9892510421+((y/2)+0.3))*1E3, (z+570.0)*1000.0, 0*100.0, 0*100.0, 0*100.0); + mavlink_msg_global_position_int_pack(54, componentId, &ret, (473780.28137103+(x+0.002))*1E3, (85489.9892510421+((y/2)+0.3))*1E3, (z+570.0)*1000.0, xSpeed*15.0*100.0, ySpeed*15.0*100.0, zSpeed*15.0*100.0); bufferlength = mavlink_msg_to_send_buffer(buffer, &ret); //add data into datastream memcpy(stream+streampointer,buffer, bufferlength); diff --git a/src/comm/MAVLinkXMLParser.cc b/src/comm/MAVLinkXMLParser.cc index cca446668350a87461cf71bd55112a4ab5c1afcd..34b830fe66579da12eb88d5dad3eda3255162f54 100644 --- a/src/comm/MAVLinkXMLParser.cc +++ b/src/comm/MAVLinkXMLParser.cc @@ -464,7 +464,7 @@ bool MAVLinkXMLParser::generate() // Add field to C structure cStructLines += QString("\t%1 %2[%3]; ///< %4\n").arg(arrayType, fieldName, QString::number(arrayLength), fieldText); // Add pack line to message_xx_pack function - packLines += QString("\ti += put_array_by_index((int8_t*)%1, sizeof(%2)*%3, i, msg->payload); // %4\n").arg(fieldName, arrayType, QString::number(arrayLength), fieldText); + packLines += QString("\ti += put_array_by_index((const int8_t*)%1, sizeof(%2)*%3, i, msg->payload); // %4\n").arg(fieldName, arrayType, QString::number(arrayLength), fieldText); // Add decode function for this type decodeLines += QString("\tmavlink_msg_%1_get_%2(msg, %1->%2);\n").arg(messageName, fieldName); arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3\n").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc index bde4e9e52b6be86f8d5796e961ad7abc535a7999..ff06c5cbf2415fc40ff9d622ee6b52130e9a6956 100644 --- a/src/uas/PxQuadMAV.cc +++ b/src/uas/PxQuadMAV.cc @@ -60,8 +60,8 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_raw_aux_t raw; mavlink_msg_raw_aux_decode(&message, &raw); quint64 time = getUnixTime(0); - emit valueChanged(uasId, "Pressure", raw.baro, time); - emit valueChanged(uasId, "Temperature", raw.temp, time); + emit valueChanged(uasId, "Pressure", "raw", raw.baro, time); + emit valueChanged(uasId, "Temperature", "raw", raw.temp, time); } break; case MAVLINK_MSG_ID_PATTERN_DETECTED: @@ -110,12 +110,12 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_msg_vision_position_estimate_decode(&message, &pos); quint64 time = getUnixTime(pos.usec); //emit valueChanged(uasId, "vis. time", pos.usec, time); - emit valueChanged(uasId, "vis. roll", pos.roll, time); - emit valueChanged(uasId, "vis. pitch", pos.pitch, time); - emit valueChanged(uasId, "vis. yaw", pos.yaw, time); - emit valueChanged(uasId, "vis. x", pos.x, time); - emit valueChanged(uasId, "vis. y", pos.y, time); - emit valueChanged(uasId, "vis. z", pos.z, time); + emit valueChanged(uasId, "vis. roll", "rad", pos.roll, time); + emit valueChanged(uasId, "vis. pitch", "rad", pos.pitch, time); + emit valueChanged(uasId, "vis. yaw", "rad", pos.yaw, time); + emit valueChanged(uasId, "vis. x", "rad/s", pos.x, time); + emit valueChanged(uasId, "vis. y", "rad/s", pos.y, time); + emit valueChanged(uasId, "vis. z", "rad/s", pos.z, time); } break; case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE: @@ -124,12 +124,12 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_msg_vicon_position_estimate_decode(&message, &pos); quint64 time = getUnixTime(pos.usec); //emit valueChanged(uasId, "vis. time", pos.usec, time); - emit valueChanged(uasId, "vicon roll", pos.roll, time); - emit valueChanged(uasId, "vicon pitch", pos.pitch, time); - emit valueChanged(uasId, "vicon yaw", pos.yaw, time); - emit valueChanged(uasId, "vicon x", pos.x, time); - emit valueChanged(uasId, "vicon y", pos.y, time); - emit valueChanged(uasId, "vicon z", pos.z, time); + emit valueChanged(uasId, "vicon roll", "rad", pos.roll, time); + emit valueChanged(uasId, "vicon pitch", "rad", pos.pitch, time); + emit valueChanged(uasId, "vicon yaw", "rad", pos.yaw, time); + emit valueChanged(uasId, "vicon x", "rad/s", pos.x, time); + emit valueChanged(uasId, "vicon y", "rad/s", pos.y, time); + emit valueChanged(uasId, "vicon z", "rad/s", pos.z, time); emit localPositionChanged(this, pos.x, pos.y, pos.z, time); } break; @@ -143,7 +143,7 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) emit errCountChanged(uasId, "IMU", "SPI0", status.spi0_err_count); emit errCountChanged(uasId, "IMU", "SPI1", status.spi1_err_count); emit errCountChanged(uasId, "IMU", "UART", status.uart_total_err_count); - emit UAS::valueChanged(this, "Load", ((float)status.load)/1000.0f, MG::TIME::getGroundTimeNow()); + emit valueChanged(uasId, "Load", "%", ((float)status.load)/1000.0f, MG::TIME::getGroundTimeNow()); } break; case MAVLINK_MSG_ID_CONTROL_STATUS: diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index afb6ec4d0226f6ef6a0882732dd2481290de113d..867e8f3c3bfec9033050f6ccc29ae59bb54707dc 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -181,7 +181,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit statusChanged(this, uasState, stateDescription); emit statusChanged(this->status); emit loadChanged(this,state.load/10.0f); - emit UAS::valueChanged(uasId, "Load", ((float)state.load)/1000.0f, MG::TIME::getGroundTimeNow()); + emit valueChanged(uasId, "Load", "%", ((float)state.load)/1000.0f, MG::TIME::getGroundTimeNow()); stateAudio = " changed status to " + uasState; } @@ -279,15 +279,15 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_msg_raw_imu_decode(&message, &raw); quint64 time = getUnixTime(raw.usec); - emit valueChanged(uasId, "accel x", raw.xacc, time); - emit valueChanged(uasId, "accel y", raw.yacc, time); - emit valueChanged(uasId, "accel z", raw.zacc, time); - emit valueChanged(uasId, "gyro roll", static_cast(raw.xgyro), time); - emit valueChanged(uasId, "gyro pitch", static_cast(raw.ygyro), time); - emit valueChanged(uasId, "gyro yaw", static_cast(raw.zgyro), time); - emit valueChanged(uasId, "mag x", raw.xmag, time); - emit valueChanged(uasId, "mag y", raw.ymag, time); - emit valueChanged(uasId, "mag z", raw.zmag, time); + emit valueChanged(uasId, "accel x", "raw", raw.xacc, time); + emit valueChanged(uasId, "accel y", "raw", raw.yacc, time); + emit valueChanged(uasId, "accel z", "raw", raw.zacc, time); + emit valueChanged(uasId, "gyro roll", "raw", static_cast(raw.xgyro), time); + emit valueChanged(uasId, "gyro pitch", "raw", static_cast(raw.ygyro), time); + emit valueChanged(uasId, "gyro yaw", "raw", static_cast(raw.zgyro), time); + emit valueChanged(uasId, "mag x", "raw", raw.xmag, time); + emit valueChanged(uasId, "mag y", "raw", raw.ymag, time); + emit valueChanged(uasId, "mag z", "raw", raw.zmag, time); } break; case MAVLINK_MSG_ID_ATTITUDE: @@ -303,20 +303,20 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) // emit valueChanged(uasId, "roll IMU", mavlink_msg_attitude_get_roll(&message), time); // emit valueChanged(uasId, "pitch IMU", mavlink_msg_attitude_get_pitch(&message), time); // emit valueChanged(uasId, "yaw IMU", mavlink_msg_attitude_get_yaw(&message), time); - emit valueChanged(uasId, "roll (rad)", mavlink_msg_attitude_get_roll(&message), time); - emit valueChanged(uasId, "pitch (rad)", mavlink_msg_attitude_get_pitch(&message), time); - emit valueChanged(uasId, "yaw (rad)", mavlink_msg_attitude_get_yaw(&message), time); - emit valueChanged(uasId, "rollspeed (rad)", attitude.rollspeed, time); - emit valueChanged(uasId, "pitchspeed (rad)", attitude.pitchspeed, time); - emit valueChanged(uasId, "yawspeed (rad)", attitude.yawspeed, time); + emit valueChanged(uasId, "roll", "rad", mavlink_msg_attitude_get_roll(&message), time); + emit valueChanged(uasId, "pitch", "rad", mavlink_msg_attitude_get_pitch(&message), time); + emit valueChanged(uasId, "yaw", "rad", mavlink_msg_attitude_get_yaw(&message), time); + emit valueChanged(uasId, "rollspeed", "rad/s", attitude.rollspeed, time); + emit valueChanged(uasId, "pitchspeed", "rad/s", attitude.pitchspeed, time); + emit valueChanged(uasId, "yawspeed", "rad/s", attitude.yawspeed, time); // Emit in angles - emit valueChanged(uasId, "roll", (attitude.roll/M_PI)*180.0, time); - emit valueChanged(uasId, "pitch", (attitude.pitch/M_PI)*180.0, time); - emit valueChanged(uasId, "yaw", (attitude.yaw/M_PI)*180.0, time); - emit valueChanged(uasId, "rollspeed", (attitude.rollspeed/M_PI)*180.0, time); - emit valueChanged(uasId, "pitchspeed", (attitude.pitchspeed/M_PI)*180.0, time); - emit valueChanged(uasId, "yawspeed", (attitude.yawspeed/M_PI)*180.0, time); + emit valueChanged(uasId, "roll", "deg", (attitude.roll/M_PI)*180.0, time); + emit valueChanged(uasId, "pitch", "deg", (attitude.pitch/M_PI)*180.0, time); + emit valueChanged(uasId, "yaw", "deg", (attitude.yaw/M_PI)*180.0, time); + emit valueChanged(uasId, "rollspeed", "deg/s", (attitude.rollspeed/M_PI)*180.0, time); + emit valueChanged(uasId, "pitchspeed", "deg/s", (attitude.pitchspeed/M_PI)*180.0, time); + emit valueChanged(uasId, "yawspeed", "deg/s", (attitude.yawspeed/M_PI)*180.0, time); emit attitudeChanged(this, attitude.roll, attitude.pitch, attitude.yaw, time); } @@ -331,12 +331,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) localX = pos.x; localY = pos.y; localZ = pos.z; - emit valueChanged(uasId, "x", pos.x, time); - emit valueChanged(uasId, "y", pos.y, time); - emit valueChanged(uasId, "z", pos.z, time); - emit valueChanged(uasId, "x speed", pos.vx, time); - emit valueChanged(uasId, "y speed", pos.vy, time); - emit valueChanged(uasId, "z speed", pos.vz, time); + emit valueChanged(uasId, "x", "m", pos.x, time); + emit valueChanged(uasId, "y", "m", pos.y, time); + emit valueChanged(uasId, "z", "m", pos.z, time); + emit valueChanged(uasId, "x speed", "m/s", pos.vx, time); + emit valueChanged(uasId, "y speed", "m/s", pos.vy, time); + emit valueChanged(uasId, "z speed", "m/s", pos.vz, time); emit localPositionChanged(this, pos.x, pos.y, pos.z, time); emit speedChanged(this, pos.vx, pos.vy, pos.vz, time); @@ -366,12 +366,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) speedX = pos.vx/100.0; speedY = pos.vy/100.0; speedZ = pos.vz/100.0; - emit valueChanged(uasId, "latitude", pos.lat, time); - emit valueChanged(uasId, "longitude", pos.lon, time); - emit valueChanged(uasId, "altitutde", pos.alt, time); - emit valueChanged(uasId, "gps x speed", speedX, time); - emit valueChanged(uasId, "gps y speed", speedY, time); - emit valueChanged(uasId, "gps z speed", speedZ, time); + emit valueChanged(uasId, "latitude", "deg", latitude, time); + emit valueChanged(uasId, "longitude", "deg", longitude, time); + emit valueChanged(uasId, "altitude", "m", altitude, time); + emit valueChanged(uasId, "gps x speed", "m/s", speedX, time); + emit valueChanged(uasId, "gps y speed", "m/s", speedY, time); + emit valueChanged(uasId, "gps z speed", "m/s", speedZ, time); emit globalPositionChanged(this, longitude, latitude, altitude, time); emit speedChanged(this, speedX, speedY, speedZ, time); // Set internal state @@ -397,8 +397,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) // quint64 time = getUnixTime(pos.usec); quint64 time = MG::TIME::getGroundTimeNow(); - emit valueChanged(uasId, "latitude", pos.lat, time); - emit valueChanged(uasId, "longitude", pos.lon, time); + emit valueChanged(uasId, "latitude", "deg", pos.lat, time); + emit valueChanged(uasId, "longitude", "deg", pos.lon, time); if (pos.fix_type > 0) { @@ -411,11 +411,11 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) alt = 0; emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN FOR ALTITUDE"); } - emit valueChanged(uasId, "altitude", pos.alt, time); + emit valueChanged(uasId, "altitude", "m", pos.alt, time); // Smaller than threshold and not NaN if (pos.v < 1000000 && pos.v == pos.v) { - emit valueChanged(uasId, "speed", pos.v, time); + emit valueChanged(uasId, "speed", "m/s", pos.v, time); //qDebug() << "GOT GPS RAW"; emit speedChanged(this, (double)pos.v, 0.0, 0.0, time); } @@ -447,9 +447,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { mavlink_raw_pressure_t pressure; mavlink_msg_raw_pressure_decode(&message, &pressure); - emit valueChanged(uasId, "abs pressure", pressure.press_abs, this->getUnixTime(0)); - emit valueChanged(uasId, "diff pressure 1", pressure.press_diff1, this->getUnixTime(0)); - emit valueChanged(uasId, "diff pressure 2", pressure.press_diff2, this->getUnixTime(0)); + quint64 time = this->getUnixTime(0); + emit valueChanged(uasId, "abs pressure", "hP", pressure.press_abs, time); + emit valueChanged(uasId, "diff pressure 1", "hP", pressure.press_diff1, time); + emit valueChanged(uasId, "diff pressure 2", "hP", pressure.press_diff2, time); } break; case MAVLINK_MSG_ID_RC_CHANNELS_RAW: @@ -506,7 +507,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) } break; case MAVLINK_MSG_ID_DEBUG: - emit valueChanged(uasId, QString("debug ") + QString::number(mavlink_msg_debug_get_ind(&message)), mavlink_msg_debug_get_value(&message), MG::TIME::getGroundTimeNow()); + emit valueChanged(uasId, QString("debug ") + QString::number(mavlink_msg_debug_get_ind(&message)), "raw", mavlink_msg_debug_get_value(&message), MG::TIME::getGroundTimeNow()); break; case MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT: { @@ -514,9 +515,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_msg_attitude_controller_output_decode(&message, &out); quint64 time = MG::TIME::getGroundTimeNowUsecs(); emit attitudeThrustSetPointChanged(this, out.roll/127.0f, out.pitch/127.0f, out.yaw/127.0f, (uint8_t)out.thrust, time); - emit valueChanged(uasId, "att control roll", out.roll, time/1000.0f); - emit valueChanged(uasId, "att control pitch", out.pitch, time/1000.0f); - emit valueChanged(uasId, "att control yaw", out.yaw, time/1000.0f); + emit valueChanged(uasId, "att control roll", "raw", out.roll, time/1000.0f); + emit valueChanged(uasId, "att control pitch", "raw", out.pitch, time/1000.0f); + emit valueChanged(uasId, "att control yaw", "raw", out.yaw, time/1000.0f); } break; case MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT: @@ -525,9 +526,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_msg_position_controller_output_decode(&message, &out); quint64 time = MG::TIME::getGroundTimeNow(); //emit positionSetPointsChanged(uasId, out.x/127.0f, out.y/127.0f, out.z/127.0f, out.yaw, time); - emit valueChanged(uasId, "pos control x", out.x, time); - emit valueChanged(uasId, "pos control y", out.y, time); - emit valueChanged(uasId, "pos control z", out.z, time); + emit valueChanged(uasId, "pos control x", "raw", out.x, time); + emit valueChanged(uasId, "pos control y", "raw", out.y, time); + emit valueChanged(uasId, "pos control z", "raw", out.z, time); } break; case MAVLINK_MSG_ID_WAYPOINT_COUNT: @@ -618,9 +619,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_msg_debug_vect_decode(&message, &vect); QString str((const char*)vect.name); quint64 time = getUnixTime(vect.usec); - emit valueChanged(uasId, str+".x", vect.x, time); - emit valueChanged(uasId, str+".y", vect.y, time); - emit valueChanged(uasId, str+".z", vect.z, time); + emit valueChanged(uasId, str+".x", "raw", vect.x, time); + emit valueChanged(uasId, str+".y", "raw", vect.y, time); + emit valueChanged(uasId, str+".z", "raw", vect.z, time); } break; //#ifdef MAVLINK_ENABLED_PIXHAWK diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 0b00419907cbe0d5b7f3af47a2f9250683bbb325..a35dd96d5bbb7ed2f76fe95c48158ea42d964de1 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -328,9 +328,9 @@ signals: * @param msec the timestamp of the message, in milliseconds */ - // FIXME Exchange the lines below against the commented ones - void valueChanged(int uasId, QString name, double value, quint64 msec); - void valueChanged(UASInterface* uas, QString name, double value, quint64 msec); + //void valueChanged(const int uasId, const QString& name, const double value, const quint64 msec); + void valueChanged(const int uasId, const QString& name, const QString& unit, const double value, const quint64 msec); + void valueChanged(const int uasId, const QString& name, const QString& unit, const int value, const quint64 msec); // void valueChanged(const int uasId, const QString& name, const double value, const quint64 msec); // //void valueChanged(UASInterface* uas, QString name, double value, quint64 msec); diff --git a/src/ui/HUD.cc b/src/ui/HUD.cc index c2b4a07936c475bf03ef8adb137b2f6e125da078..9a57336ec44443aca63e718b73015c504a3ecb60 100644 --- a/src/ui/HUD.cc +++ b/src/ui/HUD.cc @@ -250,6 +250,10 @@ void HUD::setActiveUAS(UASInterface* uas) disconnect(uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString))); disconnect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); disconnect(uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*))); + + disconnect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); + disconnect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); + disconnect(uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int))); } // Now connect the new UAS @@ -260,11 +264,15 @@ void HUD::setActiveUAS(UASInterface* uas) connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); connect(uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*))); + connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int))); + // Set new UAS this->uas = uas; } -void HUD::updateAttitudeThrustSetPoint(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 msec) +void HUD::updateAttitudeThrustSetPoint(UASInterface* uas, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 msec) { updateValue(uas, "roll desired", rollDesired, msec); updateValue(uas, "pitch desired", pitchDesired, msec); @@ -711,7 +719,9 @@ void HUD::paintHUD() QString yawAngle; // const float yawDeg = ((values.value("yaw", 0.0f)/M_PI)*180.0f)+180.f; - const float yawDeg = ((values.value("yaw", 0.0f)/M_PI)*180.0f); + + // YAW is in compass-human readable format, so 0 - 360deg. This is normal in aviation, not -180 - +180. + const float yawDeg = ((values.value("yaw", 0.0f)/M_PI)*180.0f)+180.0f; yawAngle.sprintf("%03d", (int)yawDeg); paintText(yawAngle, defaultColor, 3.5f, -3.7f, compassY+ 0.9f, &painter); @@ -730,6 +740,9 @@ void HUD::paintHUD() drawChangeIndicatorGauge(vGaugeSpacing, -15.0f, 10.0f, 5.0f, values.value("xSpeed", 0.0f), defaultColor, &painter, false); + // Waypoint name + if (waypointName != "") paintText(waypointName, defaultColor, 2.0f, (-vwidth/3.0) + 10, +vheight/3.0 + 15, &painter); + // MOVING PARTS @@ -1351,12 +1364,10 @@ void HUD::resizeGL(int w, int h) paintHUD(); } -void HUD::selectWaypoint(UASInterface* uas, int id) +void HUD::selectWaypoint(int uasId, int id) { - if (uas == this->uas) - { - waypointName = tr("WP") + QString::number(id); - } + Q_UNUSED(uasId); + waypointName = tr("WP") + QString::number(id); } void HUD::setImageSize(int width, int height, int depth, int channels) diff --git a/src/ui/HUD.h b/src/ui/HUD.h index ce647c910d8a4b042439fee29f9cb1697fcdab24..1797b77078edf45360dafc2b978e5551c9ce4e68 100644 --- a/src/ui/HUD.h +++ b/src/ui/HUD.h @@ -77,7 +77,7 @@ public slots: void updateState(UASInterface*,QString); void updateMode(int id,QString mode, QString description); void updateLoad(UASInterface*, double); - void selectWaypoint(UASInterface* uas, int id); + void selectWaypoint(int uasId, int id); void startImage(int imgid, int width, int height, int depth, int channels); void setPixels(int imgid, const unsigned char* imageData, int length, int startIndex); diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 7635631980f5251a465aaad77657d1ed3de5b934..88e9aa6e9dd30669e7193925cf8b057238aa92c7 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -628,7 +628,6 @@ void MainWindow::showToolWidget() QAction* temp = qobject_cast(sender()); int tool = temp->data().toInt(); - if (temp && dockWidgets[tool]) { if (temp->isChecked()) @@ -640,9 +639,6 @@ void MainWindow::showToolWidget() { removeDockWidget(qobject_cast(dockWidgets[tool])); } - QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(tool), currentView); - settings.setValue(chKey,temp->isChecked()); - settings.sync(); } } @@ -1306,6 +1302,25 @@ void MainWindow::clearView() // Save current state if (UASManager::instance()->getActiveUAS()) settings.setValue(getWindowStateKey(), saveState(QGC::applicationVersion())); settings.setValue(getWindowGeometryKey(), saveGeometry()); + + QAction* temp; + + // Set tool widget visibility settings for this view + foreach (int key, toolsMenuActions.keys()) + { + temp = toolsMenuActions[key]; + QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(key), currentView); + + if (temp) + { + settings.setValue(chKey,temp->isChecked()); + } + else + { + settings.setValue(chKey,false); + } + } + settings.sync(); // Remove all dock widgets from main window @@ -1500,8 +1515,13 @@ void MainWindow::showTheCentralWidget (TOOLS_WIDGET_NAMES centralWidget, VIEW_SE void MainWindow::loadDataView(QString fileName) { clearView(); + + // Unload line chart + QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(CENTRAL_LINECHART), currentView); + settings.setValue(chKey,false); + // Set data plot in settings as current widget and then run usual update procedure - QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(CENTRAL_DATA_PLOT), currentView); + chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(CENTRAL_DATA_PLOT), currentView); settings.setValue(chKey,true); presentView(); diff --git a/src/ui/linechart/LinechartPlot.cc b/src/ui/linechart/LinechartPlot.cc index 606c3bddf61be9313659c276b2e481b0c196b077..85af32ffe6d14cba76bfb6499ebc5d4d69a0ced8 100644 --- a/src/ui/linechart/LinechartPlot.cc +++ b/src/ui/linechart/LinechartPlot.cc @@ -228,7 +228,8 @@ void LinechartPlot::appendData(QString dataname, quint64 ms, double value) datalock.lock(); /* Check if dataset identifier already exists */ - if(!data.contains(dataname)) { + if(!data.contains(dataname)) + { addCurve(dataname); } diff --git a/src/ui/linechart/LinechartWidget.cc b/src/ui/linechart/LinechartWidget.cc index 6f39d19b5de84483b040b4674b5184098186d175..6e157ff1121f0f1f154ad531cdd42eb00cd4858b 100644 --- a/src/ui/linechart/LinechartWidget.cc +++ b/src/ui/linechart/LinechartWidget.cc @@ -70,6 +70,7 @@ curveMenu(new QMenu(this)), logFile(new QFile()), logindex(1), logging(false), +logStartTime(0), updateTimer(new QTimer()) { // Add elements defined in Qt Designer @@ -90,8 +91,9 @@ updateTimer(new QTimer()) curvesWidgetLayout->setColumnStretch(2, 80); curvesWidgetLayout->setColumnStretch(3, 50); curvesWidgetLayout->setColumnStretch(4, 50); -// horizontalLayout->setColumnStretch(median, 50); curvesWidgetLayout->setColumnStretch(5, 50); +// horizontalLayout->setColumnStretch(median, 50); + curvesWidgetLayout->setColumnStretch(6, 50); curvesWidget->setLayout(curvesWidgetLayout); @@ -105,7 +107,7 @@ updateTimer(new QTimer()) int labelRow = curvesWidgetLayout->rowCount(); - curvesWidgetLayout->addWidget(new QLabel("On"), labelRow, 0, 1, 2); + curvesWidgetLayout->addWidget(new QLabel(tr("On")), labelRow, 0, 1, 2); label = new QLabel(this); label->setText("Name"); @@ -116,15 +118,19 @@ updateTimer(new QTimer()) value->setText("Val"); curvesWidgetLayout->addWidget(value, labelRow, 3); + // Unit + label->setText("Unit"); + curvesWidgetLayout->addWidget(new QLabel(tr("Unit")), labelRow, 4); + // Mean mean = new QLabel(this); mean->setText("Mean"); - curvesWidgetLayout->addWidget(mean, labelRow, 4); + curvesWidgetLayout->addWidget(mean, labelRow, 5); // Variance variance = new QLabel(this); variance->setText("Variance"); - curvesWidgetLayout->addWidget(variance, labelRow, 5); + curvesWidgetLayout->addWidget(variance, labelRow, 6); // Add and customize plot elements (right side) @@ -265,18 +271,84 @@ void LinechartWidget::createLayout() } void LinechartWidget::appendData(int uasId, QString curve, double value, quint64 usec) +{ + static const QString unit("-"); + if (isVisible()) + { + // Order matters here, first append to plot, then update curve list + activePlot->appendData(curve+unit, usec, value); + // Store data + QLabel* label = curveLabels->value(curve+unit, NULL); + // Make sure the curve will be created if it does not yet exist + if(!label) + { + addCurve(curve, unit); + } + } + + // Log data + if (logging) + { + if (activePlot->isVisible(curve)) + { + if (logStartTime == 0) logStartTime = usec; + qint64 time = usec - logStartTime; + if (time < 0) time = 0; + + logFile->write(QString(QString::number(time) + "\t" + QString::number(uasId) + "\t" + curve + "\t" + QString::number(value) + "\n").toLatin1()); + logFile->flush(); + } + } +} + + +void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, double value, quint64 usec) +{ + if (isVisible()) + { + // Order matters here, first append to plot, then update curve list + activePlot->appendData(curve+unit, usec, value); + // Store data + QLabel* label = curveLabels->value(curve+unit, NULL); + // Make sure the curve will be created if it does not yet exist + if(!label) + { + qDebug() << "ADDING CURVE IN APPENDDATE DOUBLE"; + addCurve(curve, unit); + } + } + + // Log data + if (logging) + { + if (activePlot->isVisible(curve+unit)) + { + if (logStartTime == 0) logStartTime = usec; + qint64 time = usec - logStartTime; + if (time < 0) time = 0; + + logFile->write(QString(QString::number(time) + "\t" + QString::number(uasId) + "\t" + curve + "\t" + QString::number(value) + "\n").toLatin1()); + logFile->flush(); + } + } +} + +void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, int value, quint64 usec) { if (isVisible()) { // Order matters here, first append to plot, then update curve list - activePlot->appendData(curve, usec, value); + activePlot->appendData(curve+unit, usec, value); // Store data - QLabel* label = curveLabels->value(curve, NULL); + QLabel* label = curveLabels->value(curve+unit, NULL); // Make sure the curve will be created if it does not yet exist if(!label) { - addCurve(curve); + addCurve(curve, unit); } + + // Add int data + intData.insert(curve+unit, value); } // Log data @@ -284,16 +356,9 @@ void LinechartWidget::appendData(int uasId, QString curve, double value, quint64 { if (activePlot->isVisible(curve)) { - quint64 time = 0; - // Adjust time - if (activePlot->groundTime()) - { - time = QGC::groundTimeUsecs() - logStartTime; - } - else - { - time = usec - logStartTime; - } + if (logStartTime == 0) logStartTime = usec; + qint64 time = usec - logStartTime; + if (time < 0) time = 0; logFile->write(QString(QString::number(time) + "\t" + QString::number(uasId) + "\t" + curve + "\t" + QString::number(value) + "\n").toLatin1()); logFile->flush(); @@ -304,18 +369,27 @@ void LinechartWidget::appendData(int uasId, QString curve, double value, quint64 void LinechartWidget::refresh() { QString str; - + // Value QMap::iterator i; for (i = curveLabels->begin(); i != curveLabels->end(); ++i) { double val = activePlot->getCurrentValue(i.key()); - if (val > 9999 || val < 0.002) + int intval = static_cast(val); + if (intval >= 100000 || intval <= -100000) + { + str.sprintf("% 11i", intval); + } + else if (intval >= 10000 || intval <= -10000) { - str.sprintf("% 10e", val); + str.sprintf("% 11.2f", val); + } + else if (intval >= 1000 || intval <= -1000) + { + str.sprintf("% 11.4f", val); } else { - str.sprintf("% 10f", val); + str.sprintf("% 11.6f", val); } // Value i.value()->setText(str); @@ -324,7 +398,19 @@ void LinechartWidget::refresh() QMap::iterator j; for (j = curveMeans->begin(); j != curveMeans->end(); ++j) { - str.sprintf("% 8.2e", activePlot->getMean(j.key())); + double val = activePlot->getCurrentValue(j.key()); + if (val > 9999 || val < -9999) + { + str.sprintf("% 11.2f", val); + } + else if (val > 99999 || val < -99999) + { + str.sprintf("% 11d", (int)val); + } + else + { + str.sprintf("% 11.6f", val); + } j.value()->setText(str); } // QMap::iterator k; @@ -338,7 +424,7 @@ void LinechartWidget::refresh() for (l = curveVariances->begin(); l != curveVariances->end(); ++l) { // Variance - str.sprintf("% 8e", activePlot->getVariance(l.key())); + str.sprintf("% 9.4e", activePlot->getVariance(l.key())); l.value()->setText(str); } } @@ -398,7 +484,8 @@ void LinechartWidget::startLogging() if (logFile->open(QIODevice::WriteOnly | QIODevice::Text)) { logging = true; - logStartTime = QGC::groundTimeUsecs(); + logStartTime = 0; + curvesWidget->setEnabled(false); logindex++; logButton->setText(tr("Stop logging")); disconnect(logButton, SIGNAL(clicked()), this, SLOT(startLogging())); @@ -410,6 +497,7 @@ void LinechartWidget::startLogging() void LinechartWidget::stopLogging() { logging = false; + curvesWidget->setEnabled(true); if (logFile->isOpen()) { logFile->flush(); @@ -449,18 +537,16 @@ void LinechartWidget::createActions() * @param curve The id-string of the curve * @see removeCurve() **/ -void LinechartWidget::addCurve(QString curve) +void LinechartWidget::addCurve(const QString& curve, const QString& unit) { - createCurveItem(curve); -} + intData.insert(curve+unit, 0); -void LinechartWidget::createCurveItem(QString curve) -{ LinechartPlot* plot = activePlot; // QHBoxLayout *horizontalLayout; QCheckBox *checkBox; QLabel* label; QLabel* value; + QLabel* unitLabel; QLabel* mean; QLabel* variance; @@ -468,7 +554,7 @@ void LinechartWidget::createCurveItem(QString curve) checkBox = new QCheckBox(this); checkBox->setCheckable(true); - checkBox->setObjectName(curve); + checkBox->setObjectName(curve+unit); checkBox->setToolTip(tr("Enable the curve in the graph window")); checkBox->setWhatsThis(tr("Enable the curve in the graph window")); @@ -485,7 +571,7 @@ void LinechartWidget::createCurveItem(QString curve) //checkBox->setText(QString()); label->setText(curve); - QColor color = plot->getColorForCurve(curve); + QColor color = plot->getColorForCurve(curve+unit); if(color.isValid()) { QString colorstyle; colorstyle = colorstyle.sprintf("QWidget { background-color: #%X%X%X; }", color.red(), color.green(), color.blue()); @@ -496,18 +582,28 @@ void LinechartWidget::createCurveItem(QString curve) // Value value = new QLabel(this); value->setNum(0.00); + value->setStyleSheet(QString("QLabel {font-family:\"Courier\"; font-weight: bold;}")); value->setToolTip(tr("Current value of ") + curve); value->setWhatsThis(tr("Current value of ") + curve); - curveLabels->insert(curve, value); + curveLabels->insert(curve+unit, value); curvesWidgetLayout->addWidget(value, labelRow, 3); + // Unit + unitLabel = new QLabel(this); + unitLabel->setText(unit); + unitLabel->setStyleSheet(QString("QLabel {color: %1;}").arg("#AAAAAA")); + qDebug() << "UNIT" << unit; + unitLabel->setToolTip(tr("Unit of ") + curve); + unitLabel->setWhatsThis(tr("Unit of ") + curve); + curvesWidgetLayout->addWidget(unitLabel, labelRow, 4); + // Mean mean = new QLabel(this); mean->setNum(0.00); mean->setToolTip(tr("Arithmetic mean of ") + curve); mean->setWhatsThis(tr("Arithmetic mean of ") + curve); - curveMeans->insert(curve, mean); - curvesWidgetLayout->addWidget(mean, labelRow, 4); + curveMeans->insert(curve+unit, mean); + curvesWidgetLayout->addWidget(mean, labelRow, 5); // // Median // median = new QLabel(form); @@ -520,8 +616,8 @@ void LinechartWidget::createCurveItem(QString curve) variance->setNum(0.00); variance->setToolTip(tr("Variance of ") + curve); variance->setWhatsThis(tr("Variance of ") + curve); - curveVariances->insert(curve, variance); - curvesWidgetLayout->addWidget(variance, labelRow, 5); + curveVariances->insert(curve+unit, variance); + curvesWidgetLayout->addWidget(variance, labelRow, 6); /* Color picker QColor color = QColorDialog::getColor(Qt::green, this); @@ -540,7 +636,7 @@ void LinechartWidget::createCurveItem(QString curve) // Set UI components to initial state checkBox->setChecked(false); - plot->setVisible(curve, false); + plot->setVisible(curve+unit, false); } /** diff --git a/src/ui/linechart/LinechartWidget.h b/src/ui/linechart/LinechartWidget.h index 7ee69799eb5628e6af89f5d8705496f28f29e386..af76a4be79eecc0f4f900d382db9fae45a017d42 100644 --- a/src/ui/linechart/LinechartWidget.h +++ b/src/ui/linechart/LinechartWidget.h @@ -70,9 +70,14 @@ public: static const int MAX_TIME_SCROLLBAR_VALUE = 16383; ///< The maximum scrollbar value public slots: - void addCurve(QString curve); + void addCurve(const QString& curve, const QString& unit); void removeCurve(QString curve); + /** @brief Append data without unit */ void appendData(int uasId, QString curve, double data, quint64 usec); + /** @brief Append data with unit */ + void appendData(int uasId, const QString& curve, const QString& unit, double value, quint64 usec); + /** @brief Append data as int with unit */ + void appendData(int uasId, const QString& curve, const QString& unit, int value, quint64 usec); void takeButtonClick(bool checked); void setPlotWindowPosition(int scrollBarValue); void setPlotWindowPosition(quint64 position); @@ -110,6 +115,7 @@ protected: QMap* curveMeans; ///< References to the curve means QMap* curveMedians; ///< References to the curve medians QMap* curveVariances; ///< References to the curve variances + QMap intData; ///< Current values for integer-valued curves QWidget* curvesWidget; ///< The QWidget containing the curve selection button QGridLayout* curvesWidgetLayout; ///< The layout for the curvesWidget QWidget @@ -130,9 +136,9 @@ protected: QFile* logFile; unsigned int logindex; bool logging; + quint64 logStartTime; QTimer* updateTimer; LogCompressor* compressor; - quint64 logStartTime; static const int MAX_CURVE_MENUITEM_NUMBER = 8; static const int PAGESTEP_TIME_SCROLLBAR_VALUE = (MAX_TIME_SCROLLBAR_VALUE - MIN_TIME_SCROLLBAR_VALUE) / 10; diff --git a/src/ui/linechart/Linecharts.cc b/src/ui/linechart/Linecharts.cc index aba663f58159953e6962d899abe421c7defad9bc..6fc366537a0e6501937a9393ed2bb498435fcca2 100644 --- a/src/ui/linechart/Linecharts.cc +++ b/src/ui/linechart/Linecharts.cc @@ -96,7 +96,12 @@ void Linecharts::addSystem(UASInterface* uas) addWidget(widget); plots.insert(uas->getUASID(), widget); #ifndef MAVLINK_ENABLED_SLUGS - connect(uas, SIGNAL(valueChanged(int,QString,double,quint64)), widget, SLOT(appendData(int,QString,double,quint64))); + // Values without unit + //connect(uas, SIGNAL(valueChanged(int,QString,double,quint64)), widget, SLOT(appendData(int,QString,double,quint64))); + // Values with unit as double + connect(uas, SIGNAL(valueChanged(int,QString,QString,double,quint64)), widget, SLOT(appendData(int,QString,QString,double,quint64))); + // Values with unit as integer + connect(uas, SIGNAL(valueChanged(int,QString,QString,double,quint64)), widget, SLOT(appendData(int,QString,QString,double,quint64))); #endif connect(widget, SIGNAL(logfileWritten(QString)), this, SIGNAL(logfileWritten(QString))); // Set system active if this is the only system