add AT10 & remove G2 & change gimbal open

2023-10-11 20:20:36 +08:00
parent 5c35cfbe78
commit 7e757d5f23
31 changed files with 1792 additions and 1218 deletions
@@ -15,6 +15,28 @@
 #include <iostream>
 #include <string>
 #include <thread>
+namespace sv
+{
+    std::map<std::string, void *> Gimbal::IOList;
+    std::mutex Gimbal::IOListMutex;
+
+    std::map<GimbalType, std::string> gimbaltypeList =
+        {
+            {GimbalType::G1, "G1"},
+            {GimbalType::Q10f, "Q10f"},
+            {GimbalType::AT10, "AT10"}};
+
+    std::string &cvGimbalType2Str(const GimbalType &type)
+    {
+        std::map<GimbalType, std::string>::iterator temp = gimbaltypeList.find(type);
+        if (temp != gimbaltypeList.end())
+        {
+            return (temp->second);
+        }
+        throw "Error: Unsupported gimbal device type!!!!";
+        exit(-1);
+    }
+}

 /**
 * This function sets the serial port for a Gimbal object.
@@ -99,9 +121,65 @@ void sv::Gimbal::setNetPort(const int &port)
 void sv::Gimbal::setStateCallback(sv::PStateInvoke callback)
 {
    amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
-    pdevTemp->dev->setParserCallback(callback);
+    pdevTemp->setParserCallback(callback);
 }

+/**
+ * The function `creatIO` creates an IO object for a gimbal device and adds it to a list of IO objects
+ * if it doesn't already exist.
+ *
+ * @param dev A pointer to an instance of the `Gimbal` class.
+ * @param IO The parameter "IO" is a pointer to an object that represents the input/output (IO) device
+ * for the gimbal. It is used to establish a connection with the gimbal and perform communication
+ * operations.
+ *
+ * @return a boolean value.
+ */
+void *sv::Gimbal::creatIO(sv::Gimbal *dev)
+{
+    IOListMutex.lock();
+    std::map<std::string, void *>::iterator list = IOList.find(dev->m_serial_port);
+    std::pair<std::string, void *> key("NULL", nullptr);
+
+    if (list == IOList.end())
+    {
+        if (dev->m_gimbal_link == GimbalLink::SERIAL)
+        {
+            UART *ser;
+            ser = new UART(dev->m_serial_port,
+                           (uint32_t)dev->m_serial_baud_rate,
+                           serial::Timeout::simpleTimeout(dev->m_serial_timeout),
+                           (serial::bytesize_t)dev->m_serial_byte_size,
+                           (serial::parity_t)dev->m_serial_parity,
+                           (serial::stopbits_t)dev->m_serial_stopbits,
+                           (serial::flowcontrol_t)dev->m_serial_flowcontrol);
+            key.first = dev->m_serial_port;
+            key.second = (void *)ser;
+            IOList.insert(key);
+        }
+        else if (dev->m_gimbal_link == sv::GimbalLink::ETHERNET_TCP)
+        {
+        }
+        else if (dev->m_gimbal_link == sv::GimbalLink::ETHERNET_UDP)
+        {
+        }
+        else
+        {
+            std::cout << "Error: gimbal IO has opened!!!" << std::endl;
+        }
+    }
+    IOListMutex.unlock();
+
+    return key.second;
+}
+
+void sv::Gimbal::removeIO(sv::Gimbal *dev)
+{
+    IOListMutex.lock();
+    IOList.erase(dev->m_serial_port);
+
+    IOListMutex.unlock();
+}
 /**
 * The function opens a communication interface with a gimbal device and sets up a parser to handle
 * incoming data.
@@ -115,53 +193,22 @@ void sv::Gimbal::setStateCallback(sv::PStateInvoke callback)
 */
 bool sv::Gimbal::open(PStateInvoke callback)
 {
-    if (this->m_gimbal_link == GimbalLink::SERIAL)
-    {
-        this->IO = new UART(this->m_serial_port,
-                            (uint32_t)this->m_serial_baud_rate,
-                            serial::Timeout::simpleTimeout(this->m_serial_timeout),
-                            (serial::bytesize_t)this->m_serial_byte_size,
-                            (serial::parity_t)this->m_serial_parity,
-                            (serial::stopbits_t)this->m_serial_stopbits,
-                            (serial::flowcontrol_t)this->m_serial_flowcontrol);
-    }
-    // Subsequent additions
-    else if (this->m_gimbal_link == sv::GimbalLink::ETHERNET_TCP)
-    {
-        return false;
-    }
-    else if (this->m_gimbal_link == sv::GimbalLink::ETHERNET_UDP)
-    {
-        return false;
-    }
-    else
-    {
-        throw "Error: Unsupported communication interface class!!!";
-        return false;
-    }
-    std::string driverName;
-    switch (this->m_gimbal_type)
-    {
-    case sv::GimbalType::G1:
-        driverName = "G1";
-        break;
-    case sv::GimbalType::Q10f:
-        driverName = "Q10f";
-        break;
+    bool ret = false;

-    default:
-        throw "Error: Unsupported driver!!!";
-        return false;
-        break;
+    this->IO = creatIO(this);
+    if (this->IO != nullptr)
+    {
+        std::string driverName;
+        driverName = sv::cvGimbalType2Str(this->m_gimbal_type);
+        this->dev = new amovGimbal::gimbal(driverName, (amovGimbal::IOStreamBase *)this->IO);
+        amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
+        pdevTemp->startStack();
+        pdevTemp->parserAuto(callback);
+
+        ret = true;
    }
-    this->dev = new amovGimbal::gimbal(driverName, (amovGimbal::IOStreamBase *)this->IO);

-    amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
-
-    pdevTemp->dev->startStack();
-    pdevTemp->dev->parserAuto(callback);
-
-    return true;
+    return ret;
 }

 /**
@@ -333,7 +380,7 @@ int sv::Gimbal::getVideoState()
    amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
    int ret;
    amovGimbal::AMOV_GIMBAL_STATE_T state;
-    state = pdevTemp->dev->getGimabalState();
+    state = pdevTemp->getGimabalState();
    if (state.video == amovGimbal::AMOV_GIMBAL_VIDEO_TAKE)
    {
        ret = 1;
@@ -406,6 +453,7 @@ void sv::Gimbal::setAngleRateEuler(double roll_rate, double pitch_rate, double y

 sv::Gimbal::~Gimbal()
 {
+    removeIO(this);
    delete (amovGimbal::gimbal *)this->dev;
    delete (amovGimbal::IOStreamBase *)this->IO;
 }