78 lines
1.6 KiB
C++
78 lines
1.6 KiB
C++
#include <Arduino.h>
|
|
#include "SG112A.h"
|
|
|
|
SG112A::SG112A(void) {
|
|
Serial.begin(9600);
|
|
Serial.setTimeout(READ_TIMEOUT);
|
|
delay(5000);
|
|
write(CMD_GET_PPM);
|
|
read();
|
|
}
|
|
|
|
void SG112A::getSensorData(lora_data &loradata) {
|
|
write(CMD_GET_PPM);
|
|
delay(50);
|
|
uint8_t byteLen = read();
|
|
|
|
if (byteLen > 0) {
|
|
switch(buffer[3]) {
|
|
case 0x15:
|
|
loradata.ppm = (buffer[5]*256) + buffer[4];
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
void SG112A::write(byte cmd) {
|
|
uint8_t _cmd[6] = {0xAA, 0x55, cmd, 0x00, 0x00, 0x00};
|
|
uint16_t crc = crc16(_cmd, 4);
|
|
_cmd[4] = (uint8_t)(crc & 0xFF);
|
|
_cmd[5] = (uint8_t)(crc >> 8);
|
|
while (Serial.available() > 0) Serial.read();
|
|
Serial.write(_cmd, 6);
|
|
Serial.flush();
|
|
}
|
|
|
|
uint8_t SG112A::read() {
|
|
uint8_t ret = 0;
|
|
zeroBuffer();
|
|
if (Serial.available() > 0) {
|
|
ret = Serial.readBytes(buffer, SER_BUF_LEN);
|
|
}
|
|
|
|
if (buffer[0] != 0xBB && buffer[1] != 0x66)
|
|
ret = 0;
|
|
|
|
// TODO: Do CRC Check Here
|
|
|
|
return ret;
|
|
}
|
|
|
|
void SG112A::zeroBuffer() {
|
|
for (int i=0; i < SER_BUF_LEN; i++)
|
|
buffer[i] = 0x00;
|
|
}
|
|
|
|
uint16_t SG112A::crc16(uint8_t *cmd, int len){
|
|
uint16_t ret = 0xffff;
|
|
uint16_t polynomial = 0xa001;
|
|
int shift = 0x0;
|
|
int i = 0;
|
|
for (i = len - 1; i >= 0 ; i-- ){
|
|
|
|
uint16_t code = ( uint16_t )( cmd [ len -1 - i ] & 0xff );
|
|
ret = ret^code;
|
|
shift = 0x0;
|
|
while ( shift <= 7 ){
|
|
if ( ret & 0x1 ) {
|
|
ret = ret >> 1;
|
|
ret = ret^polynomial ;
|
|
} else {
|
|
ret = ret >> 1;
|
|
}
|
|
shift++;
|
|
}
|
|
}
|
|
return ret;
|
|
}
|