visus
/
teensy-cores
Observar 1
Favorito 0
Fork 0
Código Issues 0 Pull requests 0 Versões 0 Wiki Atividade
Ver código fonte

USB Flightsim for Teensy 4 

Needs accompanying changes in Arduino's boards.txt, enabling corresponding menu entries
main
Jorg Neves Bliesener 4 anos atrás
pai
19537714c2
commit
e0945a6719
6 arquivos alterados com 1187 adições e 18 exclusões
Visão dividida Mostrar estatísticas do Diff
  1. +2
    -0
      keywords.txt
  2. +1
    -1
      teensy4/WProgram.h
  3. +7
    -0
      teensy4/usb.c
  4. +12
    -17
      teensy4/usb_desc.h
  5. +841
    -0
      teensy4/usb_flightsim.cpp
  6. +324
    -0
      teensy4/usb_flightsim.h

+ 2
- 0
keywords.txt Ver arquivo

@@ -507,6 +507,8 @@ FlightSimCommand KEYWORD2
FlightSimInteger KEYWORD2
FlightSimFloat KEYWORD2
FlightSimElapsedFrames KEYWORD2
FlightSimData KEYWORD2
FlightSimEvent KEYWORD2
onChange KEYWORD2
update KEYWORD2
isEnabled KEYWORD2

+ 1
- 1
teensy4/WProgram.h Ver arquivo

@@ -55,7 +55,7 @@
#include "usb_joystick.h"
#include "usb_midi.h"
#include "usb_rawhid.h"
//#include "usb_flightsim.h"
#include "usb_flightsim.h"
//#include "usb_mtp.h"
//#include "usb_audio.h"
#include "usb_touch.h"

+ 7
- 0
teensy4/usb.c Ver arquivo

@@ -6,6 +6,7 @@
#include "usb_keyboard.h"
#include "usb_mouse.h"
#include "usb_joystick.h"
#include "usb_flightsim.h"
#include "usb_touch.h"
#include "usb_midi.h"
#include "core_pins.h" // for delay()
@@ -314,6 +315,9 @@ static void isr(void)
#ifdef MULTITOUCH_INTERFACE
usb_touchscreen_update_callback();
#endif
#ifdef FLIGHTSIM_INTERFACE
usb_flightsim_flush_output();
#endif
}
}

@@ -405,6 +409,9 @@ static void endpoint0_setup(uint64_t setupdata)
#if defined(MOUSE_INTERFACE)
usb_mouse_configure();
#endif
#if defined(FLIGHTSIM_INTERFACE)
usb_flightsim_configure();
#endif
#if defined(JOYSTICK_INTERFACE)
usb_joystick_configure();
#endif

+ 12
- 17
teensy4/usb_desc.h Ver arquivo

@@ -552,26 +552,24 @@ let me know? http://forum.pjrc.com/forums/4-Suggestions-amp-Bug-Reports
#define PRODUCT_NAME {'T','e','e','n','s','y',' ','F','l','i','g','h','t',' ','S','i','m',' ','C','o','n','t','r','o','l','s'}
#define PRODUCT_NAME_LEN 26
#define EP0_SIZE 64
#define NUM_ENDPOINTS 4
#define NUM_ENDPOINTS 3
#define NUM_INTERFACE 2
#define FLIGHTSIM_INTERFACE 0 // Flight Sim Control
#define FLIGHTSIM_TX_ENDPOINT 3
#define FLIGHTSIM_TX_SIZE 64
#define FLIGHTSIM_TX_INTERVAL 1
#define FLIGHTSIM_RX_ENDPOINT 4
#define FLIGHTSIM_RX_ENDPOINT 3
#define FLIGHTSIM_RX_SIZE 64
#define FLIGHTSIM_RX_INTERVAL 1
#define SEREMU_INTERFACE 1 // Serial emulation
#define SEREMU_TX_ENDPOINT 1
#define SEREMU_TX_ENDPOINT 2
#define SEREMU_TX_SIZE 64
#define SEREMU_TX_INTERVAL 1
#define SEREMU_RX_ENDPOINT 2
#define SEREMU_RX_SIZE 32
#define SEREMU_RX_INTERVAL 2
#define ENDPOINT1_CONFIG ENDPOINT_TRANSMIT_ONLY
#define ENDPOINT2_CONFIG ENDPOINT_RECEIVE_ONLY
#define ENDPOINT3_CONFIG ENDPOINT_TRANSMIT_ONLY
#define ENDPOINT4_CONFIG ENDPOINT_RECEIVE_ONLY
#define ENDPOINT2_CONFIG ENDPOINT_RECEIVE_INTERRUPT + ENDPOINT_TRANSMIT_INTERRUPT
#define ENDPOINT3_CONFIG ENDPOINT_RECEIVE_BULK + ENDPOINT_TRANSMIT_BULK

#elif defined(USB_FLIGHTSIM_JOYSTICK)
#define VENDOR_ID 0x16C0
@@ -582,32 +580,29 @@ let me know? http://forum.pjrc.com/forums/4-Suggestions-amp-Bug-Reports
#define PRODUCT_NAME {'T','e','e','n','s','y',' ','F','l','i','g','h','t',' ','S','i','m',' ','C','o','n','t','r','o','l','s'}
#define PRODUCT_NAME_LEN 26
#define EP0_SIZE 64
#define NUM_ENDPOINTS 5
#define NUM_ENDPOINTS 4
#define NUM_INTERFACE 3
#define FLIGHTSIM_INTERFACE 0 // Flight Sim Control
#define FLIGHTSIM_TX_ENDPOINT 3
#define FLIGHTSIM_TX_SIZE 64
#define FLIGHTSIM_TX_INTERVAL 1
#define FLIGHTSIM_RX_ENDPOINT 4
#define FLIGHTSIM_RX_ENDPOINT 3
#define FLIGHTSIM_RX_SIZE 64
#define FLIGHTSIM_RX_INTERVAL 1
#define SEREMU_INTERFACE 1 // Serial emulation
#define SEREMU_TX_ENDPOINT 1
#define SEREMU_TX_ENDPOINT 2
#define SEREMU_TX_SIZE 64
#define SEREMU_TX_INTERVAL 1
#define SEREMU_RX_ENDPOINT 2
#define SEREMU_RX_SIZE 32
#define SEREMU_RX_INTERVAL 2
#define JOYSTICK_INTERFACE 2 // Joystick
#define JOYSTICK_ENDPOINT 5
#define JOYSTICK_ENDPOINT 4
#define JOYSTICK_SIZE 12 // 12 = normal, 64 = extreme joystick
#define JOYSTICK_INTERVAL 1
#define ENDPOINT1_CONFIG ENDPOINT_TRANSMIT_ONLY
#define ENDPOINT2_CONFIG ENDPOINT_RECEIVE_ONLY
#define ENDPOINT3_CONFIG ENDPOINT_TRANSMIT_ONLY
#define ENDPOINT4_CONFIG ENDPOINT_RECEIVE_ONLY
#define ENDPOINT5_CONFIG ENDPOINT_TRANSMIT_ONLY

#define ENDPOINT2_CONFIG ENDPOINT_RECEIVE_INTERRUPT + ENDPOINT_TRANSMIT_INTERRUPT
#define ENDPOINT3_CONFIG ENDPOINT_RECEIVE_BULK + ENDPOINT_TRANSMIT_BULK
#define ENDPOINT4_CONFIG ENDPOINT_RECEIVE_UNUSED + ENDPOINT_TRANSMIT_INTERRUPT

#elif defined(USB_MTPDISK)
#define VENDOR_ID 0x16C0

+ 841
- 0
teensy4/usb_flightsim.cpp Ver arquivo

@@ -0,0 +1,841 @@
/* Teensyduino Core Library
* http://www.pjrc.com/teensy/
* Copyright (c) 2013 PJRC.COM, LLC.
*
* Permission is hereby granted, free of charge, to any person obtaining
* a copy of this software and associated documentation files (the
* "Software"), to deal in the Software without restriction, including
* without limitation the rights to use, copy, modify, merge, publish,
* distribute, sublicense, and/or sell copies of the Software, and to
* permit persons to whom the Software is furnished to do so, subject to
* the following conditions:
*
* 1. The above copyright notice and this permission notice shall be
* included in all copies or substantial portions of the Software.
*
* 2. If the Software is incorporated into a build system that allows
* selection among a list of target devices, then similar target
* devices manufactured by PJRC.COM must be included in the list of
* target devices and selectable in the same manner.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/

extern "C" {
#include "usb_dev.h"
}
#include "usb_flightsim.h"
#include "debug/printf.h"
#include "avr/pgmspace.h"
#include "core_pins.h" // for yield(), millis()
#include <string.h> // for memcpy()

#ifdef FLIGHTSIM_INTERFACE // defined by usb_dev.h -> usb_desc.h
#if F_CPU >= 20000000

FlightSimCommand * FlightSimCommand::first = NULL;
FlightSimCommand * FlightSimCommand::last = NULL;
FlightSimInteger * FlightSimInteger::first = NULL;
FlightSimInteger * FlightSimInteger::last = NULL;
FlightSimFloat * FlightSimFloat::first = NULL;
FlightSimFloat * FlightSimFloat::last = NULL;
/// JB
FlightSimEvent * FlightSimEvent::first = NULL;
FlightSimEvent * FlightSimEvent::last = NULL;
FlightSimData * FlightSimData::first = NULL;
FlightSimData * FlightSimData::last = NULL;
/// JB End

uint8_t FlightSimClass::enabled = 0;
uint8_t FlightSimClass::request_id_messages = 0;
unsigned long FlightSimClass::frameCount = 0;
elapsedMillis FlightSimClass::enableTimeout;

static unsigned int unassigned_id = 1; // TODO: move into FlightSimClass

static uint8_t tx_noautoflush=0;
static uint8_t transmit_previous_timeout=0;

#define TX_NUM 8
static transfer_t tx_transfer[TX_NUM] __attribute__ ((used, aligned(32)));
DMAMEM static uint8_t txbuffer[FLIGHTSIM_TX_SIZE * TX_NUM] __attribute__ ((aligned(32)));
static uint8_t tx_head=0;
static uint16_t tx_available=0;

#define RX_NUM 6
static transfer_t rx_transfer[RX_NUM] __attribute__ ((used, aligned(32)));
DMAMEM static uint8_t rx_buffer[RX_NUM * FLIGHTSIM_RX_SIZE] __attribute__ ((aligned(32)));
static volatile uint8_t rx_head;
static volatile uint8_t rx_tail;
static uint8_t rx_list[RX_NUM + 1];
static volatile uint32_t rx_available;

extern "C" {
static void rx_queue_transfer(int i);
static void rx_event(transfer_t *t);
static void* usb_flightsim_get_packet();
static void usb_flightsim_free_packet();
static bool wait_for_tx_buf(transfer_t *xfer);
static void send_packet(transfer_t *xfer);
}

// When the PC isn't listening, how long do we wait before discarding data?
#define TX_TIMEOUT_MSEC 40

extern volatile uint8_t usb_configuration;

FlightSimCommand::FlightSimCommand()
{
id = unassigned_id++;
if (!first) {
first = this;
} else {
last->next = this;
}
last = this;
name = NULL;
next = NULL;
FlightSimClass::request_id_messages = 1;
}

void FlightSimCommand::identify(void)
{
uint8_t len, buf[6];

if (!FlightSim.enabled || !name) return;
len = strlen((const char *)name);
buf[0] = len + 6;
buf[1] = 1;
buf[2] = id;
buf[3] = id >> 8;
buf[4] = 0;
buf[5] = 0;
FlightSimClass::xmit(buf, 6, name, len);
}

void FlightSimCommand::sendcmd(uint8_t n)
{
uint8_t buf[4];

if (!FlightSim.enabled || !name) return;
buf[0] = 4;
buf[1] = n;
buf[2] = id;
buf[3] = id >> 8;
FlightSimClass::xmit(buf, 4, NULL, 0);
}

/// JB
FlightSimEvent::FlightSimEvent()
{
id = unassigned_id++;
if (!first) {
first = this;
} else {
last->next = this;
}
last = this;
name = NULL;
next = NULL;
occur_callback = NULL;
occurredFlag = 0;
callbackInfo = NULL;
hasCallbackInfo = 0;
value = 0;
FlightSimClass::request_id_messages = 1;
}

void FlightSimEvent::identify(void)
{
uint8_t len, buf[6];

if (!FlightSim.enabled || !name) return;
len = strlen((const char *)name);
buf[0] = len + 6;
buf[1] = 1;
buf[2] = id;
buf[3] = id >> 8;
buf[4] = 3;
buf[5] = 0;
FlightSimClass::xmit(buf, 6, name, len);
}

void FlightSimEvent::send(unsigned int data, unsigned int flags)
{
uint8_t buf[4];
uint32_t txData[2];

if (!FlightSim.enabled || !name) return;
buf[0] = 12;
buf[1] = 7;
buf[2] = id;
buf[3] = id >> 8;
value = data;
txData[0] = data;
txData[1] = flags;
FlightSimClass::xmit(buf, 4, (uint8_t *)&txData, 8);
}

void FlightSimEvent::update(long val)
{
value = (unsigned int) val;
occurredFlag = true;
if (occur_callback) {
if (!hasCallbackInfo) {
(*occur_callback)(val);
} else {
(*(void(*)(long,void*))occur_callback)(val,callbackInfo);
}
}
}

FlightSimEvent * FlightSimEvent::find(unsigned int n)
{
for (FlightSimEvent *p = first; p; p = p->next) {
if (p->id == n) return p;
}
return NULL;
}


FlightSimData::FlightSimData()
{
id = unassigned_id++;
if (!first) {
first = this;
} else {
last->next = this;
}
last = this;
name = NULL;
next = NULL;
valueLen = 0;
hasCallbackInfo = 0;
callbackWithObject = 0;
callbackInfo = NULL;
change_callback = NULL;
FlightSimClass::request_id_messages = 1;
}

void FlightSimData::identify(void)
{
uint8_t len, buf[6];

if (!FlightSim.enabled || !name) return;
len = strlen((const char *)name);
buf[0] = len + 6;
buf[1] = 1;
buf[2] = id;
buf[3] = id >> 8;
buf[4] = 4;
buf[5] = 0;
FlightSimClass::xmit(buf, 6, name, len);
}

void FlightSimData::update(char *val, size_t len)
{
valueLen = len;
memcpy(value, val, len);
if (len<FLIGHTSIM_DATA_MAXLEN) {
memset(value+len,0,FLIGHTSIM_DATA_MAXLEN-len);
}
if (change_callback) {
if (!callbackWithObject) {
if (!hasCallbackInfo) {
(*change_callback)(value);
} else {
(*(void(*)(char*,void*))change_callback)(value,callbackInfo);
}
} else {
if (!hasCallbackInfo) {
(*(void(*)(FlightSimData*))change_callback)(this);
} else {
(*(void(*)(FlightSimData*,void*))change_callback)(this,callbackInfo);
}
}
}
}

FlightSimData * FlightSimData::find(unsigned int n)
{
for (FlightSimData *p = first; p; p = p->next) {
if (p->id == n) return p;
}
return NULL;
}
/// JB End


FlightSimInteger::FlightSimInteger()
{
id = unassigned_id++;
if (!first) {
first = this;
} else {
last->next = this;
}
last = this;
name = NULL;
next = NULL;
value = 0;
change_callback = NULL;
callbackInfo = NULL;
hasCallbackInfo = false;
FlightSimClass::request_id_messages = 1;
}

void FlightSimInteger::identify(void)
{
uint8_t len, buf[6];

if (!FlightSim.enabled || !name) return;
len = strlen((const char *)name);
buf[0] = len + 6;
buf[1] = 1;
buf[2] = id;
buf[3] = id >> 8;
buf[4] = 1;
buf[5] = 0;
FlightSimClass::xmit(buf, 6, name, len);
}

void FlightSimInteger::write(long val)
{
uint8_t buf[6];

value = val;
if (!FlightSim.enabled || !name) return; // TODO: mark as dirty
buf[0] = 10;
buf[1] = 2;
buf[2] = id;
buf[3] = id >> 8;
buf[4] = 1;
buf[5] = 0;
FlightSimClass::xmit(buf, 6, (uint8_t *)&value, 4);
}

void FlightSimInteger::update(long val)
{
value = val;
if (change_callback) {
if (!hasCallbackInfo) {
(*change_callback)(val);
} else {
(*(void(*)(long,void*))change_callback)(val,callbackInfo);
}
}
}

FlightSimInteger * FlightSimInteger::find(unsigned int n)
{
for (FlightSimInteger *p = first; p; p = p->next) {
if (p->id == n) return p;
}
return NULL;
}




FlightSimFloat::FlightSimFloat()
{
id = unassigned_id++;
if (!first) {
first = this;
} else {
last->next = this;
}
last = this;
name = NULL;
next = NULL;
value = 0;
change_callback = NULL;
hasCallbackInfo = false;
callbackInfo = NULL;
FlightSimClass::request_id_messages = 1;
}

void FlightSimFloat::identify(void)
{
uint8_t len, buf[6];

if (!FlightSim.enabled || !name) return;
len = strlen((const char *)name);
buf[0] = len + 6;
buf[1] = 1;
buf[2] = id;
buf[3] = id >> 8;
buf[4] = 2;
buf[5] = 0;
FlightSimClass::xmit(buf, 6, name, len);
}

void FlightSimFloat::write(float val)
{
uint8_t buf[6];

value = val;
if (!FlightSim.enabled || !name) return; // TODO: mark as dirty
buf[0] = 10;
buf[1] = 2;
buf[2] = id;
buf[3] = id >> 8;
buf[4] = 2;
buf[5] = 0;
FlightSimClass::xmit(buf, 6, (uint8_t *)&value, 4);
}

void FlightSimFloat::update(float val)
{
value = val;
if (change_callback) { // add: JB
if (!hasCallbackInfo) {
(*change_callback)(val);
} else {
(*(void(*)(float,void*))change_callback)(val,callbackInfo);
}
}
}

FlightSimFloat * FlightSimFloat::find(unsigned int n)
{
for (FlightSimFloat *p = first; p; p = p->next) {
if (p->id == n) return p;
}
return NULL;
}






FlightSimClass::FlightSimClass()
{
}


void FlightSimClass::update(void)
{
uint8_t len, maxlen, type, *p, *end;
union {
uint8_t b[4];
long l;
float f;
} data;
void *rx_packet;
uint16_t id;

while (1) {
if (!usb_configuration) break;
rx_packet = usb_flightsim_get_packet();
if (!rx_packet) break;
p = (uint8_t*) rx_packet;
end = p + FLIGHTSIM_RX_SIZE;
maxlen = FLIGHTSIM_RX_SIZE;
do {
len = p[0];
if (len < 2 || len > maxlen) break;
switch (p[1]) {
case 0x02: // write data
if (len < 10) break;
id = p[2] | (p[3] << 8);
type = p[4];
if (type == 1) {
FlightSimInteger *item = FlightSimInteger::find(id);
if (!item) break;
data.b[0] = p[6];
data.b[1] = p[7];
data.b[2] = p[8];
data.b[3] = p[9];
item->update(data.l);
} else if (type == 2) {
FlightSimFloat *item = FlightSimFloat::find(id);
if (!item) break;
data.b[0] = p[6];
data.b[1] = p[7];
data.b[2] = p[8];
data.b[3] = p[9];
item->update(data.f);
/// JB
} else if (type == 3) {
FlightSimEvent *item = FlightSimEvent::find(id);
if (!item) break;
data.b[0] = p[6];
data.b[1] = p[7];
data.b[2] = p[8];
data.b[3] = p[9];
item->update(data.f);
} else if (type == 4) {
FlightSimData *item = FlightSimData::find(id);
if (!item) break;
item->update(((char*)p)+6,len-6);
/// JB End
}
break;
case 0x03: // enable/disable
if (len < 4) break;
switch (p[2]) {
case 1:
request_id_messages = 1;
/* no break */
case 2:
enable();
frameCount++;
break;
case 3:
disable();
}
}
p += len;
maxlen -= len;
} while (p < end);
usb_flightsim_free_packet();
}
if (enabled && request_id_messages) {
request_id_messages = 0;
for (FlightSimCommand *p = FlightSimCommand::first; p; p = p->next) {
p->identify();
}
/// JB
for (FlightSimEvent *p = FlightSimEvent::first; p; p = p->next) {
p->identify();
}
for (FlightSimData *p = FlightSimData::first; p; p=p->next) {
p->identify();
}
/// JB End
for (FlightSimInteger *p = FlightSimInteger::first; p; p = p->next) {
p->identify();
// TODO: send any dirty data
}
for (FlightSimFloat *p = FlightSimFloat::first; p; p = p->next) {
p->identify();
// TODO: send any dirty data
}
}
}


bool FlightSimClass::isEnabled(void)
{
if (!usb_configuration) return false;
if (!enabled) return false;
if (enableTimeout > 1500) return false;
return true;
}



void FlightSimClass::xmit(const void *p1, uint8_t n1, const void *p2, uint8_t n2)
{
if (!enabled || !usb_configuration) return;

uint16_t total = n1 + n2;
if (total > FLIGHTSIM_TX_SIZE) {
xmit_big_packet(p1, n1, p2, n2);
return;
}

// handle small packets
tx_noautoflush = 1;
transfer_t *xfer = tx_transfer + tx_head;
if (!wait_for_tx_buf(xfer)) return;

if (total > tx_available) {
// send previous packet
uint8_t *txdata = (uint8_t *)(txbuffer + (tx_head * FLIGHTSIM_TX_SIZE) + (FLIGHTSIM_TX_SIZE - tx_available));
while (tx_available > 0) {
// fill packet
*txdata++ = 0;
tx_available--;
}
send_packet(xfer);
xfer = tx_transfer + tx_head;
if (!wait_for_tx_buf(xfer)) return;
}

uint8_t *txdata = (uint8_t *)(txbuffer + (tx_head * FLIGHTSIM_TX_SIZE) + (FLIGHTSIM_TX_SIZE - tx_available));
memcpy(txdata, p1, n1);
tx_available -= n1;
txdata += n1;
if (n2 > 0) {
memcpy(txdata, p2, n2);
tx_available -= n2;
}
if (tx_available == 0) {
// packet filled, send it
send_packet(xfer);
} else {
// wait for send until next SOF
usb_start_sof_interrupts(FLIGHTSIM_INTERFACE);
}
tx_noautoflush = 0;
}

void FlightSimClass::xmit_big_packet(const void *p1, uint8_t n1, const void *p2, uint8_t n2)
{

uint16_t remaining = n1 + n2;
if (remaining > 255) {
printf("Invalid flight sim packet length (>255)");
return;
}
tx_noautoflush =1; // don't mess with my data, I'm working on it!

transfer_t *xfer = tx_transfer + tx_head;
if (!wait_for_tx_buf(xfer)) return; // after this, tx_available is guaranteed to be > 0

bool part2 = false;
uint8_t remainingPart1 = n1;
const uint8_t *dataPtr = (const uint8_t*)p1;
bool writeFragmentHeader = false;
uint8_t fragmentCounter = 1;

uint8_t *txdata = (uint8_t *)(txbuffer + (tx_head * FLIGHTSIM_TX_SIZE) + (FLIGHTSIM_TX_SIZE - tx_available));

// fill first packet with whatever fits
uint8_t partLen = tx_available > n1 ? n1 : tx_available;
// copy first part, containing total packet length
printf("%d bytes free, adding first %d bytes from p1, writing to %x\n", tx_available, partLen, txdata);

memcpy(txdata, dataPtr, partLen);
remainingPart1 -= partLen;
txdata += partLen;
tx_available -= partLen;
if (remainingPart1) {
// there still is data from the first part that
// will go to the next packet. The boolean variable
// part2 remains false
remaining = remainingPart1+n2;
dataPtr += partLen;
} else {
// maybe we have space for some data from the second part
part2=true;
// there is no need here to check whether tx_available is
// bigger than n2. It's not. If it were, all the data
// would have fit in a single packet and xmit_big_packet
// would never have been called...
printf("adding first %d bytes from p2, writing to %x\n", tx_available, txdata);
remaining = n2;
if (tx_available) {
memcpy(txdata, p2, tx_available);
remaining -= tx_available;
}
dataPtr = (const uint8_t*)p2 + tx_available;
}
// first packet filled, send it
tx_available = 0;
send_packet(xfer);
xfer = tx_transfer + tx_head;

writeFragmentHeader = true;
printf("remaining bytes to send: %d\n", remaining);

while (remaining >0) {
if (!wait_for_tx_buf(xfer)) return;

uint8_t *txdata = (uint8_t *)(txbuffer + (tx_head * FLIGHTSIM_TX_SIZE) + (FLIGHTSIM_TX_SIZE - tx_available));

if (writeFragmentHeader) {
printf("writing header of fragment %d to %x\n", fragmentCounter, txdata);
*txdata++=(remaining+3 <= FLIGHTSIM_TX_SIZE) ? (uint8_t) remaining+3 : FLIGHTSIM_TX_SIZE;
*txdata++=0xff;
*txdata++=fragmentCounter++;
tx_available -= 3;
}
if (!part2) {
// we still need to send the first part
uint8_t partLen = tx_available > remainingPart1 ? remainingPart1 : tx_available;
printf("copying remaining %d bytes from first part to %x\n", partLen, txdata);
memcpy(txdata, dataPtr, partLen);
dataPtr += partLen;
txdata += partLen;
tx_available -= partLen;
remaining -= partLen;
remainingPart1 -= partLen;
if (!remainingPart1) {
part2=true;
dataPtr = (const uint8_t*)p2;
}
}

if (part2) {
uint8_t partLen = tx_available > remaining ? remaining : tx_available;
printf("copying %d bytes from second part to %x\n", partLen, txdata);
if (partLen) {
memcpy(txdata, dataPtr, partLen);
remaining -= partLen;
tx_available -= partLen;
txdata += partLen;
dataPtr += partLen;
}
}
writeFragmentHeader = true;
if (!tx_available) {
// packet filled, send it
send_packet(xfer);
xfer = tx_transfer + tx_head;
} else {
// send on next SOF
printf("tx_available: %d\n", tx_available);
usb_start_sof_interrupts(FLIGHTSIM_INTERFACE);
}
}
tx_noautoflush = 0; // data is ready to be transmitted on start of USB token
}

extern "C" {
void usb_flightsim_configure() {
printf("Flightsim_configure\n");
memset(tx_transfer, 0, sizeof(tx_transfer));
tx_head = 0;
tx_available = 0;
memset(rx_transfer, 0, sizeof(rx_transfer));
printf("tx_transfer: %x\n", tx_transfer);
printf("txbuffer: %x\n", txbuffer);
printf("rx_transfer: %x\n", rx_transfer);
printf("rxbuffer: %x\n", rx_buffer);
rx_head = 0;
rx_tail = 0;
rx_available = 0;
usb_config_rx(FLIGHTSIM_RX_ENDPOINT, FLIGHTSIM_RX_SIZE, 0, rx_event);
usb_config_tx(FLIGHTSIM_TX_ENDPOINT, FLIGHTSIM_TX_SIZE, 0, NULL); // TODO: is ZLP needed?
int i;
for (i=0; i < RX_NUM; i++) rx_queue_transfer(i);
tx_noautoflush = 0;
transmit_previous_timeout = 0;

}

// This gets called from usb_isr when a USB start token arrives.
// If we have a packet to transmit AND transmission isn't disabled
// by tx_noautoflush, we fill it up with zeros and send it out
// to USB
void usb_flightsim_flush_output(void)
{
if (tx_noautoflush == 0 && tx_available > 0) {
printf(" flush, %d %d\n", FLIGHTSIM_TX_SIZE, tx_available);
uint32_t head = tx_head;
transfer_t *xfer = tx_transfer + head;
uint8_t *txbuf = txbuffer + (head * FLIGHTSIM_TX_SIZE);
uint8_t *txPtr = txbuf + (FLIGHTSIM_TX_SIZE - tx_available);
while (tx_available>0) {
*txPtr++ = 0;
tx_available--;
}
usb_prepare_transfer(xfer, txbuf, FLIGHTSIM_TX_SIZE, 0);
arm_dcache_flush_delete(txbuf, FLIGHTSIM_TX_SIZE);
usb_transmit(FLIGHTSIM_TX_ENDPOINT, xfer);
if (++head >= TX_NUM) head = 0;
tx_head = head;
usb_stop_sof_interrupts(FLIGHTSIM_INTERFACE);
}
}

static bool wait_for_tx_buf(transfer_t *xfer) {
uint32_t wait_begin_at = systick_millis_count;
while (!tx_available) {
uint32_t status = usb_transfer_status(xfer);
if (!(status & 0x80)) {
if (status & 0x68) {
// TODO: what if status has errors???
}
tx_available = FLIGHTSIM_TX_SIZE;
transmit_previous_timeout = 0;
break;
}
if (systick_millis_count - wait_begin_at > TX_TIMEOUT_MSEC) {
transmit_previous_timeout = 1;
}
if (transmit_previous_timeout) {
printf("Flight sim tx timeout");
return false;
}
if (!usb_configuration) return false;
yield();
}
return true;
}

static void send_packet(transfer_t *xfer) {
uint32_t head = tx_head;
uint8_t *txbuf = txbuffer + (tx_head * FLIGHTSIM_TX_SIZE);
usb_prepare_transfer(xfer, txbuf, FLIGHTSIM_TX_SIZE, 0);
arm_dcache_flush_delete(txbuf, FLIGHTSIM_TX_SIZE);
usb_transmit(FLIGHTSIM_TX_ENDPOINT, xfer);
if (++head >= TX_NUM) head = 0;
tx_head = head;
usb_stop_sof_interrupts(FLIGHTSIM_INTERFACE);
}

static void rx_queue_transfer(int i)
{
NVIC_DISABLE_IRQ(IRQ_USB1);
void *buffer = rx_buffer + i * FLIGHTSIM_RX_SIZE;
usb_prepare_transfer(rx_transfer + i, buffer, FLIGHTSIM_RX_SIZE, i);
arm_dcache_delete(buffer, FLIGHTSIM_RX_SIZE);
usb_receive(FLIGHTSIM_RX_ENDPOINT, rx_transfer + i);
NVIC_ENABLE_IRQ(IRQ_USB1);
}

static void rx_event(transfer_t *t)
{
int len = FLIGHTSIM_RX_SIZE - ((t->status >> 16) & 0x7FFF);
len &= 0xFFFC; // MIDI packets must be multiple of 4 bytes
int i = t->callback_param;
// printf("Flight sim rx event, len=%d, i=%d", len, i);
if (len == FLIGHTSIM_RX_SIZE) {
uint32_t head = rx_head;
if (++head > RX_NUM) head = 0;
rx_list[head] = i;
rx_head = head;
rx_available += len;
} else {
// received packet with invalid length
rx_queue_transfer(i);
}
// printf(" ...done\n");
}

static void* usb_flightsim_get_packet(void)
{
void *result = NULL;
NVIC_DISABLE_IRQ(IRQ_USB1);
uint32_t tail = rx_tail;
if (tail != rx_head) {
if (++tail > RX_NUM) tail = 0;
uint32_t i = rx_list[tail];
result = rx_buffer + i * FLIGHTSIM_RX_SIZE;
rx_available -= FLIGHTSIM_RX_SIZE;
}
NVIC_ENABLE_IRQ(IRQ_USB1);
return result;
}

static void usb_flightsim_free_packet() {
NVIC_DISABLE_IRQ(IRQ_USB1);
uint32_t tail = rx_tail;
if (tail != rx_head) {
if (++tail > RX_NUM) tail = 0;
uint32_t i = rx_list[tail];
rx_tail = tail;
rx_queue_transfer(i);
}
NVIC_ENABLE_IRQ(IRQ_USB1);
}

} // extern "C"

#endif // F_CPU
#endif // FLIGHTSIM_INTERFACE

+ 324
- 0
teensy4/usb_flightsim.h Ver arquivo

@@ -0,0 +1,324 @@
/* Teensyduino Core Library
* http://www.pjrc.com/teensy/
* Copyright (c) 2013 PJRC.COM, LLC.
*
* Permission is hereby granted, free of charge, to any person obtaining
* a copy of this software and associated documentation files (the
* "Software"), to deal in the Software without restriction, including
* without limitation the rights to use, copy, modify, merge, publish,
* distribute, sublicense, and/or sell copies of the Software, and to
* permit persons to whom the Software is furnished to do so, subject to
* the following conditions:
*
* 1. The above copyright notice and this permission notice shall be
* included in all copies or substantial portions of the Software.
*
* 2. If the Software is incorporated into a build system that allows
* selection among a list of target devices, then similar target
* devices manufactured by PJRC.COM must be included in the list of
* target devices and selectable in the same manner.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/

#ifndef USBflightsim_h_
#define USBflightsim_h_

#include "usb_desc.h"

#if defined(FLIGHTSIM_INTERFACE)

#ifdef __cplusplus
extern "C" {
#endif
void usb_flightsim_configure();
void usb_flightsim_flush_output(void);
#ifdef __cplusplus
}
#endif

#ifdef __cplusplus

#include <inttypes.h>
#include "elapsedMillis.h"

// workaround for elapsedMillis.h bringing in WProgram.h which brings usb_undef.h
//#undef USB_DESC_LIST_DEFINE
//#include "usb_desc.h"

class FlightSimClass;
class FlightSimCommand;
class FlightSimInteger;

class _XpRefStr_;
#define XPlaneRef(str) ((const _XpRefStr_ *)(str))

class FlightSimClass
{
public:
FlightSimClass();
static void update(void);
static bool isEnabled(void);
static unsigned long getFrameCount(void) { return frameCount; }
private:
static uint8_t request_id_messages;
static uint8_t enabled;
static elapsedMillis enableTimeout;
static unsigned long frameCount;
static void enable(void) { enabled = 1; enableTimeout = 0; }
static void disable(void) { enabled = 0; }
static void xmit(const void *p1, uint8_t n1, const void *p2, uint8_t n2);
static void xmit_big_packet(const void *p1, uint8_t n1, const void *p2, uint8_t n2);
friend class FlightSimCommand;
friend class FlightSimInteger;
friend class FlightSimFloat;
/// JB
friend class FlightSimEvent;
friend class FlightSimData;
/// JB End
};


class FlightSimCommand
{
public:
FlightSimCommand();
void assign(const _XpRefStr_ *s) { name = s; if (FlightSimClass::enabled) identify(); }
FlightSimCommand & operator = (const _XpRefStr_ *s) { assign(s); return *this; }
void begin(void) { sendcmd(4); }
void end(void) { sendcmd(5); }
FlightSimCommand & operator = (int n) { sendcmd((n) ? 4 : 5); return *this; }
void once(void) { sendcmd(6); }
void identify(void);
private:
unsigned int id;
const _XpRefStr_ *name;
void sendcmd(uint8_t n);
FlightSimCommand *next;
static FlightSimCommand *first;
static FlightSimCommand *last;
friend class FlightSimClass;
};

/// JB
class FlightSimEvent
{
public:
FlightSimEvent();
void assign(const _XpRefStr_ *s) { name = s; if (FlightSimClass::enabled) identify(); }
FlightSimEvent & operator = (const _XpRefStr_ *s) { assign(s); return *this; }
void send() { send(0,0); }
void send(int data) { send(data,0); }
void sendOnce() { send(0,0); }
void sendOnce(int data) { send(data,0); }
void sendRepeat(int data, uint16_t initialDelay, uint16_t repeatDelay) { send(data, initialDelay<<16 | repeatDelay); }
void sendRepeat(uint16_t initialDelay, uint16_t repeatDelay) { send(0, initialDelay<<16 | repeatDelay); }
void stopRepeat() { send(0,-1); }
FlightSimEvent & operator = (int n) { send(n,0); return *this; }
bool occurred() { bool hasOccurred = occurredFlag; occurredFlag = 0; return hasOccurred; }
void identify();
static FlightSimEvent * find(unsigned int n);
void update(long val);
void onOccur(void (*fptr)(long)) {
hasCallbackInfo=false;
occur_callback = fptr;
}
void onOccur(void (*fptr)(long,void*), void* info) {
hasCallbackInfo=true;
occur_callback = (void (*)(long))fptr;
callbackInfo = info;
}
private:
void send(unsigned int data, unsigned int flags);
unsigned int id;
const _XpRefStr_ *name;
bool occurredFlag;
unsigned int value;
FlightSimEvent *next;
void (*occur_callback)(long);
void* callbackInfo;
bool hasCallbackInfo;
static FlightSimEvent *first;
static FlightSimEvent *last;
friend class FlightSimClass;
};

#define FLIGHTSIM_DATA_MAXLEN 58

class FlightSimData
{
public:
FlightSimData();
void assign(const _XpRefStr_ *s) { name = s; if (FlightSimClass::enabled) identify(); }
FlightSimData & operator = (const _XpRefStr_ *s) { assign(s); return *this; }
char *read() { return value; }
operator char* () { return value; }
void identify();
void update(char *val, size_t len);
size_t len() { return valueLen; }
static FlightSimData * find(unsigned int n);
void onChange(void (*fptr)(char *)) {
hasCallbackInfo = false;
change_callback = fptr;
}
void onChange(void (*fptr)(char *,void *), void *info) {
hasCallbackInfo = true;
change_callback = (void (*)(char*)) fptr;
callbackInfo = info;
}
void onChange(void (*fptr)(FlightSimData *)) {
callbackWithObject = true;
hasCallbackInfo = false;
change_callback = (void (*)(char*)) fptr;
}
void onChange(void (*fptr)(FlightSimData *, void*), void* info) {
callbackWithObject = true;
hasCallbackInfo = true;
change_callback = (void (*)(char*)) fptr;
callbackInfo = info;
}
private:
unsigned int id;
const _XpRefStr_ *name;
char value[FLIGHTSIM_DATA_MAXLEN];
size_t valueLen;
void (*change_callback)(char *);
void* callbackInfo;
bool hasCallbackInfo;
bool callbackWithObject;
FlightSimData *next;
static FlightSimData *first;
static FlightSimData *last;
friend class FlightSimClass;
};
/// JB End


class FlightSimInteger
{
public:
FlightSimInteger();
void assign(const _XpRefStr_ *s) { name = s; if (FlightSimClass::enabled) identify(); }
FlightSimInteger & operator = (const _XpRefStr_ *s) { assign(s); return *this; }
void write(long val);
FlightSimInteger & operator = (char n) { write((long)n); return *this; }
FlightSimInteger & operator = (int n) { write((long)n); return *this; }
FlightSimInteger & operator = (long n) { write(n); return *this; }
FlightSimInteger & operator = (unsigned char n) { write((long)n); return *this; }
FlightSimInteger & operator = (unsigned int n) { write((long)n); return *this; }
FlightSimInteger & operator = (unsigned long n) { write((long)n); return *this; }
FlightSimInteger & operator = (float n) { write((long)n); return *this; }
FlightSimInteger & operator = (double n) { write((long)n); return *this; }
long read(void) const { return value; }
operator long () const { return value; }
void identify(void);
void update(long val);
static FlightSimInteger * find(unsigned int n);
void onChange(void (*fptr)(long)) {
hasCallbackInfo=false;
change_callback = fptr;
}
void onChange(void (*fptr)(long,void*), void* info) {
hasCallbackInfo=true;
change_callback = (void (*)(long))fptr;
callbackInfo = info;
}
// TODO: math operators.... + - * / % ++ --
private:
unsigned int id;
const _XpRefStr_ *name;
long value;
void (*change_callback)(long);
void* callbackInfo;
bool hasCallbackInfo;
FlightSimInteger *next;
static FlightSimInteger *first;
static FlightSimInteger *last;
friend class FlightSimClass;
};


class FlightSimFloat
{
public:
FlightSimFloat();
void assign(const _XpRefStr_ *s) { name = s; if (FlightSimClass::enabled) identify(); }
FlightSimFloat & operator = (const _XpRefStr_ *s) { assign(s); return *this; }
void write(float val);
FlightSimFloat & operator = (char n) { write((float)n); return *this; }
FlightSimFloat & operator = (int n) { write((float)n); return *this; }
FlightSimFloat & operator = (long n) { write((float)n); return *this; }
FlightSimFloat & operator = (unsigned char n) { write((float)n); return *this; }
FlightSimFloat & operator = (unsigned int n) { write((float)n); return *this; }
FlightSimFloat & operator = (unsigned long n) { write((float)n); return *this; }
FlightSimFloat & operator = (float n) { write(n); return *this; }
FlightSimFloat & operator = (double n) { write((float)n); return *this; }
float read(void) const { return value; }
operator float () const { return value; }
void identify(void);
void update(float val);
static FlightSimFloat * find(unsigned int n);
void onChange(void (*fptr)(float)) {
hasCallbackInfo=false;
change_callback = fptr;
}
void onChange(void (*fptr)(float,void*), void* info) {
hasCallbackInfo=true;
change_callback = (void (*)(float))fptr;
callbackInfo = info;
}
// TODO: math operators.... + - * / % ++ --
private:
unsigned int id;
const _XpRefStr_ *name;
float value;
void (*change_callback)(float);
void* callbackInfo;
bool hasCallbackInfo;
FlightSimFloat *next;
static FlightSimFloat *first;
static FlightSimFloat *last;
friend class FlightSimClass;
};


class FlightSimElapsedFrames
{
private:
unsigned long count;
public:
FlightSimElapsedFrames(void) { count = FlightSimClass::getFrameCount(); }
FlightSimElapsedFrames(unsigned long val) { count = FlightSimClass::getFrameCount() - val; }
FlightSimElapsedFrames(const FlightSimElapsedFrames &orig) { count = orig.count; }
operator unsigned long () const { return FlightSimClass::getFrameCount() - count; }
FlightSimElapsedFrames & operator = (const FlightSimElapsedFrames &rhs) { count = rhs.count; return *this; }
FlightSimElapsedFrames & operator = (unsigned long val) { count = FlightSimClass::getFrameCount() - val; return *this; }
FlightSimElapsedFrames & operator -= (unsigned long val) { count += val; return *this; }
FlightSimElapsedFrames & operator += (unsigned long val) { count -= val; return *this; }
FlightSimElapsedFrames operator - (int val) const { FlightSimElapsedFrames r(*this); r.count += val; return r; }
FlightSimElapsedFrames operator - (unsigned int val) const { FlightSimElapsedFrames r(*this); r.count += val; return r; }
FlightSimElapsedFrames operator - (long val) const { FlightSimElapsedFrames r(*this); r.count += val; return r; }
FlightSimElapsedFrames operator - (unsigned long val) const { FlightSimElapsedFrames r(*this); r.count += val; return r; }
FlightSimElapsedFrames operator + (int val) const { FlightSimElapsedFrames r(*this); r.count -= val; return r; }
FlightSimElapsedFrames operator + (unsigned int val) const { FlightSimElapsedFrames r(*this); r.count -= val; return r; }
FlightSimElapsedFrames operator + (long val) const { FlightSimElapsedFrames r(*this); r.count -= val; return r; }
FlightSimElapsedFrames operator + (unsigned long val) const { FlightSimElapsedFrames r(*this); r.count -= val; return r; }
};


extern FlightSimClass FlightSim;

#endif // __cplusplus

#endif // FLIGHTSIM_INTERFACE

#endif // USBflightsim_h_

Escrever Pré-visualização
Carregando…
Cancelar
Salvar