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.
62 lines
1.3 KiB
62 lines
1.3 KiB
5 years ago
|
#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));
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|