You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
61 lines
1.3 KiB
61 lines
1.3 KiB
#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)); |
|
} |
|
|
|
} |
|
|
|
|