182 lines
4.6 KiB
C++
182 lines
4.6 KiB
C++
#include "effects/tpm2_net.h"
|
|
#include "my_fastled.h"
|
|
|
|
void Tpm2NetEffect::loop(uint16_t ms) {
|
|
int packetsize = _udp.parsePacket();
|
|
uint8_t data[255];
|
|
uint8_t type = 0x00;
|
|
if (packetsize > 0) {
|
|
_last_packet_at = millis();
|
|
DBG("TPM2.Net * Received %d bytes from %s", packetsize, _udp.remoteIP().toString().c_str());
|
|
if (packetsize < 7) {
|
|
LOGln("TPM2.Net * Packet is too short. Ignoring it.");
|
|
return;
|
|
}
|
|
_udp.read(data, 6);
|
|
if (data[0] != 0x9C) {
|
|
LOGln("TPM2.Net * Block start byte is 0x%02X, expected 0x9C", data[0]);
|
|
return;
|
|
}
|
|
if (data[1] == 0xC0) {
|
|
DBG("TPM2.Net * Received a command packet.");
|
|
type = 0xC0;
|
|
} else if (data[1] == 0xDA) {
|
|
DBG("TPM2.Net * Received a data packet.");
|
|
type = 0xDA;
|
|
} else {
|
|
LOGln("TPM2.Net * Unexpected packet type 0x%02X, expected 0xC0 or 0xDA.", data[1]);
|
|
}
|
|
uint16_t framesize = (data[2]<<8) | data[3];
|
|
uint8_t packet_number = data[4];
|
|
//uint8_t packet_count = data[5];
|
|
|
|
if (packetsize != framesize + 7) {
|
|
LOGln("TPM2.Net * Invalid packet size. Expected %d bytes, was %d bytes.", framesize+7, packetsize);
|
|
return;
|
|
}
|
|
|
|
if (type==0xC0) {
|
|
_parse_command(framesize, packet_number);
|
|
} else if (type==0xDA) {
|
|
_parse_data(framesize, packet_number);
|
|
}
|
|
} else if (_last_packet_at + 5000 < millis()) {
|
|
window->fill_with_checkerboard();
|
|
}
|
|
}
|
|
|
|
void Tpm2NetEffect::_parse_command(uint16_t size, uint8_t packet_number) {
|
|
if (packet_number!=0) {
|
|
LOGln("TPM2.Net * Command packet with packet_number > 0 (%d). Ignoring it.", packet_number);
|
|
return;
|
|
}
|
|
if (size < 2) {
|
|
LOGln("TPM2.Net * Command packet need at least 2 data bytes.");
|
|
return;
|
|
}
|
|
uint8_t ctrl = _udp.read();
|
|
bool write = ctrl & 0x80;
|
|
bool respond = ctrl & 0x40;
|
|
uint8_t cmd = _udp.read();
|
|
uint8_t data = 0;
|
|
if (write) {
|
|
if (size<3) {
|
|
LOGln("TPM2.Net * Got a write command, but no data to write.");
|
|
return;
|
|
}
|
|
data = _udp.read();
|
|
}
|
|
|
|
if (cmd == 0x0A) { // Master Brightness
|
|
if (write) {
|
|
FastLED.setBrightness(data);
|
|
} else {
|
|
uint8_t data[1] = {FastLED.getBrightness()};
|
|
_respond_with_data(data, 1);
|
|
}
|
|
} else if (cmd == 0x10 && !write) {
|
|
uint16_t pixels = window->width * window->height;
|
|
uint8_t data[2] = {(uint8_t)(pixels >> 8), (uint8_t)(pixels & 0xFF)};
|
|
_respond_with_data(data, 2);
|
|
} else {
|
|
LOGln("TPM2.Net * Unknown command. write:%d, command:0x%02X", write, cmd);
|
|
if (respond) {
|
|
_respond_unknown_command();
|
|
}
|
|
}
|
|
}
|
|
|
|
void Tpm2NetEffect::_parse_data(uint16_t framesize, uint8_t packet_number) {
|
|
if (packet_number==0) {
|
|
_pixel_index=0;
|
|
}
|
|
uint8_t len;
|
|
uint8_t data[3];
|
|
CRGB color;
|
|
while ((len = _udp.read(data, 3))==3) {
|
|
// We got 3 bytes
|
|
color.setRGB(data[0], data[1], data[2]);
|
|
window->setPixelByIndex(_pixel_index, &color);
|
|
_pixel_index++;
|
|
}
|
|
|
|
/*
|
|
bool dir_changed = false;
|
|
|
|
_x += _x_dir * settings.effects.dvd.speed;
|
|
_y += _y_dir * settings.effects.dvd.speed;
|
|
|
|
if (_x <= 0) {
|
|
_x = -_x;
|
|
_x_dir = -_x_dir;
|
|
dir_changed = true;
|
|
//LOGln("speed: %d", settings.effects.dvd.speed);
|
|
} else if (_x + (settings.effects.dvd.width << 8) >= (window->width << 8)) {
|
|
_x -= 2*settings.effects.dvd.speed;
|
|
_x_dir = -_x_dir;
|
|
dir_changed = true;
|
|
}
|
|
|
|
if (_y <= 0) {
|
|
_y = -_y;
|
|
_y_dir = -_y_dir;
|
|
dir_changed = true;
|
|
} else if (_y + (settings.effects.dvd.height << 8) >= (window->height << 8)) {
|
|
_y -= 2*settings.effects.dvd.speed;
|
|
_y_dir = -_y_dir;
|
|
dir_changed = true;
|
|
}
|
|
|
|
window->clear();
|
|
|
|
if (dir_changed) _color = (CRGB)CHSV(random8(), 255, 255);
|
|
|
|
for (int x=0; x<settings.effects.dvd.width; x++) for (int y=0; y<settings.effects.dvd.height; y++) {
|
|
window->setSubPixel(_x + (x<<8), _y + (y<<8), (CRGB*)&_color);
|
|
}
|
|
for (int x=1; x<settings.effects.dvd.width; x++) for (int y=1; y<settings.effects.dvd.height; y++) {
|
|
window->setPixel((_x>>8) + x, (_y>>8) + y, (CRGB*)&_color);
|
|
}
|
|
*/
|
|
}
|
|
|
|
void Tpm2NetEffect::_respond(uint8_t* data, uint8_t len) {
|
|
_udp.beginPacket(_udp.remoteIP(), 65442);
|
|
_udp.write(data, len);
|
|
_udp.endPacket();
|
|
}
|
|
|
|
void Tpm2NetEffect::_respond_ack() {
|
|
uint8_t data[8] = {0x9C, 0xAC, 0x00, 0x01, 0x00, 0x01, 0x00, 0x36};
|
|
_respond(data, 8);
|
|
}
|
|
|
|
void Tpm2NetEffect::_respond_unknown_command() {
|
|
uint8_t data[8] = {0x9C, 0xAC, 0x00, 0x01, 0x00, 0x01, 0x02, 0x36};
|
|
_respond(data, 8);
|
|
}
|
|
|
|
void Tpm2NetEffect::_respond_with_data(uint8_t* dat, uint8_t len) {
|
|
uint8_t data[8 + len];
|
|
data[0] = 0x9C;
|
|
data[1] = 0xAD;
|
|
data[2] = (len+1)>>8;
|
|
data[3] = (len+1)&0xFF;
|
|
data[4] = 0x00;
|
|
data[5] = 0x01;
|
|
data[6] = 0x00;
|
|
memcpy(&(data[7]), dat, len);
|
|
data[8 + len - 1] = 0x36;
|
|
_respond(data, 8 + len);
|
|
}
|
|
|
|
bool Tpm2NetEffect::can_be_shown_with_clock() { return false; }
|
|
|
|
Tpm2NetEffect::Tpm2NetEffect() {
|
|
_udp.begin(65506);
|
|
}
|
|
|
|
Tpm2NetEffect::~Tpm2NetEffect() {
|
|
_udp.stop();
|
|
}
|