#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));
    }

}