topgun x30T write pos by usart

ArduCopter/APM_Config.h

@@ -64,7 +64,7 @@
 // Put your variable definitions into the UserVariables.h file (or another file name and then change the #define below).
 //#define USERHOOK_VARIABLES "UserVariables.h"
 // Put your custom code into the UserCode.cpp with function names matching those listed below and ensure the appropriate #define below is uncommented below
-//#define USERHOOK_INIT userhook_init();                      // for code to be run once at startup
+#define USERHOOK_INIT userhook_init();                      // for code to be run once at startup
 //#define USERHOOK_FASTLOOP userhook_FastLoop();            // for code to be run at 100hz
 //#define USERHOOK_50HZLOOP userhook_50Hz();                  // for code to be run at 50hz
 #define USERHOOK_MEDIUMLOOP userhook_MediumLoop();        // for code to be run at 10hz

ArduCopter/Copter.h

@@ -26,7 +26,7 @@
 #include <stdarg.h>
 
 #include <AP_HAL/AP_HAL.h>
-
+#include "AP_Camera/AP_Camera_Serial_X30T.h"
 // Common dependencies
 #include <AP_Common/AP_Common.h>
 #include <AP_Common/Location.h>
@@ -484,7 +484,7 @@ private:
     AC_PosControl *pos_control;
     AC_WPNav *wp_nav;
     AC_Loiter *loiter_nav;
-
+     AP_Camera_Serial_X30T  *x30t;
 #if MODE_CIRCLE_ENABLED == ENABLED
     AC_Circle *circle_nav;
 #endif
@@ -539,7 +539,7 @@ private:
 
     // Landing Gear Controller
     AP_LandingGear landinggear;
-
+ 
     // terrain handling
 #if AP_TERRAIN_AVAILABLE && AC_TERRAIN && MODE_AUTO_ENABLED == ENABLED
     AP_Terrain terrain{mode_auto.mission};

ArduCopter/UserCode.cpp

@@ -1,10 +1,30 @@
 #include "Copter.h"
 
+
+
+
 #ifdef USERHOOK_INIT
 void Copter::userhook_init()
 {
     // put your initialisation code here
     // this will be called once at start-up
+    if (x30t != nullptr)
+    {
+        x30t->init();
+    }
+    else
+    {
+        if (x30t == nullptr)
+        {
+            gcs().send_text(MAV_SEVERITY_INFO,"x30t is null");
+            x30t = new AP_Camera_Serial_X30T();
+            if (x30t == nullptr || !x30t->init())
+            {
+                delete x30t;
+                x30t = nullptr;
+            }
+        }
+    }
 }
 #endif
 
@@ -26,6 +46,39 @@ void Copter::userhook_50Hz()
 void Copter::userhook_MediumLoop()
 {
     // put your 10Hz code here
+  if (x30t != nullptr)
+    {
+        int32_t lat = AP::gps().location().lat;
+        int32_t lng = AP::gps().location().lat;
+        int32_t alt = AP::gps().location().alt;
+        
+        uint8_t posdata[12];
+        posdata[0] = lat & 0xff;
+        posdata[1] = (lat >> 8) & 0xff;
+        posdata[2] = (lat >> 16) & 0xff;
+        posdata[3] = (lat >> 24) & 0xff;
+
+        posdata[4] = lng & 0xff;
+        posdata[5] = (lng >> 8) & 0xff;
+        posdata[6] = (lng >> 16) & 0xff;
+        posdata[7] = (lng >> 24) & 0xff;
+         
+        posdata[8] = alt& 0xff;
+        posdata[9] = (alt>> 8) & 0xff;
+        posdata[10] =(alt >> 16) & 0xff;
+        posdata[11] =(alt >> 24) & 0xff;
+       
+        x30t->sendPos(&posdata[0],X30T_MSG_TYPE_POS);
+
+
+       
+        
+  
+        
+
+    }
+
+   
 }
 #endif
 

libraries/AP_Camera/AP_Camera_Serial_X30T.cpp

@@ -0,0 +1,61 @@
+#include "AP_Camera_Serial_X30T.h"
+
+#define   PREAMBLE1  0x7A
+#define   PREAMBLE2  0x71
+
+
+
+
+// bool AP_Camera_Serial_X30T::detect()
+// {
+//      AP_HAL::UARTDriver *uart = nullptr;
+//     uart = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Camera_X30T, 0);
+//     return uart != nullptr;
+// }
+
+AP_Camera_Serial_X30T::AP_Camera_Serial_X30T(void )
+{
+
+}
+
+bool  AP_Camera_Serial_X30T::init(){
+  const AP_SerialManager &serial_manager = AP::serialmanager();
+  if ((uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Camera_X30T, 0))) {
+      _protocol = AP_SerialManager::SerialProtocol_Camera_X30T;
+  }
+
+  if(uart!=nullptr)
+  {
+    return true;
+  }
+return false;
+}
+
+void AP_Camera_Serial_X30T::sendPos(uint8_t *data,uint8_t type) 
+{
+      if (uart == nullptr) {
+        return ;
+    }
+  uint16_t crcsum =0;
+   uint8_t senddata[18];
+   senddata[0]= PREAMBLE1;
+   senddata[1]= PREAMBLE2;
+   senddata[2] = 12;
+   senddata[3] =  type;
+   crcsum =senddata[2]+senddata[3];
+   
+   for (int i = 0; i < 12; i++)
+   {
+       senddata[4+i] = data[i];
+       crcsum += data[i];
+   }
+   
+   senddata[16]  = crcsum&0xff;
+   senddata[17] = (crcsum&0xff00)>>8;
+     if (uart->txspace() > sizeof(senddata))
+    {
+        uart->write(&senddata[0], sizeof(senddata));
+    }
+
+}
+

libraries/AP_Camera/AP_Camera_Serial_X30T.h

@@ -0,0 +1,32 @@
+#pragma once
+#include <AP_HAL/AP_HAL.h>
+#include <AP_SerialManager/AP_SerialManager.h>
+#include <AP_Common/AP_Common.h>
+
+#define X30T_MSG_TYPE_POS    1
+class AP_Camera_Serial_X30T
+{
+
+public:
+    AP_Camera_Serial_X30T();
+      /* Do not allow copies */
+    AP_Camera_Serial_X30T(const AP_Camera_Serial_X30T &other) = delete;
+    AP_Camera_Serial_X30T &operator=(const AP_Camera_Serial_X30T&) = delete;
+
+  // singleton support
+    static AP_SerialManager *get_singleton(void) {
+        return &singleton;
+    }
+
+    void sendPos(uint8_t *data,uint8_t type) ;
+
+    bool init(void);
+    //bool detect(void);
+private:
+
+     AP_HAL::UARTDriver *uart= nullptr;                
+     AP_SerialManager::SerialProtocol _protocol;
+
+    static AP_SerialManager singleton;
+
+};

libraries/AP_SerialManager/AP_SerialManager.h

@@ -123,6 +123,7 @@ public:
         SerialProtocol_WindVane = 21,
         SerialProtocol_SLCAN = 22,
         SerialProtocol_RCIN = 23,
+        SerialProtocol_Camera_X30T = 24,
     };
 
     // get singleton instance