From 522297a441bd7787bb42fd8d0027b63712245c45 Mon Sep 17 00:00:00 2001 From: lm Date: Thu, 10 Feb 2011 13:24:10 +0100 Subject: [PATCH] Adjusted to updated MAVLink packets --- src/Waypoint.cc | 10 ++-- src/Waypoint.h | 10 ++-- src/comm/MAVLinkProtocol.cc | 2 +- src/comm/MAVLinkSimulationMAV.cc | 20 ++++---- src/comm/MAVLinkSimulationWaypointPlanner.cc | 2 +- src/comm/MAVLinkXMLParser.cc | 2 +- src/uas/UAS.cc | 48 +++++++++++--------- src/uas/UAS.h | 2 +- src/uas/UASInterface.h | 3 +- src/uas/UASWaypointManager.cc | 6 +-- src/ui/WaypointList.cc | 2 +- src/ui/WaypointView.cc | 4 +- 12 files changed, 58 insertions(+), 53 deletions(-) diff --git a/src/Waypoint.cc b/src/Waypoint.cc index 4bd0bd5aa..c5e82e2e9 100644 --- a/src/Waypoint.cc +++ b/src/Waypoint.cc @@ -32,7 +32,7 @@ This file is part of the QGROUNDCONTROL project #include "Waypoint.h" #include -Waypoint::Waypoint(quint16 _id, double _x, double _y, double _z, double _yaw, bool _autocontinue, bool _current, double _orbit, int _holdTime, MAV_FRAME _frame, MAV_ACTION _action) +Waypoint::Waypoint(quint16 _id, double _x, double _y, double _z, double _yaw, bool _autocontinue, bool _current, double _orbit, int _holdTime, MAV_FRAME _frame, MAV_COMMAND _action) : id(_id), x(_x), y(_y), @@ -72,7 +72,7 @@ bool Waypoint::load(QTextStream &loadStream) { this->id = wpParams[0].toInt(); this->frame = (MAV_FRAME) wpParams[1].toInt(); - this->action = (MAV_ACTION) wpParams[2].toInt(); + this->action = (MAV_COMMAND) wpParams[2].toInt(); this->orbit = wpParams[3].toDouble(); this->orbitDirection = wpParams[4].toInt(); this->param1 = wpParams[5].toDouble(); @@ -134,14 +134,14 @@ void Waypoint::setYaw(double yaw) void Waypoint::setAction(int action) { - if (this->action != (MAV_ACTION)action) + if (this->action != (MAV_COMMAND)action) { - this->action = (MAV_ACTION)action; + this->action = (MAV_COMMAND)action; emit changed(this); } } -void Waypoint::setAction(MAV_ACTION action) +void Waypoint::setAction(MAV_COMMAND action) { if (this->action != action) { diff --git a/src/Waypoint.h b/src/Waypoint.h index 7c1f866f4..debbd5940 100644 --- a/src/Waypoint.h +++ b/src/Waypoint.h @@ -36,7 +36,7 @@ This file is part of the PIXHAWK project #include #include #include -#include "mavlink_types.h" +#include "QGCMAVLink.h" class Waypoint : public QObject { @@ -45,7 +45,7 @@ class Waypoint : public QObject public: Waypoint(quint16 id = 0, double x = 0.0f, double y = 0.0f, double z = 0.0f, double yaw = 0.0f, bool autocontinue = false, bool current = false, double orbit = 0.15f, int holdTime = 0, - MAV_FRAME frame=MAV_FRAME_GLOBAL, MAV_ACTION action=MAV_ACTION_NAVIGATE); + MAV_FRAME frame=MAV_FRAME_GLOBAL, MAV_COMMAND action=MAV_CMD_NAV_WAYPOINT); ~Waypoint(); quint16 getId() const { return id; } @@ -66,7 +66,7 @@ public: double getParam6() const { return z; } int getTurns() const { return param1; } MAV_FRAME getFrame() const { return frame; } - MAV_ACTION getAction() const { return action; } + MAV_COMMAND getAction() const { return action; } const QString& getName() const { return name; } void save(QTextStream &saveStream); @@ -80,7 +80,7 @@ protected: double z; double yaw; MAV_FRAME frame; - MAV_ACTION action; + MAV_COMMAND action; bool autocontinue; bool current; double orbit; @@ -98,7 +98,7 @@ public slots: void setYaw(double yaw); /** @brief Set the waypoint action */ void setAction(int action); - void setAction(MAV_ACTION action); + void setAction(MAV_COMMAND action); void setFrame(MAV_FRAME frame); void setAutocontinue(bool autoContinue); void setCurrent(bool current); diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 6cc6640e2..571fcd6db 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -180,7 +180,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) // Write message to buffer mavlink_msg_to_send_buffer(buf+sizeof(quint64), &message); QByteArray b((const char*)buf, len); - if(m_logfile->write(b) < MAVLINK_MAX_PACKET_LEN+sizeof(quint64)) + if(m_logfile->write(b) < static_cast(MAVLINK_MAX_PACKET_LEN+sizeof(quint64))) { emit protocolStatusMessage(tr("MAVLink Logging failed"), tr("Could not write to file %1, disabling logging.").arg(m_logfile->fileName())); // Stop logging diff --git a/src/comm/MAVLinkSimulationMAV.cc b/src/comm/MAVLinkSimulationMAV.cc index 69863fe54..e81901039 100644 --- a/src/comm/MAVLinkSimulationMAV.cc +++ b/src/comm/MAVLinkSimulationMAV.cc @@ -134,8 +134,8 @@ void MAVLinkSimulationMAV::mainloop() pos.alt = z*1000.0; pos.lat = y*1E7; pos.lon = x*1E7; - pos.vx = sin(yaw)*10.0f*100.0f; - pos.vy = cos(yaw)*10.0f*100.0f; + pos.vx = 10.0f*100.0f; + pos.vy = 0; pos.vz = altPer100ms*10.0f*100.0f*zsign*-1.0f; mavlink_msg_global_position_int_encode(systemid, MAV_COMP_ID_IMU, &msg, &pos); link->sendMAVLinkMessage(&msg); @@ -165,11 +165,11 @@ void MAVLinkSimulationMAV::mainloop() // VFR HUD mavlink_vfr_hud_t hud; - hud.airspeed = 10; - hud.groundspeed = 9; - hud.alt = 123; - hud.heading = 90; - hud.climb = 0.1f; + hud.airspeed = pos.vx; + hud.groundspeed = pos.vx; + hud.alt = pos.alt; + hud.heading = ((yaw/M_PI)*180.0f+180.0f); + hud.climb = pos.vz; hud.throttle = 90; mavlink_msg_vfr_hud_encode(systemid, MAV_COMP_ID_IMU, &msg, &hud); link->sendMAVLinkMessage(&msg); @@ -180,9 +180,9 @@ void MAVLinkSimulationMAV::mainloop() nav.nav_pitch = pitch; nav.nav_bearing = yaw; nav.target_bearing = yaw; - nav.wp_dist = 2; - nav.alt_error = 0.5; - // xtrack + nav.wp_dist = 2.0f; + nav.alt_error = 0.5f; + nav.xtrack_error = 0.2f; mavlink_msg_nav_controller_output_encode(systemid, MAV_COMP_ID_IMU, &msg, &nav); link->sendMAVLinkMessage(&msg); } diff --git a/src/comm/MAVLinkSimulationWaypointPlanner.cc b/src/comm/MAVLinkSimulationWaypointPlanner.cc index 9ed181221..23c40c1d2 100644 --- a/src/comm/MAVLinkSimulationWaypointPlanner.cc +++ b/src/comm/MAVLinkSimulationWaypointPlanner.cc @@ -337,7 +337,7 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint(uint8_t target_systemid, ui wp->target_system = target_systemid; wp->target_component = target_compid; - if (verbose) qDebug("Sent waypoint %u (%u / %u / %u / %u / %u / %f / %f / %f / %u / %f / %f / %f / %f / %u)\n", wp->seq, wp->target_system, wp->target_component, wp->seq, wp->frame, wp->action, wp->param3, wp->param1, wp->param2, wp->current, wp->x, wp->y, wp->z, wp->param4, wp->autocontinue); + if (verbose) qDebug("Sent waypoint %u (%u / %u / %u / %u / %u / %f / %f / %f / %u / %f / %f / %f / %f / %u)\n", wp->seq, wp->target_system, wp->target_component, wp->seq, wp->frame, wp->command, wp->param3, wp->param1, wp->param2, wp->current, wp->x, wp->y, wp->z, wp->param4, wp->autocontinue); mavlink_msg_waypoint_encode(systemid, compid, &msg, wp); link->sendMAVLinkMessage(&msg); diff --git a/src/comm/MAVLinkXMLParser.cc b/src/comm/MAVLinkXMLParser.cc index 2b25075c7..8ae7ff927 100644 --- a/src/comm/MAVLinkXMLParser.cc +++ b/src/comm/MAVLinkXMLParser.cc @@ -247,7 +247,7 @@ bool MAVLinkXMLParser::generate() // Everything sane, starting with enum content currEnum = "enum " + enumName.toUpper() + "\n{\n"; - currEnumEnd = "};\n\n"; + currEnumEnd = QString("\n%1_ENUM_END\n};\n\n").arg(enumName.toUpper()); int nextEnumValue = 0; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 2b0dcfeff..ca7bf1dc6 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -440,6 +440,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit valueChanged(uasId, "climbrate", "m/s", hud.climb, time); emit valueChanged(uasId, "throttle", "m/s", hud.throttle, time); emit thrustChanged(this, hud.throttle/100.0); + emit altitudeChanged(uasId, hud.alt); + yaw = (hud.heading-180.0f/360.0f)*M_PI; + emit attitudeChanged(this, roll, pitch, yaw, getUnixTime()); + emit speedChanged(this, hud.airspeed, 0.0f, hud.climb, getUnixTime()); } break; case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: @@ -455,7 +459,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit valueChanged(uasId, "wp dist", "m", nav.wp_dist, time); emit valueChanged(uasId, "alt err", "m", nav.alt_error, time); emit valueChanged(uasId, "airspeed err", "m/s", nav.alt_error, time); - //emit valueChanged(uasId, "xtrack err", "m", nav.xtrack_error, time); + emit valueChanged(uasId, "xtrack err", "m", nav.xtrack_error, time); } break; case MAVLINK_MSG_ID_LOCAL_POSITION: @@ -1337,27 +1341,27 @@ void UAS::enableRawControllerDataTransmission(int rate) sendMessage(msg); } -void UAS::enableRawSensorFusionTransmission(int rate) -{ - // Buffers to write data to - mavlink_message_t msg; - mavlink_request_data_stream_t stream; - // Select the message to request from now on - stream.req_stream_id = MAV_DATA_STREAM_RAW_SENSOR_FUSION; - // Select the update rate in Hz the message should be send - stream.req_message_rate = rate; - // Start / stop the message - stream.start_stop = (rate) ? 1 : 0; - // The system which should take this command - stream.target_system = uasId; - // The component / subsystem which should take this command - stream.target_component = 0; - // Encode and send the message - mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); -} +//void UAS::enableRawSensorFusionTransmission(int rate) +//{ +// // Buffers to write data to +// mavlink_message_t msg; +// mavlink_request_data_stream_t stream; +// // Select the message to request from now on +// stream.req_stream_id = MAV_DATA_STREAM_RAW_SENSOR_FUSION; +// // Select the update rate in Hz the message should be send +// stream.req_message_rate = rate; +// // Start / stop the message +// stream.start_stop = (rate) ? 1 : 0; +// // The system which should take this command +// stream.target_system = uasId; +// // The component / subsystem which should take this command +// stream.target_component = 0; +// // Encode and send the message +// mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); +// // Send message twice to increase chance of reception +// sendMessage(msg); +// sendMessage(msg); +//} void UAS::enablePositionTransmission(int rate) { diff --git a/src/uas/UAS.h b/src/uas/UAS.h index da0b8b193..274cff386 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -275,7 +275,7 @@ public slots: void enableExtendedSystemStatusTransmission(int rate); void enableRCChannelDataTransmission(int rate); void enableRawControllerDataTransmission(int rate); - void enableRawSensorFusionTransmission(int rate); + //void enableRawSensorFusionTransmission(int rate); void enablePositionTransmission(int rate); void enableExtra1Transmission(int rate); void enableExtra2Transmission(int rate); diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 15d774ed4..c7dd11212 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -259,7 +259,7 @@ public slots: virtual void enableExtendedSystemStatusTransmission(int rate) = 0; virtual void enableRCChannelDataTransmission(int rate) = 0; virtual void enableRawControllerDataTransmission(int rate) = 0; - virtual void enableRawSensorFusionTransmission(int rate) = 0; + //virtual void enableRawSensorFusionTransmission(int rate) = 0; virtual void setLocalPositionSetpoint(float x, float y, float z, float yaw) = 0; virtual void setLocalPositionOffset(float x, float y, float z, float yaw) = 0; @@ -390,6 +390,7 @@ signals: void positionSetPointsChanged(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec); void localPositionChanged(UASInterface*, double x, double y, double z, quint64 usec); void globalPositionChanged(UASInterface*, double lat, double lon, double alt, quint64 usec); + void altitudeChanged(int uasid, double altitude); /** @brief Update the status of one satellite used for localization */ void gpsSatelliteStatusChanged(int uasid, int satid, float azimuth, float direction, float snr, bool used); void speedChanged(UASInterface*, double x, double y, double z, quint64 usec); diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index d30f84b72..8d0db507d 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -138,8 +138,8 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ if(wp->seq == current_wp_id) { - qDebug() << "Got WP: " << wp->seq << wp->x << wp->y << wp->z << wp->param4 << "auto:" << wp->autocontinue << "curr:" << wp->current << wp->param1 << wp->param2 << (MAV_FRAME) wp->frame << (MAV_ACTION) wp->action; - Waypoint *lwp = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->param4, wp->autocontinue, wp->current, wp->param1, wp->param2, (MAV_FRAME) wp->frame, (MAV_ACTION) wp->action); + qDebug() << "Got WP: " << wp->seq << wp->x << wp->y << wp->z << wp->param4 << "auto:" << wp->autocontinue << "curr:" << wp->current << wp->param1 << wp->param2 << (MAV_FRAME) wp->frame << (MAV_COMMAND) wp->command; + Waypoint *lwp = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->param4, wp->autocontinue, wp->current, wp->param1, wp->param2, (MAV_FRAME) wp->frame, (MAV_COMMAND) wp->command); addWaypoint(lwp, false); //get next waypoint @@ -667,7 +667,7 @@ void UASWaypointManager::writeWaypoints() cur_d->param1 = cur_s->getParam1(); cur_d->param2 = cur_s->getParam2(); cur_d->frame = cur_s->getFrame(); - cur_d->action = cur_s->getAction(); + cur_d->command = cur_s->getAction(); cur_d->seq = i; // don't read out the sequence number of the waypoint class cur_d->x = cur_s->getX(); cur_d->y = cur_s->getY(); diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index ac39970c9..03a21e897 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -188,7 +188,7 @@ void WaypointList::add() else { // Create global frame waypoint per default - wp = new Waypoint(0, uas->getLongitude(), uas->getLatitude(), uas->getAltitude(), 0.0, true, true, 0.15, 0, MAV_FRAME_GLOBAL, MAV_ACTION_NAVIGATE); + wp = new Waypoint(0, uas->getLongitude(), uas->getLatitude(), uas->getAltitude(), 0.0, true, true, 0.15, 0, MAV_FRAME_GLOBAL, MAV_CMD_NAV_WAYPOINT); uas->getWaypointManager()->addWaypoint(wp); } } diff --git a/src/ui/WaypointView.cc b/src/ui/WaypointView.cc index 89e995772..a544a7c97 100644 --- a/src/ui/WaypointView.cc +++ b/src/ui/WaypointView.cc @@ -416,12 +416,12 @@ void WaypointView::updateValues() } // Update action - MAV_ACTION action = wp->getAction(); + MAV_COMMAND action = wp->getAction(); int action_index = m_ui->comboBox_action->findData(action); // Set to "Other" action if it was -1 if (action_index == -1) { - action_index = m_ui->comboBox_action->findData(MAV_ACTION_NB); + action_index = m_ui->comboBox_action->findData(MAV_COMMAND_ENUM_END); } // Only update if changed if (m_ui->comboBox_action->currentIndex() != action_index) -- 2.22.0