From 7dba86d0dd15a9a7198351de01a7c6407558ec95 Mon Sep 17 00:00:00 2001 From: LM Date: Fri, 14 Oct 2011 18:43:54 +0200 Subject: [PATCH] Working on HIL --- src/comm/QGCFlightGearLink.cc | 48 ++++++++------- src/configuration.h | 2 +- src/uas/UAS.cc | 108 ++++++++++++++++++---------------- src/uas/UAS.h | 2 +- 4 files changed, 86 insertions(+), 74 deletions(-) diff --git a/src/comm/QGCFlightGearLink.cc b/src/comm/QGCFlightGearLink.cc index 098de173a..351e6a7dd 100644 --- a/src/comm/QGCFlightGearLink.cc +++ b/src/comm/QGCFlightGearLink.cc @@ -262,11 +262,17 @@ bool QGCFlightGearLink::disconnectSimulation() disconnect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t))); disconnect(this, SIGNAL(hilStateChanged(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t))); - process->close(); - delete process; - process = NULL; - delete socket; - socket = NULL; + if (process) + { + process->close(); + delete process; + process = NULL; + } + if (socket) + { + delete socket; + socket = NULL; + } connectState = false; @@ -288,15 +294,15 @@ bool QGCFlightGearLink::connectSimulation() QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readBytes())); - process = new QProcess(this); +// process = new QProcess(this); - connect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t))); - connect(this, SIGNAL(hilStateChanged(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t))); +// connect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t))); +// connect(this, SIGNAL(hilStateChanged(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t))); - //connect(&refreshTimer, SIGNAL(timeout()), this, SLOT(sendUAVUpdate())); - // Catch process error - QObject::connect( process, SIGNAL(error(QProcess::ProcessError)), - this, SLOT(processError(QProcess::ProcessError))); +// //connect(&refreshTimer, SIGNAL(timeout()), this, SLOT(sendUAVUpdate())); +// // Catch process error +// QObject::connect( process, SIGNAL(error(QProcess::ProcessError)), +// this, SLOT(processError(QProcess::ProcessError))); // Start Flightgear QStringList processCall; QString processFgfs; @@ -352,16 +358,18 @@ bool QGCFlightGearLink::connectSimulation() // Add new argument with this: processCall << ""; processCall << QString("--aircraft=%2").arg(aircraft); - process->start(processFgfs, processCall); +// process->start(processFgfs, processCall); - qDebug() << "STARTING: " << processFgfs << processCall; - emit flightGearConnected(connectState); - if (connectState) { - emit flightGearConnected(); - connectionStartTime = QGC::groundTimeUsecs()/1000; - } - qDebug() << "STARTING SIM"; + +// emit flightGearConnected(connectState); +// if (connectState) { +// emit flightGearConnected(); +// connectionStartTime = QGC::groundTimeUsecs()/1000; +// } +// qDebug() << "STARTING SIM"; + + qDebug() << "STARTING: " << processFgfs << processCall; start(LowPriority); return connectState; diff --git a/src/configuration.h b/src/configuration.h index 7cb390cb0..cbcbf8082 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -12,7 +12,7 @@ #define WITH_TEXT_TO_SPEECH 1 #define QGC_APPLICATION_NAME "QGroundControl" -#define QGC_APPLICATION_VERSION "v. 1.0.0 (Alpha RC17)" +#define QGC_APPLICATION_VERSION "v. 1.0.0 (Alpha RC18)" namespace QGC diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index f4f393e60..cc0f1d412 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -337,10 +337,14 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_msg_sys_status_decode(&message, &state); emit loadChanged(this,state.load/10.0f); - // FIXME REMOVE LATER emit valueChanged(uasId, "Load", "%", ((float)state.load)/10.0f, getUnixTime()); + + currentVoltage = state.voltage_battery/1000.0f; lpVoltage = filterVoltage(currentVoltage); + + qDebug() << "BATT VOLTAGE:" << currentVoltage << "RAW" << state.voltage_battery; + if (startVoltage == 0) startVoltage = currentVoltage; timeRemaining = calculateTimeRemaining(); if (!batteryRemainingEstimateEnabled && chargeLevel != -1) @@ -1194,18 +1198,11 @@ QList UAS::getComponentIds() void UAS::setMode(int mode) { - if (mode >= (int)MAV_MODE_PREFLIGHT && mode < (int)MAV_MODE_ENUM_END) - { - //this->mode = mode; //no call assignament, update receive message from UAS - mavlink_message_t msg; - mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, (uint8_t)mode, (uint16_t)navMode); - sendMessage(msg); - qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", REQUEST TO SET MODE " << (uint8_t)mode; - } - else - { - qDebug() << "uas Mode not assign: " << mode; - } + //this->mode = mode; //no call assignament, update receive message from UAS + mavlink_message_t msg; + mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, (uint8_t)mode, (uint16_t)navMode); + sendMessage(msg); + qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", REQUEST TO SET MODE " << (uint8_t)mode; } void UAS::sendMessage(mavlink_message_t message) @@ -1923,7 +1920,8 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double manualYawAngle = yaw * yawScaling; manualThrust = thrust * thrustScaling; - if(mode == (int)MAV_MODE_MANUAL_ARMED) + // If system has manual inputs enabled and is armed + if((mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) { mavlink_message_t message; mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, (float)manualRollAngle, (float)manualPitchAngle, (float)manualYawAngle, (float)manualThrust, controlRollManual, controlPitchManual, controlYawManual, controlThrustManual); @@ -2083,7 +2081,7 @@ void UAS::startHil() // Connect Flight Gear Link simulation->connectSimulation(); mavlink_message_t msg; - mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode, navMode | MAV_MODE_FLAG_HIL_ENABLED); + mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode | MAV_MODE_FLAG_HIL_ENABLED, navMode); sendMessage(msg); } @@ -2091,7 +2089,7 @@ void UAS::stopHil() { simulation->disconnectSimulation(); mavlink_message_t msg; - mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode, navMode & !MAV_MODE_FLAG_HIL_ENABLED); + mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode & !MAV_MODE_FLAG_HIL_ENABLED, navMode); sendMessage(msg); } @@ -2153,44 +2151,50 @@ const QString& UAS::getShortState() const QString UAS::getShortModeTextFor(int id) { QString mode; + uint8_t modeid = id; + + qDebug() << "MODE:" << modeid; - switch (id) { - case (uint8_t)MAV_MODE_PREFLIGHT: + // BASE MODE DECODING + if (modeid & MAV_MODE_FLAG_DECODE_POSITION_AUTO) + { + mode = "AUTO"; + } + else if (modeid & MAV_MODE_FLAG_DECODE_POSITION_GUIDED) + { + mode = "GUIDED"; + } + else if (modeid & MAV_MODE_FLAG_DECODE_POSITION_STABILIZE) + { + mode = "STABILIZED"; + } + else if (modeid & MAV_MODE_FLAG_DECODE_POSITION_TEST) + { + mode = "TEST"; + } + else if (modeid & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) + { + mode = "MANUAL"; + } + else + { mode = "PREFLIGHT"; - break; - case (uint8_t)MAV_MODE_MANUAL_ARMED: - mode = "A|MANUAL"; - break; - case (uint8_t)MAV_MODE_MANUAL_DISARMED: - mode = "D|MANUAL"; - break; - case (uint8_t)MAV_MODE_AUTO_ARMED: - mode = "A|AUTO"; - break; - case (uint8_t)MAV_MODE_AUTO_DISARMED: - mode = "D|AUTO"; - break; - case (uint8_t)MAV_MODE_GUIDED_ARMED: - mode = "A|GUIDED"; - break; - case (uint8_t)MAV_MODE_GUIDED_DISARMED: - mode = "D|GUIDED"; - break; - case (uint8_t)MAV_MODE_STABILIZE_ARMED: - mode = "A|STABILIZED"; - break; - case (uint8_t)MAV_MODE_STABILIZE_DISARMED: - mode = "D|STABILIZED"; - break; - case (uint8_t)MAV_MODE_TEST_ARMED: - mode = "A|TEST"; - break; - case (uint8_t)MAV_MODE_TEST_DISARMED: - mode = "D|TEST"; - break; - default: - mode = "UNKNOWN"; - break; + } + + // ARMED STATE DECODING + if (modeid & MAV_MODE_FLAG_DECODE_POSITION_SAFETY) + { + mode.prepend("A|"); + } + else + { + mode.prepend("D|"); + } + + // HARDWARE IN THE LOOP DECODING + if (modeid & MAV_MODE_FLAG_DECODE_POSITION_HIL) + { + mode.prepend("HIL:"); } return mode; diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 6501d2d71..2e684ed2b 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -165,7 +165,7 @@ protected: //COMMENTS FOR TEST UNIT bool batteryRemainingEstimateEnabled; ///< If the estimate is enabled, QGC will try to estimate the remaining battery life float chargeLevel; ///< Charge level of battery, in percent int timeRemaining; ///< Remaining time calculated based on previous and current - int mode; ///< The current mode of the MAV + uint8_t mode; ///< The current mode of the MAV int status; ///< The current status of the MAV uint32_t navMode; ///< The current navigation mode of the MAV quint64 onboardTimeOffset; -- 2.22.0