From 80e38a20b70fb0b8248df562eba0ddb0da67fc4b Mon Sep 17 00:00:00 2001 From: Holden Date: Sat, 2 Nov 2024 20:04:58 -0400 Subject: [PATCH] Android: Fix Serial Issues --- android/build.gradle | 4 +- .../gradle/wrapper/gradle-wrapper.properties | 2 +- .../libs/qtandroidserialport/CMakeLists.txt | 2 + .../qserialport_android.cpp | 194 ++- .../libs/qtandroidserialport/qserialport_p.h | 18 +- android/src/AndroidInit.cpp | 22 +- android/src/AndroidInterface.cc | 2 +- android/src/AndroidSerial.cc | 183 ++- android/src/AndroidSerial.h | 9 +- .../mavlink/qgroundcontrol/QGCActivity.java | 1222 +---------------- .../qgroundcontrol/QGCSerialListener.java | 31 - .../qgroundcontrol/QGCUsbSerialManager.java | 1187 ++++++++++++++++ src/Joystick/JoystickAndroid.cc | 2 +- 13 files changed, 1533 insertions(+), 1345 deletions(-) delete mode 100644 android/src/org/mavlink/qgroundcontrol/QGCSerialListener.java create mode 100644 android/src/org/mavlink/qgroundcontrol/QGCUsbSerialManager.java diff --git a/android/build.gradle b/android/build.gradle index 101adece335..e3b2174a83f 100644 --- a/android/build.gradle +++ b/android/build.gradle @@ -5,7 +5,7 @@ buildscript { } dependencies { - classpath 'com.android.tools.build:gradle:8.6.1' + classpath 'com.android.tools.build:gradle:8.7.2' } } @@ -20,7 +20,7 @@ apply plugin: 'com.android.application' dependencies { implementation fileTree(dir: 'libs', include: ['*.jar', '*.aar']) implementation 'androidx.core:core:1.12.0' - implementation 'com.github.mik3y:usb-serial-for-android:3.8.0' + implementation 'com.github.mik3y:usb-serial-for-android:3.8.1' } def getTimestampStr() { return new Date().format('yyyyMMddHHmmss') } diff --git a/android/gradle/wrapper/gradle-wrapper.properties b/android/gradle/wrapper/gradle-wrapper.properties index 0aaefbcaf0f..df97d72b8b9 100644 --- a/android/gradle/wrapper/gradle-wrapper.properties +++ b/android/gradle/wrapper/gradle-wrapper.properties @@ -1,6 +1,6 @@ distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.10.1-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-8.10.2-bin.zip networkTimeout=10000 validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME diff --git a/android/libs/qtandroidserialport/CMakeLists.txt b/android/libs/qtandroidserialport/CMakeLists.txt index 5aa6d61bf7b..f3817ba1fd3 100644 --- a/android/libs/qtandroidserialport/CMakeLists.txt +++ b/android/libs/qtandroidserialport/CMakeLists.txt @@ -27,3 +27,5 @@ target_link_libraries(qtandroidserialport ) target_include_directories(qtandroidserialport PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +# target_compile_definitions(qtandroidserialport PUBLIC QIODEVICE_DEBUG) diff --git a/android/libs/qtandroidserialport/qserialport_android.cpp b/android/libs/qtandroidserialport/qserialport_android.cpp index 7b5e8b84250..3b5d06b9744 100644 --- a/android/libs/qtandroidserialport/qserialport_android.cpp +++ b/android/libs/qtandroidserialport/qserialport_android.cpp @@ -1,13 +1,11 @@ #include "qserialport_p.h" -#include +#include "QGCLoggingCategory.h" #include -#include -#include #include -QGC_LOGGING_CATEGORY(AndroidSerialPortLog, "qgc.android.libs.qtandroidserialport.qserialport_android") +QGC_LOGGING_CATEGORY(AndroidSerialPortLog, "test.android.libs.qtandroidserialport.qserialport_android") QT_BEGIN_NAMESPACE @@ -16,7 +14,7 @@ bool QSerialPortPrivate::open(QIODevice::OpenMode mode) qCDebug(AndroidSerialPortLog) << "Opening" << systemLocation.toLatin1().constData(); m_deviceId = AndroidSerial::open(systemLocation, this); - if (m_deviceId == BAD_PORT) { + if (m_deviceId == INVALID_DEVICE_ID) { qCWarning(AndroidSerialPortLog) << "Error opening" << systemLocation.toLatin1().constData(); setError(QSerialPortErrorInfo(QSerialPort::DeviceNotFoundError)); return false; @@ -39,14 +37,14 @@ bool QSerialPortPrivate::open(QIODevice::OpenMode mode) } if (mode & QIODevice::ReadOnly) { - // readBufferMaxSize = AndroidSerial::getReadBufferSize(); + // Q_Q(QSerialPort); + // q->setReadBufferSize(AndroidSerial::getReadBufferSize()); startAsyncRead(); + } else { + _stopAsyncRead(); } - /*if (!clear(QSerialPort::AllDirections)) { - close(); - return false; - }*/ + // clear(QSerialPort::AllDirections); return true; } @@ -55,13 +53,18 @@ void QSerialPortPrivate::close() { qCDebug(AndroidSerialPortLog) << "Closing" << systemLocation.toLatin1().data(); - descriptor = -1; - m_pendingBytesWritten = 0; - m_deviceId = BAD_PORT; + // (void) flush(); + // (void) clear(QSerialPort::AllDirections); - if (!AndroidSerial::close(m_deviceId)) { - setError(QSerialPortErrorInfo(QSerialPort::ResourceError, QStringLiteral("Closing device failed"))); + if (m_deviceId != INVALID_DEVICE_ID) { + if (!AndroidSerial::close(m_deviceId)) { + setError(QSerialPortErrorInfo(QSerialPort::ResourceError, QStringLiteral("Closing device failed"))); + } + m_deviceId = INVALID_DEVICE_ID; } + + descriptor = -1; + m_pendingBytesWritten = 0; } bool QSerialPortPrivate::_stopAsyncRead() @@ -69,7 +72,13 @@ bool QSerialPortPrivate::_stopAsyncRead() if (!AndroidSerial::readThreadRunning(m_deviceId)) { return true; } - return AndroidSerial::stopReadThread(m_deviceId); + + const bool result = AndroidSerial::stopReadThread(m_deviceId); + if (!result) { + setError(QSerialPortErrorInfo(QSerialPort::UnknownError, QSerialPort::tr("Failed to stop async read"))); + } + + return result; } bool QSerialPortPrivate::startAsyncRead() @@ -77,10 +86,16 @@ bool QSerialPortPrivate::startAsyncRead() if (AndroidSerial::readThreadRunning(m_deviceId)) { return true; } - return AndroidSerial::startReadThread(m_deviceId); + + const bool result = AndroidSerial::startReadThread(m_deviceId); + if (!result) { + setError(QSerialPortErrorInfo(QSerialPort::UnknownError, QSerialPort::tr("Failed to start async read"))); + } + + return result; } -void QSerialPortPrivate::newDataArrived(char *bytes, int length) +void QSerialPortPrivate::newDataArrived(const char *bytes, int length) { Q_Q(QSerialPort); @@ -101,6 +116,10 @@ void QSerialPortPrivate::newDataArrived(char *bytes, int length) (void) memcpy(ptr, bytes, static_cast(bytesToRead)); emit q->readyRead(); + + QMutexLocker locker(&m_readMutex); + m_readWaitCondition.wakeAll(); + locker.unlock(); } void QSerialPortPrivate::exceptionArrived(const QString &ex) @@ -110,30 +129,27 @@ void QSerialPortPrivate::exceptionArrived(const QString &ex) bool QSerialPortPrivate::waitForReadyRead(int msecs) { - const qint64 orig = static_cast(buffer.size()); - - if (orig > 0) { - return true; - } + QMutexLocker locker(&m_readMutex); - for (int i = 0; i < msecs; i++) { - if (orig < buffer.size()) { - return true; - } else { - QThread::msleep(1); - } + const bool result = m_readWaitCondition.wait(&m_readMutex, msecs); + if (!result) { + setError(QSerialPortErrorInfo(QSerialPort::TimeoutError, QSerialPort::tr("Timeout while waiting for ready read"))); } - return false; + return result; } bool QSerialPortPrivate::waitForBytesWritten(int msecs) { - // if (writeBuffer.isEmpty() && (m_pendingBytesWritten <= 0)) { - // return false; - // } + // QMutexLocker locker(&m_writeMutex); + + // const bool result = m_writeWaitCondition.wait(&m_writeMutex, msecs); + const bool result = _writeDataOneShot(msecs); + if (!result) { + setError(QSerialPortErrorInfo(QSerialPort::TimeoutError, QSerialPort::tr("Timeout while waiting for bytes written"))); + } - return _writeDataOneShot(msecs); + return result; } bool QSerialPortPrivate::_writeDataOneShot(int msecs) @@ -143,16 +159,20 @@ bool QSerialPortPrivate::_writeDataOneShot(int msecs) m_pendingBytesWritten = -1; while (!writeBuffer.isEmpty()) { - m_pendingBytesWritten = _writeToPort(writeBuffer.readPointer(), writeBuffer.nextDataBlockSize(), msecs, false); + m_pendingBytesWritten = _writeToPort(writeBuffer.readPointer(), writeBuffer.nextDataBlockSize(), msecs); if (m_pendingBytesWritten <= 0) { - setError(QSerialPortErrorInfo(QSerialPort::WriteError)); + setError(QSerialPortErrorInfo(QSerialPort::WriteError, QSerialPort::tr("Failed to write data one shot"))); return false; } writeBuffer.free(m_pendingBytesWritten); emit q->bytesWritten(m_pendingBytesWritten); + + // QMutexLocker locker(&m_writeMutex); + // m_writeWaitCondition.wakeAll(); + // locker.unlock(); } return (m_pendingBytesWritten >= 0); @@ -160,18 +180,33 @@ bool QSerialPortPrivate::_writeDataOneShot(int msecs) qint64 QSerialPortPrivate::_writeToPort(const char *data, qint64 maxSize, int timeout, bool async) { - return AndroidSerial::write(m_deviceId, data, maxSize, timeout, async); + const qint64 result = AndroidSerial::write(m_deviceId, data, maxSize, timeout, async); + if (result < 0) { + setError(QSerialPortErrorInfo(QSerialPort::WriteError, QSerialPort::tr("Failed to write to port"))); + } + + return result; } qint64 QSerialPortPrivate::writeData(const char *data, qint64 maxSize) { - (void) writeBuffer.append(data, maxSize); - return _writeDataOneShot(0); + const qint64 result = _writeToPort(data, maxSize); + if (result < 0) { + setError(QSerialPortErrorInfo(QSerialPort::WriteError, QSerialPort::tr("Failed to write data"))); + } + + return result; } bool QSerialPortPrivate::flush() { - return _writeDataOneShot(0); + const bool result = _writeDataOneShot(0); + if (!result) { + qCWarning(AndroidSerialPortLog) << "Flush operation failed"; + setError(QSerialPortErrorInfo(QSerialPort::UnknownError, QSerialPort::tr("Failed to flush"))); + } + + return result; } bool QSerialPortPrivate::clear(QSerialPort::Directions directions) @@ -191,7 +226,12 @@ bool QSerialPortPrivate::clear(QSerialPort::Directions directions) } } - return AndroidSerial::purgeBuffers(m_deviceId, input, output); + const bool result = AndroidSerial::purgeBuffers(m_deviceId, input, output); + if (!result) { + setError(QSerialPortErrorInfo(QSerialPort::UnknownError, QSerialPort::tr("Failed to purge buffers"))); + } + + return result; } QSerialPort::PinoutSignals QSerialPortPrivate::pinoutSignals() @@ -201,12 +241,22 @@ QSerialPort::PinoutSignals QSerialPortPrivate::pinoutSignals() bool QSerialPortPrivate::setDataTerminalReady(bool set) { - return AndroidSerial::setDataTerminalReady(m_deviceId, set); + const bool result = AndroidSerial::setDataTerminalReady(m_deviceId, set); + if (!result) { + setError(QSerialPortErrorInfo(QSerialPort::UnknownError, QSerialPort::tr("Failed to set DTR"))); + } + + return result; } bool QSerialPortPrivate::setRequestToSend(bool set) { - return AndroidSerial::setRequestToSend(m_deviceId, set); + const bool result = AndroidSerial::setRequestToSend(m_deviceId, set); + if (!result) { + setError(QSerialPortErrorInfo(QSerialPort::UnknownError, QSerialPort::tr("Failed to set RTS"))); + } + + return result; } bool QSerialPortPrivate::_setParameters(int baudRate, int dataBits, int stopBits, int parity) @@ -245,7 +295,12 @@ bool QSerialPortPrivate::setBaudRate(qint32 baudRate, QSerialPort::Directions di return false; } - return _setParameters(baudRate, m_dataBits, m_stopBits, m_parity); + const bool result = _setParameters(baudRate, m_dataBits, m_stopBits, m_parity); + if (!result) { + setError(QSerialPortErrorInfo(QSerialPort::UnknownError, QSerialPort::tr("Failed to set data bits"))); + } + + return result; } bool QSerialPortPrivate::setDataBits(QSerialPort::DataBits dataBits) @@ -263,12 +318,19 @@ bool QSerialPortPrivate::setDataBits(QSerialPort::DataBits dataBits) numBits = AndroidSerial::Data7; break; case QSerialPort::Data8: - default: numBits = AndroidSerial::Data8; break; + default: + setError(QSerialPortErrorInfo(QSerialPort::UnsupportedOperationError, QSerialPort::tr("Invalid data bits type"))); + return false; + } + + const bool result = _setParameters(inputBaudRate, numBits, m_stopBits, m_parity); + if (!result) { + setError(QSerialPortErrorInfo(QSerialPort::UnknownError, QSerialPort::tr("Failed to set data bits"))); } - return _setParameters(inputBaudRate, numBits, m_stopBits, m_parity); + return result; } bool QSerialPortPrivate::setParity(QSerialPort::Parity parity) @@ -289,12 +351,19 @@ bool QSerialPortPrivate::setParity(QSerialPort::Parity parity) par = AndroidSerial::OddParity; break; case QSerialPort::NoParity: - default: par = AndroidSerial::NoParity; break; + default: + setError(QSerialPortErrorInfo(QSerialPort::UnsupportedOperationError, QSerialPort::tr("Invalid parity type"))); + return false; } - return _setParameters(inputBaudRate, m_dataBits, m_stopBits, par); + const bool result = _setParameters(inputBaudRate, m_dataBits, m_stopBits, par); + if (!result) { + setError(QSerialPortErrorInfo(QSerialPort::UnknownError, QSerialPort::tr("Failed to set parity"))); + } + + return result; } bool QSerialPortPrivate::setStopBits(QSerialPort::StopBits stopBits) @@ -309,12 +378,19 @@ bool QSerialPortPrivate::setStopBits(QSerialPort::StopBits stopBits) stop = AndroidSerial::OneAndHalfStop; break; case QSerialPort::OneStop: - default: stop = AndroidSerial::OneStop; break; + default: + setError(QSerialPortErrorInfo(QSerialPort::UnsupportedOperationError, QSerialPort::tr("Invalid Stop Bits type"))); + return false; + } + + const bool result = _setParameters(inputBaudRate, m_dataBits, stop, m_parity); + if (!result) { + setError(QSerialPortErrorInfo(QSerialPort::UnknownError, QSerialPort::tr("Failed to set StopBits"))); } - return _setParameters(inputBaudRate, m_dataBits, stop, m_parity); + return result; } bool QSerialPortPrivate::setFlowControl(QSerialPort::FlowControl flowControl) @@ -329,17 +405,29 @@ bool QSerialPortPrivate::setFlowControl(QSerialPort::FlowControl flowControl) control = AndroidSerial::XonXoffFlowControl; break; case QSerialPort::NoFlowControl: - default: control = AndroidSerial::NoFlowControl; break; + default: + setError(QSerialPortErrorInfo(QSerialPort::UnsupportedOperationError, QSerialPort::tr("Invalid Flow Control type"))); + return false; + } + + const bool result = AndroidSerial::setFlowControl(m_deviceId, control); + if (!result) { + setError(QSerialPortErrorInfo(QSerialPort::UnknownError, QSerialPort::tr("Failed to set StopBits"))); } - return AndroidSerial::setFlowControl(m_deviceId, control); + return result; } bool QSerialPortPrivate::setBreakEnabled(bool set) { - return AndroidSerial::setBreak(m_deviceId, set); + const bool result = AndroidSerial::setBreak(m_deviceId, set); + if (!result) { + setError(QSerialPortErrorInfo(QSerialPort::UnknownError, QSerialPort::tr("Failed to set Break Enabled"))); + } + + return result; } typedef QMap BaudRateMap; diff --git a/android/libs/qtandroidserialport/qserialport_p.h b/android/libs/qtandroidserialport/qserialport_p.h index 08923cb5833..36a87e7f56f 100644 --- a/android/libs/qtandroidserialport/qserialport_p.h +++ b/android/libs/qtandroidserialport/qserialport_p.h @@ -20,15 +20,18 @@ #include #include #include +#include +#include #include "qserialport.h" #include -#define BAD_PORT 0 -#define MAX_READ_SIZE (16 * 1024); +constexpr int INVALID_DEVICE_ID = 0; +constexpr qint64 DEFAULT_READ_BUFFER_SIZE = 16 * 1024; +constexpr qint64 DEFAULT_WRITE_BUFFER_SIZE = 32768; #ifndef QSERIALPORT_BUFFERSIZE -#define QSERIALPORT_BUFFERSIZE 32768 +#define QSERIALPORT_BUFFERSIZE DEFAULT_WRITE_BUFFER_SIZE #endif Q_DECLARE_LOGGING_CATEGORY(AndroidSerialPortLog) @@ -110,7 +113,7 @@ class QSerialPortPrivate : public QIODevicePrivate qint64 writeData(const char *data, qint64 maxSize); - void newDataArrived(char *bytes, int length); + void newDataArrived(const char *bytes, int length); void exceptionArrived(const QString &ex); static QList standardBaudRates(); @@ -131,10 +134,15 @@ class QSerialPortPrivate : public QIODevicePrivate qint64 m_pendingBytesWritten = 0; - int m_deviceId = BAD_PORT; + int m_deviceId = INVALID_DEVICE_ID; int m_dataBits = AndroidSerial::Data8; int m_stopBits = AndroidSerial::OneStop; int m_parity = AndroidSerial::NoParity; + + mutable QMutex m_readMutex; + mutable QMutex m_writeMutex; + QWaitCondition m_readWaitCondition; + QWaitCondition m_writeWaitCondition; }; QT_END_NAMESPACE diff --git a/android/src/AndroidInit.cpp b/android/src/AndroidInit.cpp index b1833df3bd7..33f3c7182ef 100644 --- a/android/src/AndroidInit.cpp +++ b/android/src/AndroidInit.cpp @@ -10,6 +10,7 @@ #include QGC_LOGGING_CATEGORY(AndroidInitLog, "qgc.android.init"); + static jobject _context = nullptr; static jobject _class_loader = nullptr; @@ -25,27 +26,29 @@ extern "C" } #endif -static void jniInit(JNIEnv *env, jobject context) +static jboolean jniInit(JNIEnv *env, jobject context) { qCDebug(AndroidInitLog) << Q_FUNC_INFO; const jclass context_cls = env->GetObjectClass(context); if (!context_cls) { - return; + return JNI_FALSE; } const jmethodID get_class_loader_id = env->GetMethodID(context_cls, "getClassLoader", "()Ljava/lang/ClassLoader;"); if (QJniEnvironment::checkAndClearExceptions(env)) { - return; + return JNI_FALSE; } const jobject class_loader = env->CallObjectMethod(context, get_class_loader_id); if (QJniEnvironment::checkAndClearExceptions(env)) { - return; + return JNI_FALSE; } _context = env->NewGlobalRef(context); _class_loader = env->NewGlobalRef(class_loader); + + return JNI_TRUE; } static jint jniSetNativeMethods() @@ -53,7 +56,7 @@ static jint jniSetNativeMethods() qCDebug(AndroidInitLog) << Q_FUNC_INFO; const JNINativeMethod javaMethods[] { - {"nativeInit", "()V", reinterpret_cast(jniInit)} + {"nativeInit", "()Z", reinterpret_cast(jniInit)} }; QJniEnvironment jniEnv; @@ -62,16 +65,19 @@ static jint jniSetNativeMethods() jclass objectClass = jniEnv->FindClass(AndroidInterface::kJniQGCActivityClassName); if (!objectClass) { qCWarning(AndroidInitLog) << "Couldn't find class:" << AndroidInterface::kJniQGCActivityClassName; + (void) jniEnv.checkAndClearExceptions(); return JNI_ERR; } - const jint val = jniEnv->RegisterNatives(objectClass, javaMethods, sizeof(javaMethods) / sizeof(javaMethods[0])); + const jint val = jniEnv->RegisterNatives(objectClass, javaMethods, std::size(javaMethods)); if (val < 0) { qCWarning(AndroidInitLog) << "Error registering methods:" << val; - } else { - qCDebug(AndroidInitLog) << "Main Native Functions Registered"; + (void) jniEnv.checkAndClearExceptions(); + return JNI_ERR; } + qCDebug(AndroidInitLog) << "Main Native Functions Registered"; + (void) jniEnv.checkAndClearExceptions(); return JNI_OK; diff --git a/android/src/AndroidInterface.cc b/android/src/AndroidInterface.cc index 1717b4a3c8a..25820d70631 100644 --- a/android/src/AndroidInterface.cc +++ b/android/src/AndroidInterface.cc @@ -44,7 +44,7 @@ void setNativeMethods() } QJniEnvironment jniEnv; - jint val = jniEnv->RegisterNatives(objectClass, javaMethods, sizeof(javaMethods) / sizeof(javaMethods[0])); + jint val = jniEnv->RegisterNatives(objectClass, javaMethods, std::size(javaMethods)); if (val < 0) { qCWarning(AndroidInterfaceLog) << "Error registering methods:" << val; diff --git a/android/src/AndroidSerial.cc b/android/src/AndroidSerial.cc index e59185aea5c..e755efddabf 100644 --- a/android/src/AndroidSerial.cc +++ b/android/src/AndroidSerial.cc @@ -17,6 +17,34 @@ Q_LOGGING_CATEGORY(AndroidSerialLog, "qgc.android.serial"); namespace AndroidSerial { +jclass getSerialManagerClass() +{ + static jclass javaClass = nullptr; + + if (!javaClass) { + QJniEnvironment env; + if (!env.isValid()) { + qCWarning(AndroidSerialLog) << "Invalid QJniEnvironment"; + return nullptr; + } + + if (!QJniObject::isClassAvailable(kJniUsbSerialManagerClassName)) { + qCWarning(AndroidSerialLog) << "Class Not Available"; + return nullptr; + } + + javaClass = env.findClass(kJniUsbSerialManagerClassName); + if (!javaClass) { + qCWarning(AndroidSerialLog) << "Class Not Found"; + return nullptr; + } + + env.checkAndClearExceptions(); + } + + return javaClass; +} + void setNativeMethods() { qCDebug(AndroidSerialLog) << "Registering Native Functions"; @@ -29,21 +57,23 @@ void setNativeMethods() (void) AndroidInterface::cleanJavaException(); - jclass objectClass = AndroidInterface::getActivityClass(); + QJniEnvironment jniEnv; + jclass objectClass = getSerialManagerClass(); if (!objectClass) { qCWarning(AndroidSerialLog) << "Couldn't find class:" << objectClass; + (void) jniEnv.checkAndClearExceptions(); return; } - QJniEnvironment jniEnv; - jint val = jniEnv->RegisterNatives(objectClass, javaMethods, sizeof(javaMethods) / sizeof(javaMethods[0])); - + const jint val = jniEnv->RegisterNatives(objectClass, javaMethods, std::size(javaMethods)); if (val < 0) { - qCWarning(AndroidSerialLog) << "Error registering methods: " << val; - } else { - qCDebug(AndroidSerialLog) << "Native Functions Registered"; + qCWarning(AndroidSerialLog) << "Error registering methods:" << val; + (void) jniEnv.checkAndClearExceptions(); + return; } + qCDebug(AndroidSerialLog) << "Native Functions Registered"; + (void) jniEnv.checkAndClearExceptions(); } @@ -52,66 +82,72 @@ void jniDeviceHasDisconnected(JNIEnv *env, jobject obj, jlong classPtr) Q_UNUSED(env); Q_UNUSED(obj); - if (classPtr != 0) { - QSerialPortPrivate* const serialPortPrivate = reinterpret_cast(classPtr); - qCDebug(AndroidSerialLog) << "Device disconnected" << serialPortPrivate->systemLocation.toLatin1().constData(); - QSerialPort* const serialPort = static_cast(serialPortPrivate->q_ptr); - serialPort->close(); + if (classPtr == 0) { + return; } + + QSerialPortPrivate* const serialPortPrivate = reinterpret_cast(classPtr); + qCDebug(AndroidSerialLog) << "Device disconnected" << serialPortPrivate->systemLocation.toLatin1().constData(); + QSerialPort* const serialPort = static_cast(serialPortPrivate->q_ptr); + serialPort->close(); } void jniDeviceNewData(JNIEnv *env, jobject obj, jlong classPtr, jbyteArray data) { Q_UNUSED(obj); - if (classPtr != 0) { - jbyte* const bytes = env->GetByteArrayElements(data, nullptr); - const jsize len = env->GetArrayLength(data); - // QByteArray data = QByteArray::fromRawData(reinterpret_cast(bytes), len); - QSerialPortPrivate* const serialPort = reinterpret_cast(classPtr); - serialPort->newDataArrived(reinterpret_cast(bytes), len); - env->ReleaseByteArrayElements(data, bytes, JNI_ABORT); - (void) QJniEnvironment::checkAndClearExceptions(env); + if (classPtr == 0) { + return; } + + jbyte* const bytes = env->GetByteArrayElements(data, nullptr); + const jsize len = env->GetArrayLength(data); + // QByteArray data(reinterpret_cast(bytes), len); + QSerialPortPrivate* const serialPort = reinterpret_cast(classPtr); + serialPort->newDataArrived(reinterpret_cast(bytes), len); + env->ReleaseByteArrayElements(data, bytes, JNI_ABORT); + (void) QJniEnvironment::checkAndClearExceptions(env); } void jniDeviceException(JNIEnv *env, jobject obj, jlong classPtr, jstring message) { Q_UNUSED(obj); - if (classPtr != 0) { - const char* const string = env->GetStringUTFChars(message, nullptr); - const QString str = QString::fromUtf8(string); - QSerialPortPrivate* const serialPort = reinterpret_cast(classPtr); - serialPort->exceptionArrived(str); - env->ReleaseStringUTFChars(message, string); - (void) QJniEnvironment::checkAndClearExceptions(env); + if (classPtr == 0) { + return; } + + const char* const string = env->GetStringUTFChars(message, nullptr); + const QString str = QString::fromUtf8(string); + QSerialPortPrivate* const serialPort = reinterpret_cast(classPtr); + serialPort->exceptionArrived(str); + env->ReleaseStringUTFChars(message, string); + (void) QJniEnvironment::checkAndClearExceptions(env); } QList availableDevices() { - jclass javaClass = AndroidInterface::getActivityClass(); + QList serialPortInfoList; + + jclass javaClass = getSerialManagerClass(); if (!javaClass) { qCWarning(AndroidSerialLog) << "Class Not Found"; - return QList(); + return serialPortInfoList; } QJniEnvironment env; const jmethodID methodId = env.findStaticMethod(javaClass, "availableDevicesInfo", "()[Ljava/lang/String;"); if (!methodId) { qCWarning(AndroidSerialLog) << "Method Not Found"; - return QList(); + return serialPortInfoList; } const QJniObject result = QJniObject::callStaticObjectMethod(javaClass, methodId); if (!result.isValid()) { qCDebug(AndroidSerialLog) << "Method Call Failed"; - return QList(); + return serialPortInfoList; } - QList serialPortInfoList; - const jobjectArray objArray = result.object(); const jsize count = env->GetArrayLength(objArray); @@ -138,9 +174,9 @@ QList availableDevices() priv.manufacturer = strList.at(2); priv.serialNumber = strList.at(3); priv.productIdentifier = strList.at(4).toInt(); - priv.hasProductIdentifier = (priv.productIdentifier != BAD_PORT); + priv.hasProductIdentifier = (priv.productIdentifier != INVALID_DEVICE_ID); priv.vendorIdentifier = strList.at(5).toInt(); - priv.hasVendorIdentifier = (priv.vendorIdentifier != BAD_PORT); + priv.hasVendorIdentifier = (priv.vendorIdentifier != INVALID_DEVICE_ID); (void) serialPortInfoList.append(priv); } @@ -155,7 +191,7 @@ int getDeviceId(const QString &portName) (void) AndroidInterface::cleanJavaException(); const int result = QJniObject::callStaticMethod( - AndroidInterface::getActivityClass(), + getSerialManagerClass(), "getDeviceId", "(Ljava/lang/String;)I", name.object() @@ -169,7 +205,7 @@ int getDeviceHandle(int deviceId) { (void) AndroidInterface::cleanJavaException(); const int result = QJniObject::callStaticMethod( - AndroidInterface::getActivityClass(), + getSerialManagerClass(), "getDeviceHandle", "(I)I", deviceId @@ -181,14 +217,13 @@ int getDeviceHandle(int deviceId) int open(const QString &portName, QSerialPortPrivate *classPtr) { - QJniObject name = QJniObject::fromString(portName); + const QJniObject name = QJniObject::fromString(portName); (void) AndroidInterface::cleanJavaException(); const int deviceId = QJniObject::callStaticMethod( - AndroidInterface::getActivityClass(), + getSerialManagerClass(), "open", - "(Landroid/content/Context;Ljava/lang/String;J)I", - QNativeInterface::QAndroidApplication::context(), + "(Ljava/lang/String;J)I", name.object(), reinterpret_cast(classPtr) ); @@ -201,7 +236,7 @@ bool close(int deviceId) { (void) AndroidInterface::cleanJavaException(); const bool result = QJniObject::callStaticMethod( - AndroidInterface::getActivityClass(), + getSerialManagerClass(), "close", "(I)Z", deviceId @@ -217,7 +252,7 @@ bool isOpen(const QString &portName) (void) AndroidInterface::cleanJavaException(); const bool result = QJniObject::callStaticMethod( - AndroidInterface::getActivityClass(), + getSerialManagerClass(), "isDeviceNameOpen", "(Ljava/lang/String;)Z", name.object() @@ -231,7 +266,7 @@ QByteArray read(int deviceId, int length, int timeout) { (void) AndroidInterface::cleanJavaException(); const QJniObject result = QJniObject::callStaticMethod( - AndroidInterface::getActivityClass(), + getSerialManagerClass(), "read", "(II)[B", deviceId, @@ -244,7 +279,7 @@ QByteArray read(int deviceId, int length, int timeout) QJniEnvironment jniEnv; jbyte* const bytes = jniEnv->GetByteArrayElements(jarray, nullptr); const jsize len = jniEnv->GetArrayLength(jarray); - QByteArray data = QByteArray::fromRawData(reinterpret_cast(bytes), len); + QByteArray data(reinterpret_cast(bytes), len); jniEnv->ReleaseByteArrayElements(jarray, bytes, JNI_ABORT); (void) jniEnv.checkAndClearExceptions(); @@ -253,8 +288,18 @@ QByteArray read(int deviceId, int length, int timeout) int write(int deviceId, QByteArrayView data, int length, int timeout, bool async) { + if (static_cast(data.size()) > length) { + qCWarning(AndroidSerialLog) << QString("data size (%1) exceeds write length (%2)").arg(data.size()).arg(length); + return -1; + } + QJniEnvironment jniEnv; jbyteArray jarray = jniEnv->NewByteArray(static_cast(length)); + if (!jarray) { + qCWarning(AndroidSerialLog) << "Failed to create jbyteArray"; + return -1; + } + jniEnv->SetByteArrayRegion(jarray, 0, static_cast(length), reinterpret_cast(data.constData())); (void) jniEnv.checkAndClearExceptions(); @@ -262,16 +307,16 @@ int write(int deviceId, QByteArrayView data, int length, int timeout, bool async int result; if (async) { result = QJniObject::callStaticMethod( - AndroidInterface::getActivityClass(), + getSerialManagerClass(), "writeAsync", - "(I[B)V", + "(I[BI)I", deviceId, jarray, timeout ); } else { result = QJniObject::callStaticMethod( - AndroidInterface::getActivityClass(), + getSerialManagerClass(), "write", "(I[BII)I", deviceId, @@ -290,7 +335,7 @@ bool setParameters(int deviceId, int baudRate, int dataBits, int stopBits, int p { (void) AndroidInterface::cleanJavaException(); const bool result = QJniObject::callStaticMethod( - AndroidInterface::getActivityClass(), + getSerialManagerClass(), "setParameters", "(IIIII)Z", deviceId, @@ -308,7 +353,7 @@ bool getCarrierDetect(int deviceId) { (void) AndroidInterface::cleanJavaException(); const bool result = QJniObject::callStaticMethod( - AndroidInterface::getActivityClass(), + getSerialManagerClass(), "getCarrierDetect", "(I)Z", deviceId @@ -322,7 +367,7 @@ bool getClearToSend(int deviceId) { (void) AndroidInterface::cleanJavaException(); const bool result = QJniObject::callStaticMethod( - AndroidInterface::getActivityClass(), + getSerialManagerClass(), "getClearToSend", "(I)Z", deviceId @@ -336,7 +381,7 @@ bool getDataSetReady(int deviceId) { (void) AndroidInterface::cleanJavaException(); const bool result = QJniObject::callStaticMethod( - AndroidInterface::getActivityClass(), + getSerialManagerClass(), "getDataSetReady", "(I)Z", deviceId @@ -350,7 +395,7 @@ bool getDataTerminalReady(int deviceId) { (void) AndroidInterface::cleanJavaException(); const bool result = QJniObject::callStaticMethod( - AndroidInterface::getActivityClass(), + getSerialManagerClass(), "getDataTerminalReady", "(I)Z", deviceId @@ -364,7 +409,7 @@ bool setDataTerminalReady(int deviceId, bool set) { (void) AndroidInterface::cleanJavaException(); const bool result = QJniObject::callStaticMethod( - AndroidInterface::getActivityClass(), + getSerialManagerClass(), "setDataTerminalReady", "(IZ)Z", deviceId, @@ -379,7 +424,7 @@ bool getRingIndicator(int deviceId) { (void) AndroidInterface::cleanJavaException(); const bool result = QJniObject::callStaticMethod( - AndroidInterface::getActivityClass(), + getSerialManagerClass(), "getRingIndicator", "(I)Z", deviceId @@ -393,7 +438,7 @@ bool getRequestToSend(int deviceId) { (void) AndroidInterface::cleanJavaException(); const bool result = QJniObject::callStaticMethod( - AndroidInterface::getActivityClass(), + getSerialManagerClass(), "getRequestToSend", "(I)Z", deviceId @@ -407,7 +452,7 @@ bool setRequestToSend(int deviceId, bool set) { (void) AndroidInterface::cleanJavaException(); const bool result = QJniObject::callStaticMethod( - AndroidInterface::getActivityClass(), + getSerialManagerClass(), "setRequestToSend", "(IZ)Z", deviceId, @@ -422,7 +467,7 @@ QSerialPort::PinoutSignals getControlLines(int deviceId) { (void) AndroidInterface::cleanJavaException(); const QJniObject result = QJniObject::callStaticMethod( - AndroidInterface::getActivityClass(), + getSerialManagerClass(), "getControlLines", "(I)[I", deviceId @@ -437,10 +482,6 @@ QSerialPort::PinoutSignals getControlLines(int deviceId) QSerialPort::PinoutSignals data = QSerialPort::PinoutSignals::fromInt(0); for (jsize i = 0; i < len; i++) { const jint value = ints[i]; - if ((value < ControlLine::RtsControlLine) || (value > ControlLine::RiControlLine)) { - continue; - } - const ControlLine line = static_cast(value); switch (line) { case ControlLine::RtsControlLine: @@ -462,9 +503,11 @@ QSerialPort::PinoutSignals getControlLines(int deviceId) (void) data.setFlag(QSerialPort::PinoutSignal::RingIndicatorSignal, true); break; default: + qCWarning(AndroidSerialLog) << "Unknown ControlLine value:" << value; break; } } + jniEnv->ReleaseIntArrayElements(jarray, ints, JNI_ABORT); (void) jniEnv.checkAndClearExceptions(); @@ -475,7 +518,7 @@ int getFlowControl(int deviceId) { (void) AndroidInterface::cleanJavaException(); const int result = QJniObject::callStaticMethod( - AndroidInterface::getActivityClass(), + getSerialManagerClass(), "getFlowControl", "(I)I", deviceId @@ -489,7 +532,7 @@ bool setFlowControl(int deviceId, int flowControl) { (void) AndroidInterface::cleanJavaException(); const bool result = QJniObject::callStaticMethod( - AndroidInterface::getActivityClass(), + getSerialManagerClass(), "setFlowControl", "(II)Z", deviceId, @@ -504,7 +547,7 @@ bool purgeBuffers(int deviceId, bool input, bool output) { (void) AndroidInterface::cleanJavaException(); const bool result = QJniObject::callStaticMethod( - AndroidInterface::getActivityClass(), + getSerialManagerClass(), "purgeBuffers", "(IZZ)Z", deviceId, @@ -520,7 +563,7 @@ bool setBreak(int deviceId, bool set) { (void) AndroidInterface::cleanJavaException(); const bool result = QJniObject::callStaticMethod( - AndroidInterface::getActivityClass(), + getSerialManagerClass(), "setBreak", "(IZ)Z", deviceId, @@ -535,7 +578,7 @@ bool startReadThread(int deviceId) { (void) AndroidInterface::cleanJavaException(); const bool result = QJniObject::callStaticMethod( - AndroidInterface::getActivityClass(), + getSerialManagerClass(), "startIoManager", "(I)Z", deviceId @@ -549,7 +592,7 @@ bool stopReadThread(int deviceId) { (void) AndroidInterface::cleanJavaException(); const bool result = QJniObject::callStaticMethod( - AndroidInterface::getActivityClass(), + getSerialManagerClass(), "stopIoManager", "(I)Z", deviceId @@ -563,7 +606,7 @@ bool readThreadRunning(int deviceId) { (void) AndroidInterface::cleanJavaException(); const bool result = QJniObject::callStaticMethod( - AndroidInterface::getActivityClass(), + getSerialManagerClass(), "ioManagerRunning", "(I)Z", deviceId @@ -577,7 +620,7 @@ int getReadBufferSize(int deviceId) { (void) AndroidInterface::cleanJavaException(); const int result = QJniObject::callStaticMethod( - AndroidInterface::getActivityClass(), + getSerialManagerClass(), "ioManagerReadBufferSize", "(I)I", deviceId diff --git a/android/src/AndroidSerial.h b/android/src/AndroidSerial.h index ecd36f84ab0..f75ad1725ba 100644 --- a/android/src/AndroidSerial.h +++ b/android/src/AndroidSerial.h @@ -53,9 +53,12 @@ namespace AndroidSerial XonXoffInlineFlowControl }; - static constexpr char CHAR_XON = 17; - static constexpr char CHAR_XOFF = 19; + constexpr char CHAR_XON = 17; + constexpr char CHAR_XOFF = 19; + constexpr const char *kJniUsbSerialManagerClassName = "org/mavlink/qgroundcontrol/QGCUsbSerialManager"; + + jclass getSerialManagerClass(); void setNativeMethods(); void jniDeviceHasDisconnected(JNIEnv *env, jobject obj, jlong classPtr); void jniDeviceNewData(JNIEnv *env, jobject obj, jlong classPtr, jbyteArray data); @@ -86,4 +89,4 @@ namespace AndroidSerial bool stopReadThread(int deviceId); bool readThreadRunning(int deviceId); int getReadBufferSize(int deviceId); -}; // namespace AndroidSerial +} // namespace AndroidSerial diff --git a/android/src/org/mavlink/qgroundcontrol/QGCActivity.java b/android/src/org/mavlink/qgroundcontrol/QGCActivity.java index f470b11138f..7c2025aa8fe 100644 --- a/android/src/org/mavlink/qgroundcontrol/QGCActivity.java +++ b/android/src/org/mavlink/qgroundcontrol/QGCActivity.java @@ -1,195 +1,73 @@ package org.mavlink.qgroundcontrol; -import java.io.IOException; -import java.lang.reflect.Method; -import java.util.ArrayList; -import java.util.HashMap; -import java.util.List; -import java.util.Timer; -import java.util.EnumSet; -import java.util.TimerTask; - -import android.app.Activity; -import android.app.PendingIntent; -import android.bluetooth.BluetoothDevice; -import android.content.BroadcastReceiver; import android.content.Context; -import android.content.Intent; -import android.content.IntentFilter; -import android.hardware.usb.UsbDevice; -import android.hardware.usb.UsbDeviceConnection; -import android.hardware.usb.UsbManager; -import android.net.wifi.WifiManager; import android.os.Bundle; import android.os.PowerManager; -import android.os.Process; -import android.os.storage.StorageManager; -import android.os.storage.StorageVolume; +import android.net.wifi.WifiManager; import android.util.Log; import android.view.WindowManager; -// import org.freedesktop.gstreamer.GStreamer; - -import com.hoho.android.usbserial.driver.*; -import com.hoho.android.usbserial.util.*; - import org.qtproject.qt.android.bindings.QtActivity; -import org.qtproject.qt.android.QtNative; - -// TODO: -// UsbSerialDriver getDriver(); -// UsbEndpoint getWriteEndpoint(); -// UsbEndpoint getReadEndpoint(); -// boolean getXON() throws IOException; -// UsbAccessory, UsbConfiguration, UsbConstants, UsbDevice, UsbDeviceConnection, UsbEndpoint, UsbInterface, UsbManager, UsbRequest -public class QGCActivity extends QtActivity -{ +public class QGCActivity extends QtActivity { private static final String TAG = QGCActivity.class.getSimpleName(); - private static final String SCREEN_BRIGHT_WAKE_LOCK_TAG = "QGroundControl"; // BuildConfig.LIBRARY_PACKAGE_NAME, Context.getPackageName() - private static final String MULTICAST_LOCK_TAG = "QGroundControl"; // BuildConfig.LIBRARY_PACKAGE_NAME, Context.getPackageName() + private static final String SCREEN_BRIGHT_WAKE_LOCK_TAG = "QGroundControl"; + private static final String MULTICAST_LOCK_TAG = "QGroundControl"; private static QGCActivity m_instance = null; - private static Context m_context; - - private static final int BAD_DEVICE_ID = 0; - private static final String ACTION_USB_PERMISSION = "org.mavlink.qgroundcontrol.action.USB_PERMISSION"; - private static PendingIntent m_usbPermissionIntent = null; - private static UsbManager m_usbManager = null; - private static List m_drivers; - private static HashMap m_serialIoManagerHashByDeviceId; - private static HashMap m_fileDescriptorHashByDeviceId; - private static HashMap m_classPtrHashByDeviceId; - private static PowerManager.WakeLock m_wakeLock; - private static WifiManager.MulticastLock m_wifiMulticastLock; - - private final static BroadcastReceiver m_usbReceiver = new BroadcastReceiver() { - public void onReceive(Context context, Intent intent) - { - final String action = intent.getAction(); - Log.i(TAG, "BroadcastReceiver USB action " + action); - - if (ACTION_USB_PERMISSION.equals(action)) { - _handleUsbPermission(intent); - } else if (UsbManager.ACTION_USB_DEVICE_DETACHED.equals(action)) { - _handleUsbDeviceDetached(intent); - } else if (UsbManager.ACTION_USB_DEVICE_ATTACHED.equals(action)) { - _handleUsbDeviceAttached(intent); - } - - try { - nativeUpdateAvailableJoysticks(); - } catch (final Exception ex) { - Log.e(TAG, "Exception nativeUpdateAvailableJoysticks()", ex); - } - } - }; - - private static void _handleUsbPermission(final Intent intent) - { - synchronized (m_instance) { - final UsbDevice device = (UsbDevice)intent.getParcelableExtra(UsbManager.EXTRA_DEVICE); - if (device != null) { - if (intent.getBooleanExtra(UsbManager.EXTRA_PERMISSION_GRANTED, false)) { - Log.i(TAG, "Permission granted to " + device.getDeviceName()); - } else { - Log.i(TAG, "Permission denied for " + device.getDeviceName()); - } - } - } - } + private PowerManager.WakeLock m_wakeLock; + private WifiManager.MulticastLock m_wifiMulticastLock; - private static void _handleUsbDeviceDetached(final Intent intent) - { - final UsbDevice device = intent.getParcelableExtra(UsbManager.EXTRA_DEVICE); - if (device != null) { - if (m_classPtrHashByDeviceId.containsKey(device.getDeviceId())) { - nativeDeviceHasDisconnected(m_classPtrHashByDeviceId.get(device.getDeviceId())); - } - } - } - - private static void _handleUsbDeviceAttached(final Intent intent) - { - final UsbDevice device = intent.getParcelableExtra(UsbManager.EXTRA_DEVICE); - if (device != null) { - final UsbSerialDriver driver = _findDriverByDeviceId(device.getDeviceId()); - if ((driver != null) && !m_usbManager.hasPermission(device)) { - Log.i(TAG, "Requesting permission for device " + device.getDeviceName()); - m_usbManager.requestPermission(device, m_usbPermissionIntent); - } - } - } - - // Native C++ functions which connect back to QSerialPort code - private static native void nativeDeviceHasDisconnected(final long classPtr); - public static native void nativeDeviceException(final long classPtr, final String message); - public static native void nativeDeviceNewData(final long classPtr, final byte[] data); - private static native void nativeUpdateAvailableJoysticks(); - - // Native C++ functions called to log output - public static native void qgcLogDebug(final String message); - public static native void qgcLogWarning(final String message); - - public native void nativeInit(); - - // QGCActivity singleton - public QGCActivity() - { + public QGCActivity() { m_instance = this; - m_drivers = new ArrayList<>(); - m_classPtrHashByDeviceId = new HashMap<>(); - m_serialIoManagerHashByDeviceId = new HashMap<>(); - m_fileDescriptorHashByDeviceId = new HashMap<>(); } - public void onInit(int status) - { - // System.loadLibrary("gstreamer_android"); + /** + * Returns the singleton instance of QGCActivity. + * + * @return The current instance of QGCActivity. + */ + public static QGCActivity getInstance() { + return m_instance; } @Override - public void onCreate(Bundle savedInstanceState) - { + public void onCreate(Bundle savedInstanceState) { super.onCreate(savedInstanceState); nativeInit(); - _acquireWakeLock(); - _keepScreenOn(); - _setupMulticastLock(); - _registerReceivers(); - _setupUsbPermissionIntent(); - - m_usbManager = (UsbManager) m_instance.getSystemService(Context.USB_SERVICE); + acquireWakeLock(); + keepScreenOn(); + setupMulticastLock(); - // try { - // GStreamer.init(this); - // } catch (final Exception ex) { - // Log.e(TAG, "Exception GStreamer.init(this)", ex); - // } + QGCUsbSerialManager.initialize(this); } @Override - protected void onDestroy() - { + protected void onDestroy() { try { - _releaseMulticastLock(); - _releaseWakeLock(); - } catch(final Exception e) { - Log.e(TAG, "Exception onDestroy()", e); + releaseMulticastLock(); + releaseWakeLock(); + QGCUsbSerialManager.cleanup(this); + } catch (final Exception e) { + Log.e(TAG, "Exception onDestroy()", e); } super.onDestroy(); } - private void _keepScreenOn() - { + /** + * Keeps the screen on by adding the appropriate window flag. + */ + private void keepScreenOn() { getWindow().addFlags(WindowManager.LayoutParams.FLAG_KEEP_SCREEN_ON); } - private void _acquireWakeLock() - { + /** + * Acquires a wake lock to keep the CPU running. + */ + private void acquireWakeLock() { final PowerManager pm = (PowerManager) getSystemService(Context.POWER_SERVICE); m_wakeLock = pm.newWakeLock(PowerManager.SCREEN_BRIGHT_WAKE_LOCK, SCREEN_BRIGHT_WAKE_LOCK_TAG); if (m_wakeLock != null) { @@ -199,18 +77,21 @@ private void _acquireWakeLock() } } - private void _releaseWakeLock() - { - if (m_wakeLock != null) { + /** + * Releases the wake lock if held. + */ + private void releaseWakeLock() { + if (m_wakeLock != null && m_wakeLock.isHeld()) { m_wakeLock.release(); } } - // Workaround for QTBUG-73138 - private void _setupMulticastLock() - { + /** + * Sets up a multicast lock to allow multicast packets. + */ + private void setupMulticastLock() { if (m_wifiMulticastLock == null) { - final WifiManager wifi = (WifiManager)getSystemService(Context.WIFI_SERVICE); + final WifiManager wifi = (WifiManager) getSystemService(Context.WIFI_SERVICE); m_wifiMulticastLock = wifi.createMulticastLock(MULTICAST_LOCK_TAG); m_wifiMulticastLock.setReferenceCounted(true); } @@ -219,1017 +100,18 @@ private void _setupMulticastLock() Log.d(TAG, "Multicast lock: " + m_wifiMulticastLock.toString()); } - private void _releaseMulticastLock() - { - if (m_wifiMulticastLock != null) { - m_wifiMulticastLock.release(); - Log.d(TAG, "Multicast lock released."); - } - } - - private void _registerReceivers() - { - final IntentFilter filter = new IntentFilter(); - filter.addAction(UsbManager.ACTION_USB_DEVICE_ATTACHED); - filter.addAction(UsbManager.ACTION_USB_DEVICE_DETACHED); - filter.addAction(ACTION_USB_PERMISSION); - filter.addAction(BluetoothDevice.ACTION_ACL_CONNECTED); - filter.addAction(BluetoothDevice.ACTION_ACL_DISCONNECTED); - if (android.os.Build.VERSION.SDK_INT >= android.os.Build.VERSION_CODES.TIRAMISU) { - registerReceiver(m_usbReceiver, filter, RECEIVER_EXPORTED); - } else { - registerReceiver(m_usbReceiver, filter); - } - } - - private void _setupUsbPermissionIntent() - { - int intentFlags = 0; - if (android.os.Build.VERSION.SDK_INT >= android.os.Build.VERSION_CODES.M) { - intentFlags = PendingIntent.FLAG_IMMUTABLE; - } - m_usbPermissionIntent = PendingIntent.getBroadcast(this, 0, new Intent(ACTION_USB_PERMISSION), intentFlags); - } - - private static UsbSerialDriver _findDriverByDeviceId(int deviceId) - { - for (final UsbSerialDriver driver: m_drivers) { - if (driver.getDevice().getDeviceId() == deviceId) { - return driver; - } - } - - return null; - } - - private static UsbSerialDriver _findDriverByDeviceName(final String deviceName) - { - for (final UsbSerialDriver driver: m_drivers) { - if (driver.getDevice().getDeviceName().equals(deviceName)) { - return driver; - } - } - - return null; - } - - private static UsbSerialPort _findPortByDeviceId(int deviceId) - { - final UsbSerialDriver driver = _findDriverByDeviceId(deviceId); - if (driver == null) { - return null; - } - - final List ports = driver.getPorts(); - if (ports.isEmpty()) { - Log.w(TAG, "No ports available on device ID " + deviceId); - return null; - } - - final UsbSerialPort port = ports.get(0); - return port; - } - - /*private static void _refreshUsbDevices() - { - final UsbSerialProber usbDefaultProber = UsbSerialProber.getDefaultProber(); - final UsbSerialProber usbQGCProber = QGCProber.getQGCProber(); - - m_drivers.clear(); - for (final UsbDevice device : m_usbManager.getDeviceList().values()) { - UsbSerialDriver driver = usbDefaultProber.probeDevice(device); - if (driver == null) { - driver = usbQGCProber.probeDevice(device); - } - if (driver != null) { - m_drivers.add(driver); - } - } - }*/ - /** - * @brief Incrementally updates the list of drivers connected to the device. + * Releases the multicast lock if held. */ - private static void _updateCurrentDrivers() - { - final UsbSerialProber usbDefaultProber = UsbSerialProber.getDefaultProber(); - List currentDrivers = usbDefaultProber.findAllDrivers(m_usbManager); - if (currentDrivers.isEmpty()) { - currentDrivers = QGCProber.getQGCProber().findAllDrivers(m_usbManager); - } - - if (currentDrivers.isEmpty()) { - return; - } - - _removeStaleDrivers(currentDrivers); - - _addNewDrivers(currentDrivers); - } - - private static void _removeStaleDrivers(final List currentDrivers) - { - for (int i = m_drivers.size() - 1; i >= 0; i--) { - boolean found = false; - for (final UsbSerialDriver currentDriver : currentDrivers) { - if (m_drivers.get(i).getDevice().getDeviceId() == currentDriver.getDevice().getDeviceId()) { - found = true; - break; - } - } - - if (!found) { - Log.i(TAG, "Remove stale driver " + m_drivers.get(i).getDevice().getDeviceName()); - m_drivers.remove(i); - } - } - } - - private static void _addNewDrivers(final List currentDrivers) - { - for (final UsbSerialDriver newDriver : currentDrivers) { - boolean found = false; - for (final UsbSerialDriver driver : m_drivers) { - if (newDriver.getDevice().getDeviceId() == driver.getDevice().getDeviceId()) { - found = true; - break; - } - } - - if (!found) { - _addDriver(newDriver); - } - } - } - - private static void _addDriver(final UsbSerialDriver newDriver) - { - final UsbDevice device = newDriver.getDevice(); - final String deviceName = device.getDeviceName(); - - m_drivers.add(newDriver); - Log.i(TAG, "Adding new driver " + deviceName); - - if (m_usbManager.hasPermission(device)) { - Log.i(TAG, "Already have permission to use device " + deviceName); - } else { - Log.i(TAG, "Requesting permission to use device " + deviceName); - m_usbManager.requestPermission(device, m_usbPermissionIntent); - } - } - - /** - * @brief Returns an array of device info for each device. - * - * @return String[] Device info in the format "DeviceName:ProductName:Manufacturer:SerialNumber:ProductId:VendorId". - */ - public static String[] availableDevicesInfo() - { - _updateCurrentDrivers(); - - if (m_usbManager.getDeviceList().size() < 1) { - return null; - } - - final List deviceInfoList = new ArrayList<>(); - - for (final UsbDevice device : m_usbManager.getDeviceList().values()) { - final String deviceInfo = _formatDeviceInfo(device); - deviceInfoList.add(deviceInfo); - } - - return deviceInfoList.toArray(new String[0]); - } - - private static String _formatDeviceInfo(final UsbDevice device) - { - String deviceInfo = ""; - deviceInfo += device.getDeviceName() + ":"; - deviceInfo += device.getProductName() + ":"; - deviceInfo += device.getManufacturerName() + ":"; - deviceInfo += device.getSerialNumber() + ":"; - deviceInfo += device.getProductId() + ":"; - deviceInfo += device.getVendorId() + ":"; - - Log.i(TAG, "_formatDeviceInfo " + deviceInfo); - - return deviceInfo; - } - - /** - * @brief Opens the specified device. - * - * @param classPtr Data to associate with the device and pass back through to native calls. - * @return int Device ID. - */ - public static int open(final Context parentContext, final String deviceName, final long classPtr) - { - m_context = parentContext; - - final UsbSerialDriver driver = _findDriverByDeviceName(deviceName); - if (driver == null) { - Log.w(TAG, "Attempt to open unknown device " + deviceName); - return BAD_DEVICE_ID; - } - - final List ports = driver.getPorts(); - if (ports.isEmpty()) { - Log.w(TAG, "No ports available on device " + deviceName); - return BAD_DEVICE_ID; - } - - final UsbSerialPort port = ports.get(0); - - final UsbDevice device = driver.getDevice(); - final int deviceId = device.getDeviceId(); - - if (!_openDriver(port, device, deviceId, classPtr)) { - return BAD_DEVICE_ID; - } - - return deviceId; - } - - private static boolean _openDriver(final UsbSerialPort port, final UsbDevice device, final int deviceId, final long classPtr) - { - final UsbDeviceConnection connection = m_usbManager.openDevice(device); - if (connection == null) { - Log.w(TAG, "No Usb Device Connection"); - // TODO: add UsbManager.requestPermission(driver.getDevice(), ..) handling here? - return false; - } - - try { - port.open(connection); - } catch (final IOException ex) { - Log.e(TAG, "Error opening driver", ex); - return false; - } - - if (!setParameters(deviceId, 115200, 8, UsbSerialPort.STOPBITS_1, UsbSerialPort.PARITY_NONE)) { - return false; - } - - if (!_createIoManager(deviceId, port, classPtr)) { - return false; - } - - m_classPtrHashByDeviceId.put(deviceId, classPtr); - m_fileDescriptorHashByDeviceId.put(deviceId, connection.getFileDescriptor()); - - /*final ArrayList descriptors = UsbUtils.getDescriptors(connection); - for (byte[] descriptor : descriptors) { - Log.d(TAG, "Descriptor: " + HexDump.toHexString(descriptor)); - }*/ - - Log.d(TAG, "Port open successful"); - return true; - } - - private static boolean _createIoManager(final int deviceId, final UsbSerialPort port, final long classPtr) - { - if (m_serialIoManagerHashByDeviceId.get(deviceId) != null) { - return true; - } - - final QGCSerialListener listener = new QGCSerialListener(classPtr); - final SerialInputOutputManager ioManager = new SerialInputOutputManager(port, listener); - - // ioManager.setReadBufferSize(4096); - // ioManager.setWriteBufferSize(4096); - - Log.d(TAG, "Read Buffer Size: " + ioManager.getReadBufferSize()); - Log.d(TAG, "Write Buffer Size: " + ioManager.getWriteBufferSize()); - - try { - ioManager.setReadTimeout(0); - } catch (final IllegalStateException e) { - Log.e(TAG, "Set Read Timeout exception:", e); - return false; - } - - ioManager.setWriteTimeout(0); - - try { - ioManager.setThreadPriority(Process.THREAD_PRIORITY_URGENT_AUDIO); - } catch (final IllegalStateException e) { - Log.e(TAG, "Set Thread Priority exception:", e); - } - - m_serialIoManagerHashByDeviceId.put(deviceId, ioManager); - - return true; - } - - public static boolean startIoManager(final int deviceId) - { - if (ioManagerRunning(deviceId)) { - return true; - } - - final SerialInputOutputManager ioManager = m_serialIoManagerHashByDeviceId.get(deviceId); - if (ioManager == null) { - return false; - } - - try { - ioManager.start(); - } catch (final IllegalStateException e) { - Log.e(TAG, "IO Manager Start exception:", e); - return false; - } - - return true; - } - - public static boolean stopIoManager(final int deviceId) - { - final SerialInputOutputManager ioManager = m_serialIoManagerHashByDeviceId.get(deviceId); - if (ioManager == null) { - return false; - } - - final SerialInputOutputManager.State ioState = ioManager.getState(); - if (ioState == SerialInputOutputManager.State.STOPPED || ioState == SerialInputOutputManager.State.STOPPING) { - return true; - } - - if (ioManagerRunning(deviceId)) { - ioManager.stop(); - } - - return true; - } - - public static boolean ioManagerRunning(final int deviceId) - { - final SerialInputOutputManager ioManager = m_serialIoManagerHashByDeviceId.get(deviceId); - if (ioManager == null) { - return false; - } - - final SerialInputOutputManager.State ioState = ioManager.getState(); - return (ioState == SerialInputOutputManager.State.RUNNING); - } - - public static int ioManagerReadBufferSize(final int deviceId) - { - final SerialInputOutputManager ioManager = m_serialIoManagerHashByDeviceId.get(deviceId); - if (ioManager == null) { - return 0; - } - - return ioManager.getReadBufferSize(); - } - - /** - * @brief Closes the device. - * - * @param deviceId ID number from the open command. - * @return true if the operation is successful, false otherwise. - */ - public static boolean close(int deviceId) - { - final UsbSerialPort port = _findPortByDeviceId(deviceId); - if (port == null) { - return false; - } - - try { - port.close(); - } catch (final IOException ex) { - Log.e(TAG, "Error closing driver:", ex); - return false; - } - - if (stopIoManager(deviceId)) { - m_serialIoManagerHashByDeviceId.remove(deviceId); - } - - m_classPtrHashByDeviceId.remove(deviceId); - m_fileDescriptorHashByDeviceId.remove(deviceId); - - return true; - } - - /** - * @brief Writes data to the device. - * - * @param deviceId ID number from the open command. - * @param data Byte array of data to write. - * @param timeoutMsec Amount of time in milliseconds to wait for the write to occur. - * @return int Number of bytes written. - */ - public static int write(final int deviceId, final byte[] data, final int length, final int timeoutMSec) - { - final UsbSerialPort port = _findPortByDeviceId(deviceId); - if (port == null) { - return -1; - } - - try { - port.write(data, length, timeoutMSec); - } catch (final SerialTimeoutException e) { - Log.e(TAG, "Write timeout occurred", e); - return -1; - } catch (final IOException e) { - Log.e(TAG, "Error writing data", e); - return -1; - } - - return length; - } - - /** - * Writes data to the device asynchronously. - * - * @param deviceId ID number from the open command. - * @param data Byte array of data to write. - */ - public static int writeAsync(final int deviceId, final byte[] data, final int timeoutMSec) - { - final SerialInputOutputManager ioManager = m_serialIoManagerHashByDeviceId.get(deviceId); - if (ioManager == null) { - Log.w(TAG, "IO Manager not found for device ID " + deviceId); - return -1; - } - - if (ioManager.getReadTimeout() == 0) { - return -1; - } - - ioManager.setWriteTimeout(timeoutMSec); - - ioManager.writeAsync(data); - - return data.length; - } - - public static byte[] read(final int deviceId, final int length, final int timeoutMs) - { - final UsbSerialPort port = _findPortByDeviceId(deviceId); - if (port == null) { - return new byte[] {}; - } - - byte[] buffer = new byte[length]; - int bytesRead = 0; - - try { - // TODO: Use bytesRead - bytesRead = port.read(buffer, timeoutMs); - } catch (final IOException e) { - Log.e(TAG, "Error reading data", e); - } - - return buffer; - } - - public static boolean isDeviceNameValid(final String name) - { - for (final UsbSerialDriver driver: m_drivers) { - if (driver.getDevice().getDeviceName().equals(name)) { - return true; - } - } - - return false; - } - - public static boolean isDeviceNameOpen(final String name) - { - for (final UsbSerialDriver driver: m_drivers) { - final List ports = driver.getPorts(); - if (!ports.isEmpty() && name.equals(driver.getDevice().getDeviceName()) && ports.get(0).isOpen()) { - return true; - } - } - - return false; - } - - /** - * @brief Get Device ID. - * - * @param deviceName Device Name. - * @return int Device ID. - */ - public static int getDeviceId(final String deviceName) - { - final UsbSerialDriver driver = _findDriverByDeviceName(deviceName); - if (driver == null) { - Log.w(TAG, "Attempt to open unknown device " + deviceName); - return BAD_DEVICE_ID; - } - - final UsbDevice device = driver.getDevice(); - final int deviceId = device.getDeviceId(); - - return deviceId; - } - - /** - * @brief Sets the parameters on an open port. - * - * @param deviceId ID number from the open command. - * @param baudRate Decimal value of the baud rate. For example, 9600, 57600, 115200, etc. - * @param dataBits Number of data bits. Valid numbers are 5, 6, 7, 8. - * @param stopBits Number of stop bits. Valid numbers are 1, 2. - * @param parity Parity setting: No Parity=0, Odd Parity=1, Even Parity=2. - * @return true if the operation is successful, false otherwise. - */ - public static boolean setParameters(final int deviceId, final int baudRate, final int dataBits, final int stopBits, final int parity) - { - final UsbSerialPort port = _findPortByDeviceId(deviceId); - if (port == null) { - return false; - } - - try { - port.setParameters(baudRate, dataBits, stopBits, parity); - } catch (final IOException e) { - Log.e(TAG, "Error setting parameters", e); - return false; - } catch (final UnsupportedOperationException e) { - Log.e(TAG, "Error setting parameters", e); - return false; - } - - return true; - } - - private static boolean _isControlLineSupported(final UsbSerialPort port, final UsbSerialPort.ControlLine controlLine) - { - EnumSet supportedControlLines; - - try { - supportedControlLines = port.getSupportedControlLines(); - } catch (final IOException e) { - Log.e(TAG, "Error getting supported control lines", e); - return false; - } - - return supportedControlLines.contains(controlLine); - } - - /** - * @brief Gets the Carrier Detect (CD) flag. - * - * @param deviceId ID number from the open command. - */ - public static boolean getCarrierDetect(final int deviceId) - { - final UsbSerialPort port = _findPortByDeviceId(deviceId); - if (port == null) { - return false; - } - - if (!_isControlLineSupported(port, UsbSerialPort.ControlLine.CD)) { - Log.e(TAG, "Getting CD Not Supported"); - return false; - } - - boolean cd; - - try { - cd = port.getCD(); - } catch (final IOException e) { - Log.e(TAG, "Error getting CD", e); - return false; - } catch (final UnsupportedOperationException e) { - Log.e(TAG, "Error getting CD", e); - return false; - } - - return cd; - } - - /** - * @brief Gets the Clear To Send (CTS) flag. - * - * @param deviceId ID number from the open command. - */ - public static boolean getClearToSend(final int deviceId) - { - final UsbSerialPort port = _findPortByDeviceId(deviceId); - if (port == null) { - return false; - } - - if (!_isControlLineSupported(port, UsbSerialPort.ControlLine.CTS)) { - Log.e(TAG, "Getting CTS Not Supported"); - return false; - } - - boolean cts; - - try { - cts = port.getCTS(); - } catch (final IOException e) { - Log.e(TAG, "Error getting CTS", e); - return false; - } catch (final UnsupportedOperationException e) { - Log.e(TAG, "Error getting CTS", e); - return false; - } - - return cts; - } - - /** - * @brief Gets the Data Set Ready (DSR) flag. - * - * @param deviceId ID number from the open command. - */ - public static boolean getDataSetReady(final int deviceId) - { - final UsbSerialPort port = _findPortByDeviceId(deviceId); - if (port == null) { - return false; - } - - if (!_isControlLineSupported(port, UsbSerialPort.ControlLine.DSR)) { - Log.e(TAG, "Getting DSR Not Supported"); - return false; - } - - boolean dsr; - - try { - dsr = port.getDSR(); - } catch (final IOException e) { - Log.e(TAG, "Error getting DSR", e); - return false; - } catch (final UnsupportedOperationException e) { - Log.e(TAG, "Error getting DSR", e); - return false; - } - - return dsr; - } - - /** - * @brief Gets the Data Terminal Ready (DTR) flag. - * - * @param deviceId ID number from the open command. - */ - public static boolean getDataTerminalReady(final int deviceId) - { - final UsbSerialPort port = _findPortByDeviceId(deviceId); - if (port == null) { - return false; - } - - if (!_isControlLineSupported(port, UsbSerialPort.ControlLine.DTR)) { - Log.e(TAG, "Getting DTR Not Supported"); - return false; - } - - boolean dtr; - - try { - dtr = port.getDTR(); - } catch (final IOException e) { - Log.e(TAG, "Error getting DTR", e); - return false; - } catch (final UnsupportedOperationException e) { - Log.e(TAG, "Error getting DTR", e); - return false; - } - - return dtr; - } - - /** - * @brief Sets the Data Terminal Ready (DTR) flag on the device. - * - * @param deviceId ID number from the open command. - * @param on true to turn on, false to turn off. - * @return true if the operation is successful, false otherwise. - */ - public static boolean setDataTerminalReady(final int deviceId, final boolean on) - { - final UsbSerialPort port = _findPortByDeviceId(deviceId); - if (port == null) { - return false; - } - - if (!_isControlLineSupported(port, UsbSerialPort.ControlLine.DTR)) { - Log.e(TAG, "Setting DTR Not Supported"); - return false; - } - - try { - port.setDTR(on); - } catch (final IOException e) { - Log.e(TAG, "Error setting DTR", e); - return false; - } catch (final UnsupportedOperationException e) { - Log.e(TAG, "Error setting DTR", e); - return false; - } - - return true; - } - - /** - * @brief Gets the Ring Indicator (RI) flag. - * - * @param deviceId ID number from the open command. - */ - public static boolean getRingIndicator(final int deviceId) - { - final UsbSerialPort port = _findPortByDeviceId(deviceId); - if (port == null) { - return false; - } - - if (!_isControlLineSupported(port, UsbSerialPort.ControlLine.RI)) { - Log.e(TAG, "Getting RI Not Supported"); - return false; - } - - boolean ri; - - try { - ri = port.getRI(); - } catch (final IOException e) { - Log.e(TAG, "Error getting RI", e); - return false; - } catch (final UnsupportedOperationException e) { - Log.e(TAG, "Error getting RI", e); - return false; - } - - return ri; - } - - /** - * @brief Gets the Request to Send (RTS) flag. - * - * @param deviceId ID number from the open command. - */ - public static boolean getRequestToSend(final int deviceId) - { - final UsbSerialPort port = _findPortByDeviceId(deviceId); - if (port == null) { - return false; - } - - if (!_isControlLineSupported(port, UsbSerialPort.ControlLine.RTS)) { - Log.e(TAG, "Getting RTS Not Supported"); - return false; - } - - boolean rts; - - try { - rts = port.getRTS(); - } catch (final IOException e) { - Log.e(TAG, "Error getting RTS", e); - return false; - } catch (final UnsupportedOperationException e) { - Log.e(TAG, "Error getting RTS", e); - return false; - } - - return rts; - } - - /** - * @brief Sets the Request to Send (RTS) flag. - * - * @param deviceId ID number from the open command. - * @param on true to turn on, false to turn off. - * @return true if the operation is successful, false otherwise. - */ - public static boolean setRequestToSend(final int deviceId, final boolean on) - { - final UsbSerialPort port = _findPortByDeviceId(deviceId); - if (port == null) { - return false; - } - - if (!_isControlLineSupported(port, UsbSerialPort.ControlLine.RTS)) { - Log.e(TAG, "Setting RTS Not Supported"); - return false; - } - - try { - port.setRTS(on); - } catch (final IOException e) { - Log.e(TAG, "Error setting RTS", e); - return false; - } catch (final UnsupportedOperationException e) { - Log.e(TAG, "Error setting RTS", e); - return false; - } - - return true; - } - - /** - * @brief Gets the Control Lines flags. - * - * @param deviceId ID number from the open command. - */ - public static int[] getControlLines(final int deviceId) - { - final UsbSerialPort port = _findPortByDeviceId(deviceId); - if (port == null) { - return new int[] {}; - } - - EnumSet supportedControlLines; - - try { - supportedControlLines = port.getSupportedControlLines(); - } catch (final IOException e) { - Log.e(TAG, "Error getting supported control lines", e); - return new int[] {}; - } - - if (supportedControlLines.isEmpty()) { - Log.e(TAG, "Control Lines Not Supported"); - return new int[] {}; - } - - EnumSet controlLines; - - try { - controlLines = port.getControlLines(); - } catch (final IOException e) { - Log.e(TAG, "Error getting RTS", e); - return new int[] {}; - } - - return controlLines.stream().mapToInt(UsbSerialPort.ControlLine::ordinal).toArray(); - } - - /** - * @brief Gets the Flow Control type. - * - * @param deviceId ID number from the open command. - */ - public static int getFlowControl(final int deviceId) - { - final UsbSerialPort port = _findPortByDeviceId(deviceId); - if (port == null) { - return 0; - } - - final EnumSet supportedFlowControl = port.getSupportedFlowControl(); - - if (supportedFlowControl.isEmpty()) { - Log.e(TAG, "Flow Control Not Supported"); - return 0; - } - - final UsbSerialPort.FlowControl flowControl = port.getFlowControl(); - - return flowControl.ordinal(); - } - - /** - * @brief Sets the Flow Control flag. - * - * @param deviceId ID number from the open command. - */ - public static boolean setFlowControl(final int deviceId, final int flowControl) - { - if (getFlowControl(deviceId) == flowControl) { - return true; - } - - if ((flowControl < 0) || (flowControl >= UsbSerialPort.FlowControl.values().length)) { - Log.w(TAG, "Invalid flow control ordinal " + flowControl); - return false; - } - - final UsbSerialPort.FlowControl flowControlEnum = UsbSerialPort.FlowControl.values()[flowControl]; - - final UsbSerialPort port = _findPortByDeviceId(deviceId); - if (port == null) { - return false; - } - - final EnumSet supportedFlowControl = port.getSupportedFlowControl(); - - if (!supportedFlowControl.contains(flowControlEnum)) { - Log.e(TAG, "Setting Flow Control Not Supported"); - return false; - } - - // TODO: This shouldn't be necessary but an UnsupportedOperationException is thrown for NONE - // if ((flowControlEnum == UsbSerialPort.FlowControl.NONE) && (supportedFlowControl.size() == 1)) { - // return true; - // } - - try { - port.setFlowControl(flowControlEnum); - } catch (final IOException e) { - Log.e(TAG, "Error setting Flow Control", e); - return false; - } catch (final UnsupportedOperationException e) { - Log.e(TAG, "Error setting Flow Control", e); - return false; - } - - return true; - } - - /** - * Sets the break condition on the device. - * - * @param deviceId ID number from the open command. - * @param on true to set break, false to clear break. - * @return true if the operation is successful, false otherwise. - */ - public static boolean setBreak(final int deviceId, final boolean on) - { - final UsbSerialPort port = _findPortByDeviceId(deviceId); - if (port == null) { - return false; - } - - try { - port.setBreak(on); - } catch (final IOException e) { - Log.e(TAG, "Error setting break condition", e); - return false; - } - - return true; - } - - /** - * @brief Purges the hardware buffers based on the input and output flags. - * - * @param deviceId ID number from the open command. - * @param input true to purge the input buffer. - * @param output true to purge the output buffer. - * @return true if the operation is successful, false otherwise. - */ - public static boolean purgeBuffers(final int deviceId, final boolean input, final boolean output) - { - final UsbSerialPort port = _findPortByDeviceId(deviceId); - if (port == null) { - return false; - } - - try { - port.purgeHwBuffers(input, output); - } catch (final IOException e) { - Log.e(TAG, "Error purging buffers", e); - return false; - } catch (final UnsupportedOperationException e) { - Log.e(TAG, "Error purging buffers", e); - return false; - } - - return true; - } - - /** - * @brief Gets the native device handle (file descriptor). - * - * @param deviceId ID number from the open command. - * @return int Device handle. - */ - public static int getDeviceHandle(final int deviceId) - { - return m_fileDescriptorHashByDeviceId.getOrDefault(deviceId, -1); - } - - public static String getSDCardPath() - { - final StorageManager storageManager = (StorageManager) m_instance.getSystemService(Activity.STORAGE_SERVICE); - final List volumes = storageManager.getStorageVolumes(); - for (final StorageVolume vol : volumes) { - final String path = getStorageVolumePath(vol); - if (path != null) { - return path; - } + private void releaseMulticastLock() { + if (m_wifiMulticastLock != null && m_wifiMulticastLock.isHeld()) { + m_wifiMulticastLock.release(); + Log.d(TAG, "Multicast lock released."); } - - return ""; } - private static String getStorageVolumePath(final StorageVolume vol) - { - try { - final Method methodGetPath = vol.getClass().getMethod("getPath"); - final String path = (String) methodGetPath.invoke(vol); - if (vol.isRemovable()) { - Log.i(TAG, "removable sd card mounted " + path); - return path; - } else { - Log.i(TAG, "storage mounted " + path); - } - } catch (final Exception e) { - Log.e(TAG, "Error getting storage volume path", e); - } - - return null; - } + // Native C++ functions + public native boolean nativeInit(); + public native void qgcLogDebug(final String message); + public native void qgcLogWarning(final String message); } diff --git a/android/src/org/mavlink/qgroundcontrol/QGCSerialListener.java b/android/src/org/mavlink/qgroundcontrol/QGCSerialListener.java deleted file mode 100644 index 85399c54c6f..00000000000 --- a/android/src/org/mavlink/qgroundcontrol/QGCSerialListener.java +++ /dev/null @@ -1,31 +0,0 @@ -package org.mavlink.qgroundcontrol; - -import android.util.Log; - -import com.hoho.android.usbserial.util.SerialInputOutputManager; - -public class QGCSerialListener implements SerialInputOutputManager.Listener -{ - private final static String TAG = QGCSerialListener.class.getSimpleName(); - - private long classPtr; - - QGCSerialListener(long classPtr) - { - this.classPtr = classPtr; - } - - @Override - public void onNewData(byte[] data) - { - // Log.i(TAG, "Serial read:" + new String(data, StandardCharsets.UTF_8)); - QGCActivity.nativeDeviceNewData(classPtr, data); - } - - @Override - public void onRunError(Exception ex) - { - Log.e(TAG, "onRunError Exception", ex); - QGCActivity.nativeDeviceException(classPtr, ex.getMessage()); - } -} diff --git a/android/src/org/mavlink/qgroundcontrol/QGCUsbSerialManager.java b/android/src/org/mavlink/qgroundcontrol/QGCUsbSerialManager.java new file mode 100644 index 00000000000..e72c8bcf9a9 --- /dev/null +++ b/android/src/org/mavlink/qgroundcontrol/QGCUsbSerialManager.java @@ -0,0 +1,1187 @@ +package org.mavlink.qgroundcontrol; + +import android.app.PendingIntent; +import android.bluetooth.BluetoothDevice; +import android.content.*; +import android.hardware.usb.*; +import android.os.Process; +import android.util.Log; + +import com.hoho.android.usbserial.driver.*; +import com.hoho.android.usbserial.util.*; + +import java.io.IOException; +import java.lang.reflect.Method; +import java.util.*; + +public class QGCUsbSerialManager { + private static final String TAG = QGCUsbSerialManager.class.getSimpleName(); + private static final String ACTION_USB_PERMISSION = "org.mavlink.qgroundcontrol.action.USB_PERMISSION"; + private static final int BAD_DEVICE_ID = 0; + + private static UsbManager usbManager; + private static PendingIntent usbPermissionIntent; + + private static List drivers = new ArrayList<>(); + private static HashMap serialIoManagerHashByDeviceId = new HashMap<>(); + private static HashMap fileDescriptorHashByDeviceId = new HashMap<>(); + private static HashMap classPtrHashByDeviceId = new HashMap<>(); + + // Native methods + private static native void nativeDeviceHasDisconnected(final long classPtr); + public static native void nativeDeviceException(final long classPtr, final String message); + public static native void nativeDeviceNewData(final long classPtr, final byte[] data); + private static native void nativeUpdateAvailableJoysticks(); + + /** + * Initializes the UsbSerialManager. Should be called once, typically from QGCActivity.onCreate(). + * + * @param context The application context. + */ + public static void initialize(Context context) { + if (usbManager != null) { + // Already initialized + return; + } + + usbManager = (UsbManager) context.getSystemService(Context.USB_SERVICE); + setupUsbPermissionIntent(context); + registerReceivers(context); + updateCurrentDrivers(); + } + + /** + * Sets up the PendingIntent for USB permission requests. + * + * @param context The application context. + */ + private static void setupUsbPermissionIntent(Context context) { + int intentFlags = 0; + if (android.os.Build.VERSION.SDK_INT >= android.os.Build.VERSION_CODES.M) { + intentFlags = PendingIntent.FLAG_IMMUTABLE; + } + usbPermissionIntent = PendingIntent.getBroadcast(context, 0, new Intent(ACTION_USB_PERMISSION), intentFlags); + } + + /** + * Registers the BroadcastReceiver to listen for USB-related events. + * + * @param context The application context. + */ + private static void registerReceivers(Context context) { + IntentFilter filter = new IntentFilter(); + filter.addAction(UsbManager.ACTION_USB_DEVICE_ATTACHED); + filter.addAction(UsbManager.ACTION_USB_DEVICE_DETACHED); + filter.addAction(ACTION_USB_PERMISSION); + filter.addAction(BluetoothDevice.ACTION_ACL_CONNECTED); + filter.addAction(BluetoothDevice.ACTION_ACL_DISCONNECTED); + + if (android.os.Build.VERSION.SDK_INT >= android.os.Build.VERSION_CODES.TIRAMISU) { + context.registerReceiver(usbReceiver, filter, Context.RECEIVER_EXPORTED); + } else { + context.registerReceiver(usbReceiver, filter); + } + } + + /** + * BroadcastReceiver to handle USB events. + */ + private static final BroadcastReceiver usbReceiver = new BroadcastReceiver() { + @Override + public void onReceive(Context context, Intent intent) { + final String action = intent.getAction(); + Log.i(TAG, "BroadcastReceiver USB action " + action); + + switch (action) { + case ACTION_USB_PERMISSION: + handleUsbPermission(intent); + break; + case UsbManager.ACTION_USB_DEVICE_DETACHED: + handleUsbDeviceDetached(intent); + break; + case UsbManager.ACTION_USB_DEVICE_ATTACHED: + handleUsbDeviceAttached(intent); + break; + default: + break; + } + + try { + nativeUpdateAvailableJoysticks(); + } catch (final Exception ex) { + Log.e(TAG, "Exception nativeUpdateAvailableJoysticks()", ex); + } + } + }; + + /** + * Handles USB permission results. + * + * @param intent The intent containing permission data. + */ + private static void handleUsbPermission(final Intent intent) { + synchronized (QGCUsbSerialManager.class) { + final UsbDevice device = intent.getParcelableExtra(UsbManager.EXTRA_DEVICE); + if (device != null) { + if (intent.getBooleanExtra(UsbManager.EXTRA_PERMISSION_GRANTED, false)) { + Log.i(TAG, "Permission granted to " + device.getDeviceName()); + // updateCurrentDrivers(device.getDeviceId()); + } else { + Log.i(TAG, "Permission denied for " + device.getDeviceName()); + nativeDeviceException(classPtrHashByDeviceId.get(device.getDeviceId()), "USB Permission Denied"); + } + } + } + } + + /** + * Handles USB device detachment events. + * + * @param intent The intent containing device data. + */ + private static void handleUsbDeviceDetached(final Intent intent) { + final UsbDevice device = intent.getParcelableExtra(UsbManager.EXTRA_DEVICE); + if (device != null) { + final int deviceId = device.getDeviceId(); + if (classPtrHashByDeviceId.containsKey(deviceId)) { + final long classPtr = classPtrHashByDeviceId.get(deviceId); + nativeDeviceHasDisconnected(classPtr); + close(deviceId); + } + } + } + + /** + * Handles USB device attachment events. + * + * @param intent The intent containing device data. + */ + private static void handleUsbDeviceAttached(final Intent intent) { + final UsbDevice device = intent.getParcelableExtra(UsbManager.EXTRA_DEVICE); + if (device != null) { + final UsbSerialDriver driver = findDriverByDeviceId(device.getDeviceId()); + if (driver != null) { + if (usbManager.hasPermission(device)) { + Log.i(TAG, "Already have permission to use device " + device.getDeviceName()); + addDriver(driver); + } else { + Log.i(TAG, "Requesting permission for device " + device.getDeviceName()); + usbManager.requestPermission(device, usbPermissionIntent); + } + } + } + } + + /** + * Finds a USB serial driver by its device ID. + * + * @param deviceId The device ID. + * @return The corresponding UsbSerialDriver or null if not found. + */ + private static UsbSerialDriver findDriverByDeviceId(final int deviceId) { + for (final UsbSerialDriver driver : drivers) { + if (driver.getDevice().getDeviceId() == deviceId) { + return driver; + } + } + return null; + } + + /** + * Finds a USB serial driver by its device name. + * + * @param deviceName The device name. + * @return The corresponding UsbSerialDriver or null if not found. + */ + private static UsbSerialDriver findDriverByDeviceName(final String deviceName) { + for (final UsbSerialDriver driver : drivers) { + if (driver.getDevice().getDeviceName().equals(deviceName)) { + return driver; + } + } + return null; + } + + /** + * Finds a USB serial port by its device ID. + * + * @param deviceId The device ID. + * @return The corresponding UsbSerialPort or null if not found. + */ + private static UsbSerialPort findPortByDeviceId(final int deviceId) { + final UsbSerialDriver driver = findDriverByDeviceId(deviceId); + if (driver == null) { + return null; + } + + final List ports = driver.getPorts(); + if (ports.isEmpty()) { + Log.w(TAG, "No ports available on device ID " + deviceId); + return null; + } + + return ports.get(0); + } + + /** + * Retrieves information about all available USB serial devices. + * + * @return An array of device information strings or null if no devices are available. + */ + public static String[] availableDevicesInfo() { + updateCurrentDrivers(); + + if (usbManager.getDeviceList().size() < 1) { + return null; + } + + final List deviceInfoList = new ArrayList<>(); + + for (final UsbDevice device : usbManager.getDeviceList().values()) { + final String deviceInfo = formatDeviceInfo(device); + deviceInfoList.add(deviceInfo); + } + + return deviceInfoList.toArray(new String[0]); + } + + /** + * Formats device information into a standardized string. + * + * @param device The UsbDevice to format. + * @return A formatted string containing device information. + */ + private static String formatDeviceInfo(final UsbDevice device) { + String deviceInfo = ""; + deviceInfo += device.getDeviceName() + ":"; + deviceInfo += device.getProductName() + ":"; + deviceInfo += device.getManufacturerName() + ":"; + deviceInfo += device.getSerialNumber() + ":"; + deviceInfo += device.getProductId() + ":"; + deviceInfo += device.getVendorId(); + + Log.i(TAG, "Formatted Device Info: " + deviceInfo); + + return deviceInfo; + } + + /** + * Opens a USB serial device. + * + * @param deviceName The name of the device to open. + * @param classPtr A native pointer associated with the device. + * @return The device ID if successful, or BAD_DEVICE_ID if failed. + */ + public static int open(final String deviceName, final long classPtr) { + final UsbSerialDriver driver = findDriverByDeviceName(deviceName); + if (driver == null) { + Log.w(TAG, "Attempt to open unknown device " + deviceName); + return BAD_DEVICE_ID; + } + + final List ports = driver.getPorts(); + if (ports.isEmpty()) { + Log.w(TAG, "No ports available on device " + deviceName); + return BAD_DEVICE_ID; + } + + final UsbSerialPort port = ports.get(0); + final UsbDevice device = driver.getDevice(); + final int deviceId = device.getDeviceId(); + + if (!openDriver(port, device, deviceId, classPtr)) { + Log.e(TAG, "Failed to open driver for device " + deviceName); + nativeDeviceException(classPtr, "Failed to open driver for device: " + deviceName); + return BAD_DEVICE_ID; + } + + return deviceId; + } + + /** + * Opens the driver for a specific USB serial port. + * + * @param port The UsbSerialPort to open. + * @param device The UsbDevice associated with the port. + * @param deviceId The device ID. + * @param classPtr A native pointer associated with the device. + * @return True if successful, false otherwise. + */ + private static boolean openDriver(final UsbSerialPort port, final UsbDevice device, final int deviceId, final long classPtr) { + final UsbDeviceConnection connection = usbManager.openDevice(device); + if (connection == null) { + Log.w(TAG, "No Usb Device Connection"); + nativeDeviceException(classPtr, "No USB device connection for device: " + device.getDeviceName()); + return false; + } + + try { + port.open(connection); + } catch (final IOException ex) { + Log.e(TAG, "Error opening driver for device " + device.getDeviceName(), ex); + nativeDeviceException(classPtr, "Error opening driver: " + ex.getMessage()); + return false; + } + + if (!setParameters(deviceId, 115200, 8, UsbSerialPort.STOPBITS_1, UsbSerialPort.PARITY_NONE)) { + return false; + } + + if (!createIoManager(deviceId, port, classPtr)) { + return false; + } + + classPtrHashByDeviceId.put(deviceId, classPtr); + fileDescriptorHashByDeviceId.put(deviceId, connection.getFileDescriptor()); + + Log.d(TAG, "Port open successful"); + return true; + } + + /** + * Creates and initializes the SerialInputOutputManager for a device. + * + * @param deviceId The device ID. + * @param port The UsbSerialPort to manage. + * @param classPtr A native pointer associated with the device. + * @return True if successful, false otherwise. + */ + private static boolean createIoManager(final int deviceId, final UsbSerialPort port, final long classPtr) { + if (serialIoManagerHashByDeviceId.containsKey(deviceId)) { + return true; + } + + final QGCSerialListener listener = new QGCSerialListener(classPtr); + final SerialInputOutputManager ioManager = new SerialInputOutputManager(port, listener); + + Log.d(TAG, "Read Buffer Size: " + ioManager.getReadBufferSize()); + Log.d(TAG, "Write Buffer Size: " + ioManager.getWriteBufferSize()); + + try { + ioManager.setReadTimeout(0); + } catch (final IllegalStateException e) { + Log.e(TAG, "Set Read Timeout exception:", e); + return false; + } + + ioManager.setWriteTimeout(0); + + try { + ioManager.setThreadPriority(Process.THREAD_PRIORITY_URGENT_AUDIO); + } catch (final IllegalStateException e) { + Log.e(TAG, "Set Thread Priority exception:", e); + } + + serialIoManagerHashByDeviceId.put(deviceId, ioManager); + + return true; + } + + /** + * Starts the SerialInputOutputManager for a specific device. + * + * @param deviceId The device ID. + * @return True if successful or already running, false otherwise. + */ + public static boolean startIoManager(final int deviceId) { + if (ioManagerRunning(deviceId)) { + return true; + } + + final SerialInputOutputManager ioManager = serialIoManagerHashByDeviceId.get(deviceId); + if (ioManager == null) { + return false; + } + + try { + ioManager.start(); + } catch (final IllegalStateException e) { + Log.e(TAG, "IO Manager Start exception:", e); + return false; + } + + Log.d(TAG, "Serial I/O Manager started for device ID " + deviceId); + return true; + } + + /** + * Stops the SerialInputOutputManager for a specific device. + * + * @param deviceId The device ID. + * @return True if successful or already stopped, false otherwise. + */ + public static boolean stopIoManager(final int deviceId) { + final SerialInputOutputManager ioManager = serialIoManagerHashByDeviceId.get(deviceId); + if (ioManager == null) { + return false; + } + + final SerialInputOutputManager.State ioState = ioManager.getState(); + if (ioState == SerialInputOutputManager.State.STOPPED || ioState == SerialInputOutputManager.State.STOPPING) { + return true; + } + + if (ioManagerRunning(deviceId)) { + ioManager.stop(); + } + + return true; + } + + /** + * Checks if the SerialInputOutputManager is running for a specific device. + * + * @param deviceId The device ID. + * @return True if running, false otherwise. + */ + public static boolean ioManagerRunning(final int deviceId) { + final SerialInputOutputManager ioManager = serialIoManagerHashByDeviceId.get(deviceId); + if (ioManager == null) { + return false; + } + + final SerialInputOutputManager.State ioState = ioManager.getState(); + return (ioState == SerialInputOutputManager.State.RUNNING); + } + + /** + * Retrieves the read buffer size of the SerialInputOutputManager for a specific device. + * + * @param deviceId The device ID. + * @return The read buffer size, or 0 if not available. + */ + public static int ioManagerReadBufferSize(final int deviceId) { + final SerialInputOutputManager ioManager = serialIoManagerHashByDeviceId.get(deviceId); + if (ioManager == null) { + return 0; + } + + return ioManager.getReadBufferSize(); + } + + /** + * Closes the USB serial device. + * + * @param deviceId The device ID. + * @return True if successful, false otherwise. + */ + public static boolean close(int deviceId) { + final UsbSerialPort port = findPortByDeviceId(deviceId); + if (port == null) { + return false; + } + + try { + port.close(); + } catch (final IOException ex) { + Log.e(TAG, "Error closing driver:", ex); + return false; + } + + if (stopIoManager(deviceId)) { + serialIoManagerHashByDeviceId.remove(deviceId); + } + + classPtrHashByDeviceId.remove(deviceId); + fileDescriptorHashByDeviceId.remove(deviceId); + + Log.d(TAG, "Device " + deviceId + " closed successfully."); + return true; + } + + /** + * Writes data to the USB serial device. + * + * @param deviceId The device ID. + * @param data The byte array of data to write. + * @param length The number of bytes to write. + * @param timeoutMSec The timeout in milliseconds. + * @return The number of bytes written, or -1 if failed. + */ + public static int write(final int deviceId, final byte[] data, final int length, final int timeoutMSec) { + final UsbSerialPort port = findPortByDeviceId(deviceId); + if (port == null) { + return -1; + } + + try { + port.write(data, length, timeoutMSec); + } catch (final SerialTimeoutException e) { + Log.e(TAG, "Write timeout occurred", e); + return -1; + } catch (final IOException e) { + Log.e(TAG, "Error writing data", e); + return -1; + } + + return length; + } + + /** + * Writes data asynchronously to the USB serial device. + * + * @param deviceId The device ID. + * @param data The byte array of data to write. + * @param timeoutMSec The timeout in milliseconds. + * @return The number of bytes written, or -1 if failed. + */ + public static int writeAsync(final int deviceId, final byte[] data, final int timeoutMSec) { + final SerialInputOutputManager ioManager = serialIoManagerHashByDeviceId.get(deviceId); + if (ioManager == null) { + Log.w(TAG, "IO Manager not found for device ID " + deviceId); + return -1; + } + + synchronized(ioManager) { + if (ioManager.getReadTimeout() == 0) { + Log.w(TAG, "Read Timeout is 0 for writeAsync"); + return -1; + } + + ioManager.setWriteTimeout(timeoutMSec); + + ioManager.writeAsync(data); + + return data.length; + } + } + + /** + * Reads data from the USB serial device. + * + * @param deviceId The device ID. + * @param length The number of bytes to read. + * @param timeoutMs The timeout in milliseconds. + * @return A byte array containing the read data. + */ + public static byte[] read(final int deviceId, final int length, final int timeoutMs) { + final UsbSerialPort port = findPortByDeviceId(deviceId); + if (port == null) { + return new byte[] {}; + } + + byte[] buffer = new byte[length]; + int bytesRead = 0; + + try { + bytesRead = port.read(buffer, timeoutMs); + } catch (final IOException e) { + Log.e(TAG, "Error reading data", e); + } + + if (bytesRead < length) { + return Arrays.copyOf(buffer, bytesRead); + } + + return buffer; + } + + /** + * Checks if a device name is valid (i.e., exists in the current driver list). + * + * @param name The device name to check. + * @return True if valid, false otherwise. + */ + public static boolean isDeviceNameValid(final String name) { + for (final UsbSerialDriver driver : drivers) { + if (driver.getDevice().getDeviceName().equals(name)) { + return true; + } + } + return false; + } + + /** + * Checks if a device name is currently open. + * + * @param name The device name to check. + * @return True if open, false otherwise. + */ + public static boolean isDeviceNameOpen(final String name) { + for (final UsbSerialDriver driver : drivers) { + final List ports = driver.getPorts(); + if (!ports.isEmpty() && name.equals(driver.getDevice().getDeviceName()) && ports.get(0).isOpen()) { + return true; + } + } + return false; + } + + /** + * Retrieves the device ID for a given device name. + * + * @param deviceName The device name. + * @return The device ID, or BAD_DEVICE_ID if not found. + */ + public static int getDeviceId(final String deviceName) { + final UsbSerialDriver driver = findDriverByDeviceName(deviceName); + if (driver == null) { + Log.w(TAG, "Attempt to get ID of unknown device " + deviceName); + return BAD_DEVICE_ID; + } + + final UsbDevice device = driver.getDevice(); + return device.getDeviceId(); + } + + /** + * Sets the parameters on an open USB serial port. + * + * @param deviceId The device ID. + * @param baudRate The baud rate (e.g., 9600, 115200). + * @param dataBits The number of data bits (5, 6, 7, 8). + * @param stopBits The number of stop bits (1, 2). + * @param parity The parity setting (0: None, 1: Odd, 2: Even). + * @return True if successful, false otherwise. + */ + public static boolean setParameters(final int deviceId, final int baudRate, final int dataBits, final int stopBits, final int parity) { + final UsbSerialPort port = findPortByDeviceId(deviceId); + if (port == null) { + return false; + } + + try { + port.setParameters(baudRate, dataBits, stopBits, parity); + } catch (final IOException | UnsupportedOperationException e) { + Log.e(TAG, "Error setting parameters", e); + return false; + } + + return true; + } + + /** + * Retrieves the carrier detect (CD) flag from the device. + * + * @param deviceId The device ID. + * @return True if CD is active, false otherwise. + */ + public static boolean getCarrierDetect(final int deviceId) { + final UsbSerialPort port = findPortByDeviceId(deviceId); + if (port == null) { + return false; + } + + if (!isControlLineSupported(port, UsbSerialPort.ControlLine.CD)) { + Log.e(TAG, "Getting CD Not Supported"); + return false; + } + + boolean cd; + + try { + cd = port.getCD(); + } catch (final IOException | UnsupportedOperationException e) { + Log.e(TAG, "Error getting CD", e); + return false; + } + + return cd; + } + + /** + * Retrieves the clear to send (CTS) flag from the device. + * + * @param deviceId The device ID. + * @return True if CTS is active, false otherwise. + */ + public static boolean getClearToSend(final int deviceId) { + final UsbSerialPort port = findPortByDeviceId(deviceId); + if (port == null) { + return false; + } + + if (!isControlLineSupported(port, UsbSerialPort.ControlLine.CTS)) { + Log.e(TAG, "Getting CTS Not Supported"); + return false; + } + + boolean cts; + + try { + cts = port.getCTS(); + } catch (final IOException | UnsupportedOperationException e) { + Log.e(TAG, "Error getting CTS", e); + return false; + } + + return cts; + } + + /** + * Retrieves the data set ready (DSR) flag from the device. + * + * @param deviceId The device ID. + * @return True if DSR is active, false otherwise. + */ + public static boolean getDataSetReady(final int deviceId) { + final UsbSerialPort port = findPortByDeviceId(deviceId); + if (port == null) { + return false; + } + + if (!isControlLineSupported(port, UsbSerialPort.ControlLine.DSR)) { + Log.e(TAG, "Getting DSR Not Supported"); + return false; + } + + boolean dsr; + + try { + dsr = port.getDSR(); + } catch (final IOException | UnsupportedOperationException e) { + Log.e(TAG, "Error getting DSR", e); + return false; + } + + return dsr; + } + + /** + * Retrieves the data terminal ready (DTR) flag from the device. + * + * @param deviceId The device ID. + * @return True if DTR is active, false otherwise. + */ + public static boolean getDataTerminalReady(final int deviceId) { + final UsbSerialPort port = findPortByDeviceId(deviceId); + if (port == null) { + return false; + } + + if (!isControlLineSupported(port, UsbSerialPort.ControlLine.DTR)) { + Log.e(TAG, "Getting DTR Not Supported"); + return false; + } + + boolean dtr; + + try { + dtr = port.getDTR(); + } catch (final IOException | UnsupportedOperationException e) { + Log.e(TAG, "Error getting DTR", e); + return false; + } + + return dtr; + } + + /** + * Sets the data terminal ready (DTR) flag on the device. + * + * @param deviceId The device ID. + * @param on True to set DTR, false to clear. + * @return True if successful, false otherwise. + */ + public static boolean setDataTerminalReady(final int deviceId, final boolean on) { + final UsbSerialPort port = findPortByDeviceId(deviceId); + if (port == null) { + return false; + } + + if (!isControlLineSupported(port, UsbSerialPort.ControlLine.DTR)) { + Log.e(TAG, "Setting DTR Not Supported"); + return false; + } + + try { + port.setDTR(on); + } catch (final IOException | UnsupportedOperationException e) { + Log.e(TAG, "Error setting DTR", e); + return false; + } + + return true; + } + + /** + * Retrieves the ring indicator (RI) flag from the device. + * + * @param deviceId The device ID. + * @return True if RI is active, false otherwise. + */ + public static boolean getRingIndicator(final int deviceId) { + final UsbSerialPort port = findPortByDeviceId(deviceId); + if (port == null) { + return false; + } + + if (!isControlLineSupported(port, UsbSerialPort.ControlLine.RI)) { + Log.e(TAG, "Getting RI Not Supported"); + return false; + } + + boolean ri; + + try { + ri = port.getRI(); + } catch (final IOException | UnsupportedOperationException e) { + Log.e(TAG, "Error getting RI", e); + return false; + } + + return ri; + } + + /** + * Retrieves the request to send (RTS) flag from the device. + * + * @param deviceId The device ID. + * @return True if RTS is active, false otherwise. + */ + public static boolean getRequestToSend(final int deviceId) { + final UsbSerialPort port = findPortByDeviceId(deviceId); + if (port == null) { + return false; + } + + if (!isControlLineSupported(port, UsbSerialPort.ControlLine.RTS)) { + Log.e(TAG, "Getting RTS Not Supported"); + return false; + } + + boolean rts; + + try { + rts = port.getRTS(); + } catch (final IOException | UnsupportedOperationException e) { + Log.e(TAG, "Error getting RTS", e); + return false; + } + + return rts; + } + + /** + * Sets the request to send (RTS) flag on the device. + * + * @param deviceId The device ID. + * @param on True to set RTS, false to clear. + * @return True if successful, false otherwise. + */ + public static boolean setRequestToSend(final int deviceId, final boolean on) { + final UsbSerialPort port = findPortByDeviceId(deviceId); + if (port == null) { + return false; + } + + if (!isControlLineSupported(port, UsbSerialPort.ControlLine.RTS)) { + Log.e(TAG, "Setting RTS Not Supported"); + return false; + } + + try { + port.setRTS(on); + } catch (final IOException | UnsupportedOperationException e) { + Log.e(TAG, "Error setting RTS", e); + return false; + } + + return true; + } + + /** + * Retrieves the supported control lines from the device. + * + * @param deviceId The device ID. + * @return An array of control line ordinals. + */ + public static int[] getControlLines(final int deviceId) { + final UsbSerialPort port = findPortByDeviceId(deviceId); + if (port == null) { + return new int[] {}; + } + + EnumSet supportedControlLines; + + try { + supportedControlLines = port.getSupportedControlLines(); + } catch (final IOException e) { + Log.e(TAG, "Error getting supported control lines", e); + return new int[] {}; + } + + if (supportedControlLines.isEmpty()) { + Log.e(TAG, "Control Lines Not Supported"); + return new int[] {}; + } + + EnumSet controlLines; + + try { + controlLines = port.getControlLines(); + } catch (final IOException e) { + Log.e(TAG, "Error getting control lines", e); + return new int[] {}; + } + + int[] lines = new int[controlLines.size()]; + int index = 0; + for (UsbSerialPort.ControlLine line : controlLines) { + lines[index++] = line.ordinal(); + } + + return lines; // controlLines.stream().mapToInt(UsbSerialPort.ControlLine::ordinal).toArray(); + } + + /** + * Retrieves the current flow control setting from the device. + * + * @param deviceId The device ID. + * @return The flow control ordinal, or 0 if not supported. + */ + public static int getFlowControl(final int deviceId) { + final UsbSerialPort port = findPortByDeviceId(deviceId); + if (port == null) { + return 0; + } + + final EnumSet supportedFlowControl = port.getSupportedFlowControl(); + + if (supportedFlowControl.isEmpty()) { + Log.e(TAG, "Flow Control Not Supported"); + return 0; + } + + final UsbSerialPort.FlowControl flowControl = port.getFlowControl(); + + return flowControl.ordinal(); + } + + /** + * Sets the flow control setting on the device. + * + * @param deviceId The device ID. + * @param flowControl The flow control ordinal. + * @return True if successful, false otherwise. + */ + public static boolean setFlowControl(final int deviceId, final int flowControl) { + if (getFlowControl(deviceId) == flowControl) { + return true; + } + + if ((flowControl < 0) || (flowControl >= UsbSerialPort.FlowControl.values().length)) { + Log.w(TAG, "Invalid flow control ordinal " + flowControl); + return false; + } + + final UsbSerialPort.FlowControl flowControlEnum = UsbSerialPort.FlowControl.values()[flowControl]; + + final UsbSerialPort port = findPortByDeviceId(deviceId); + if (port == null) { + return false; + } + + final EnumSet supportedFlowControl = port.getSupportedFlowControl(); + + if (!supportedFlowControl.contains(flowControlEnum)) { + Log.e(TAG, "Setting Flow Control Not Supported"); + return false; + } + + try { + port.setFlowControl(flowControlEnum); + } catch (final IOException | UnsupportedOperationException e) { + Log.e(TAG, "Error setting Flow Control", e); + return false; + } + + return true; + } + + /** + * Sets the break condition on the device. + * + * @param deviceId The device ID. + * @param on True to set break, false to clear break. + * @return True if successful, false otherwise. + */ + public static boolean setBreak(final int deviceId, final boolean on) { + final UsbSerialPort port = findPortByDeviceId(deviceId); + if (port == null) { + return false; + } + + try { + port.setBreak(on); + } catch (final IOException | UnsupportedOperationException e) { + Log.e(TAG, "Error setting break condition", e); + return false; + } + + return true; + } + + /** + * Purges the hardware buffers on the device. + * + * @param deviceId The device ID. + * @param input True to purge the input buffer. + * @param output True to purge the output buffer. + * @return True if successful, false otherwise. + */ + public static boolean purgeBuffers(final int deviceId, final boolean input, final boolean output) { + final UsbSerialPort port = findPortByDeviceId(deviceId); + if (port == null) { + return false; + } + + try { + port.purgeHwBuffers(input, output); + } catch (final IOException | UnsupportedOperationException e) { + Log.e(TAG, "Error purging buffers", e); + return false; + } + + return true; + } + + /** + * Retrieves the native device handle (file descriptor). + * + * @param deviceId The device ID. + * @return The device handle, or -1 if not found. + */ + public static int getDeviceHandle(final int deviceId) { + return fileDescriptorHashByDeviceId.getOrDefault(deviceId, -1); + } + + /** + * Checks if a specific control line is supported by the device. + * + * @param port The UsbSerialPort. + * @param controlLine The control line to check. + * @return True if supported, false otherwise. + */ + private static boolean isControlLineSupported(final UsbSerialPort port, final UsbSerialPort.ControlLine controlLine) { + EnumSet supportedControlLines; + + try { + supportedControlLines = port.getSupportedControlLines(); + } catch (final IOException e) { + Log.e(TAG, "Error getting supported control lines", e); + return false; + } + + return supportedControlLines.contains(controlLine); + } + + /** + * Updates the current list of USB serial drivers by scanning connected devices. + * + * @param deviceId The device ID to update, or -1 to update all. + */ + private static void updateCurrentDrivers() { + final UsbSerialProber usbDefaultProber = UsbSerialProber.getDefaultProber(); + List currentDrivers = usbDefaultProber.findAllDrivers(usbManager); + if (currentDrivers.isEmpty()) { + currentDrivers = QGCProber.getQGCProber().findAllDrivers(usbManager); + } + + if (currentDrivers.isEmpty()) { + return; + } + + removeStaleDrivers(currentDrivers); + addNewDrivers(currentDrivers); + } + + /** + * Removes drivers that are no longer connected. + * + * @param currentDrivers The list of currently connected drivers. + */ + private static void removeStaleDrivers(final List currentDrivers) { + for (int i = drivers.size() - 1; i >= 0; i--) { + boolean found = false; + for (final UsbSerialDriver currentDriver : currentDrivers) { + if (drivers.get(i).getDevice().getDeviceId() == currentDriver.getDevice().getDeviceId()) { + found = true; + break; + } + } + + if (!found) { + Log.i(TAG, "Remove stale driver " + drivers.get(i).getDevice().getDeviceName()); + drivers.remove(i); + } + } + } + + /** + * Adds new drivers that are not already in the driver list. + * + * @param currentDrivers The list of currently connected drivers. + */ + private static void addNewDrivers(final List currentDrivers) { + for (final UsbSerialDriver newDriver : currentDrivers) { + boolean found = false; + for (final UsbSerialDriver driver : drivers) { + if (newDriver.getDevice().getDeviceId() == driver.getDevice().getDeviceId()) { + found = true; + break; + } + } + + if (!found) { + addDriver(newDriver); + } + } + } + + /** + * Adds a new USB serial driver to the driver list and requests permission if needed. + * + * @param newDriver The UsbSerialDriver to add. + */ + private static void addDriver(final UsbSerialDriver newDriver) { + final UsbDevice device = newDriver.getDevice(); + final String deviceName = device.getDeviceName(); + + drivers.add(newDriver); + Log.i(TAG, "Adding new driver " + deviceName); + + if (usbManager.hasPermission(device)) { + Log.i(TAG, "Already have permission to use device " + deviceName); + // Optionally, you can open the driver here if needed + } else { + Log.i(TAG, "Requesting permission to use device " + deviceName); + usbManager.requestPermission(device, usbPermissionIntent); + } + } + + /** + * Cleans up resources by unregistering the BroadcastReceiver. + * Should be called when the manager is no longer needed, typically from QGCActivity.onDestroy(). + */ + public static void cleanup(Context context) { + try { + context.unregisterReceiver(usbReceiver); + Log.i(TAG, "BroadcastReceiver unregistered successfully."); + } catch (final IllegalArgumentException e) { + Log.w(TAG, "Receiver not registered: " + e.getMessage()); + } + } + + /** + * Inner class to handle serial data callbacks. + */ + private static class QGCSerialListener implements SerialInputOutputManager.Listener { + private long classPtr; + + public QGCSerialListener(long classPtr) { + this.classPtr = classPtr; + } + + @Override + public void onRunError(Exception e) { + Log.e(TAG, "Runner stopped.", e); + nativeDeviceException(classPtr, "Runner stopped: " + e.getMessage()); + } + + @Override + public void onNewData(final byte[] data) { + nativeDeviceNewData(classPtr, data); + } + } +} diff --git a/src/Joystick/JoystickAndroid.cc b/src/Joystick/JoystickAndroid.cc index f574be03bf8..0bbf431ea7c 100644 --- a/src/Joystick/JoystickAndroid.cc +++ b/src/Joystick/JoystickAndroid.cc @@ -285,7 +285,7 @@ bool JoystickAndroid::init() { return true; } -static const char kJniClassName[] {"org/mavlink/qgroundcontrol/QGCActivity"}; +static const char kJniClassName[] {"org/mavlink/qgroundcontrol/QGCUsbSerialManager"}; static void jniUpdateAvailableJoysticks(JNIEnv *envA, jobject thizA) {