diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 1c87164bfc..03f00e7fc4 100644 --- a/ArduCopter/APM_Config.h +++ b/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 diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 76371dba5f..478ffcce35 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -26,7 +26,7 @@ #include #include - +#include "AP_Camera/AP_Camera_Serial_X30T.h" // Common dependencies #include #include @@ -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}; diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index d84818aa07..48a87585b4 100644 --- a/ArduCopter/UserCode.cpp +++ b/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 diff --git a/libraries/AP_Camera/AP_Camera_Serial_X30T.cpp b/libraries/AP_Camera/AP_Camera_Serial_X30T.cpp new file mode 100644 index 0000000000..059c937212 --- /dev/null +++ b/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)); + } + +} + diff --git a/libraries/AP_Camera/AP_Camera_Serial_X30T.h b/libraries/AP_Camera/AP_Camera_Serial_X30T.h new file mode 100644 index 0000000000..560b45d7ef --- /dev/null +++ b/libraries/AP_Camera/AP_Camera_Serial_X30T.h @@ -0,0 +1,32 @@ +#pragma once +#include +#include +#include + +#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; + +}; diff --git a/libraries/AP_SerialManager/AP_SerialManager.h b/libraries/AP_SerialManager/AP_SerialManager.h index f0517bf167..3798dead77 100644 --- a/libraries/AP_SerialManager/AP_SerialManager.h +++ b/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