void AssetScaleBoneSetLocalTranslation(ASSET_SCALE_BONE* bone, const float* translation)
{
float scale;
COPY_VECTOR3(bone->position, translation);
SET_MAX(bone->position[0], ASSET_SCALE_BONE_MAX_VALUE);
SET_MAX(bone->position[1], ASSET_SCALE_BONE_MAX_VALUE);
SET_MAX(bone->position[2], ASSET_SCALE_BONE_MAX_VALUE);
scale = (bone->position[0] + bone->position[1] + bone->position[2]) / 3.0f;
bone->interface_data.model->scale_factor = scale;
}
int main(int argc, char *argv[])
{
struct relay *relays;
int relay_count, is_server;
int i;
fd_set readfds;
int max = 0;
int ok;
parse_args(argc, argv, &relays, &relay_count, &is_server);
for (i = 0; i < relay_count; i++) {
if (is_server) {
setup_server_listen(&relays[i]);
}
else {
setup_tcp_client(&relays[i]);
}
setup_udp_recv(&relays[i]);
setup_udp_send(&relays[i]);
}
if (is_server) {
await_incoming_connections(relays, relay_count);
}
do {
FD_ZERO(&readfds);
for (i = 0; i < relay_count; i++) {
FD_SET(relays[i].tcp_sock, &readfds);
SET_MAX(relays[i].tcp_sock);
FD_SET(relays[i].udp_recv_sock, &readfds);
SET_MAX(relays[i].udp_recv_sock);
}
if (select(max, &readfds, NULL, NULL, NULL) < 0) {
if (errno != EINTR) {
perror("main loop: select");
exit(1);
}
}
ok = 0;
for (i = 0; i < relay_count; i++) {
if (FD_ISSET(relays[i].tcp_sock, &readfds)) {
ok += tcp_to_udp(&relays[i]);
}
if (FD_ISSET(relays[i].udp_recv_sock, &readfds)) {
ok += udp_to_tcp(&relays[i]);
}
}
} while (ok == 0);
exit(0);
} /* main */
/* await_incoming_connections()
* Wait for connections to be established to all the TCP listeners.
* Fill in the tcp_sock element of each relay.
* Exit on any errors.
*/
static void await_incoming_connections(struct relay *relays, int relay_count)
{
int i;
fd_set readfds;
int max = 0;
int all_connected;
do {
FD_ZERO(&readfds);
all_connected = 1;
for (i = 0; i < relay_count; i++) {
if (relays[i].tcp_sock == -1) {
/* Only count relays we haven't had connections on yet */
all_connected = 0;
FD_SET(relays[i].tcp_listen_sock, &readfds);
SET_MAX(relays[i].tcp_listen_sock);
}
}
if (all_connected) break;
if (select(max, &readfds, NULL, NULL, NULL) < 0) {
if (errno != EINTR) {
perror("await_incoming_connection: select");
exit(1);
}
}
for (i = 0; i < relay_count; i++) {
if (FD_ISSET(relays[i].tcp_listen_sock, &readfds)) {
struct sockaddr_in client_addr;
int addrlen = sizeof(client_addr);
if ((relays[i].tcp_sock =
accept(relays[i].tcp_listen_sock,
(struct sockaddr *) &client_addr, &addrlen)) < 0) {
perror("await_incoming_connections: accept");
exit(1);
}
if (debug) {
fprintf(stderr, "TCP connection from %s/%hu\n",
inet_ntoa(client_addr.sin_addr),
ntohs(client_addr.sin_port));
}
}
}
} while (!all_connected);
} /* await_incoming_connections */
void handle_Control ( )
{
unsigned int usb_cnt=0;
if((EP_STATUS_OUT & 0x01)||(EP_STATUS_IN & 0x01)) // 8 byte setup data received
{
EP_STATUS_OUT = 0x01; // write 1 to clear
EP_STATUS_IN = 0x01;
EP_STATE[0] = EP_IDLE; // force EP_STATE[0] to EP_IDLE
system.usbp0_data.wait_out_report=0;
if (CNT0 == 8) // Make sure that EP 0 has an Out Packet of size 8 byte
{
// added on 2011.5.8 to fix usb set_report error.
if (EP0_ADDR_DEF==0x21 && EP0_ADDR_DEF_1 == 0x09 )
{
USBINT0 = 0xc0;
Set_Report();
system.usbp0_data.report_id=EP0_ADDR_DEF_2;
system.usbp0_data.report_type = EP0_ADDR_DEF_3;
set_ep_rdy(EP_0); // 1:Endpoint 0 ready for transfer USB data.
}
else if(EP0_ADDR_DEF==0x21 && EP0_ADDR_DEF_1 == 0x01 )
{ // Fix SET_CUR request error
SET_CUR();
}
else
{
fifo_Read (EP_0, 8, (UINT8 *) & setup);
// Get setup Packet off of Fifo,
// it is currently Big-Endian
// Compiler Specific - these next three
// statements swap the bytes of the
// setup packet words to Big Endian so
// they can be compared to other 16-bit
// values elsewhere properly
// USBINT0 = 0xc0;
// set_ep_rdy(EP_0); // 1:Endpoint 0 ready for transfer USB data.
setup.wValue.i = setup.wValue.c[MSB] + ((int)setup.wValue.c[LSB] << 8); // High byte and low byte exchange()
setup.wIndex.i = setup.wIndex.c[MSB] + ( (int)setup.wIndex.c[LSB] << 8);
setup.wLength.i = setup.wLength.c[MSB] + ( (int)setup.wLength.c[LSB] << 8);
// Intercept HID class - specific requests
// Class request.
// if( (setup.bmRequestType & ~ 0x80) == DSC_HID)
if((setup.bmRequestType & cRequestType) == cClassReq)
{
#if 0
switch (setup.bRequest)
{
case GET_REPORT:
Get_Report ( );
break;
case SET_REPORT:
Set_Report();
break;
case GET_IDLE:
Get_Idle ( );
break;
case SET_IDLE:
Set_Idle ( );
break;
case GET_PROTOCOL:
Get_Protocol ( );
break;
case SET_PROTOCOL:
Set_Protocol ( );
break;
default:
force_Stall ( ); // Send stall to host if invalid
break; // request
}
#endif
switch(setup.bRequest)
{
// bmRequestType, bRequest, bChannelNum.(CN), bControlSelect(CS, include volume, mute etc.), bInterfaceNO, bTerminal/unit ID, wLength.
// CTL: 21 01 00 02 00 05 02 00
// DO: 00 00
case cSET_CUR:
if(setup.wIndex.c[1] >= 3) // HID interface.
{
Get_Report();
}
else
SET_CUR();
break;
case cSET_MIN:
if(setup.wIndex.c[1] >= 3) // HID interface.
{
Get_Idle();
}
else
SET_MIN();
break;
case cSET_MAX:
SET_MAX();
break;
case cSET_RES:
SET_RES();
break;
case cSET_MEM:
SET_MEM();
break;
/*
case GET_REPORT:
Get_Report();
break;
*/
case SET_REPORT:
Set_Report();
break;
/*
case GET_IDLE:
Get_Idle ( );
break;
*/
case SET_IDLE:
Set_Idle ( );
break;
case cGET_CUR:
GET_CUR();
break;
case cGET_MIN:
GET_MIN();
break;
case cGET_MAX:
GET_MAX();
break;
case cGET_RES:
GET_RES();
break;
case cGET_MEM:
GET_MEM();
break;
case cGET_STAT:
GET_STAT();
break;
default:
force_Stall ( ); // Send stall to host if invalid
break; // request
}
}
else
{
// Standard request.
switch (setup.bRequest) // Call correct subroutine to handle
{ // each kind of standard request
case GET_STATUS:
Get_Status ( );
break;
case CLEAR_FEATURE:
Clear_Feature ( );
break;
case SET_FEATURE:
Set_Feature ( );
break;
case SET_ADDRESS:
Set_Address ( );
break;
case GET_DESCRIPTOR:
Get_Descriptor ( );
break;
case GET_CONFIGURATION:
Get_Configuration ( );
break;
case SET_CONFIGURATION:
Set_Configuration( );
break;
case GET_INTERFACE:
Get_Interface ( );
break;
case SET_INTERFACE: // 01 0b 00 00(wValue) 01 00(wIndex) 00 00
Set_Interface ( ); // wValue:Interface alternate setting(0x0 is zero bandwith alternate setting, 0x01 is normal isochronous operate)
break; // wIndex: Interface number of one of the audiostreaming interface.
default:
force_Stall ( ); // Send stall to host if invalid request
break;
}
}
}
}
else
{
force_Stall ( ); // if rec setup data is not 8 byte , protacal stall
}
}
else if (EP_STATE[0] == EP_SET_ADDRESS) // Handle Status Phase of Set Address
{
FUNCT_ADR = setup.wValue.c[LSB]; // set usb Address SFR
EP_STATE[0] = EP_IDLE;
}
else if (EP_STATE[0] == EP_WAIT_STATUS)
{
EP_STATE[0] = EP_IDLE;
wait_tx_status=0;
}
else if (EP_STATE[0] == EP_RX) // See if endpoint should transmit
{ // Out Token. PC->Device.
if (DATASIZE >= EP0_PACKET_SIZE)
{
fifo_Read(EP_0, EP0_PACKET_SIZE, (unsigned char * )DATAPTR);
// Advance data pointer
DATAPTR += EP0_PACKET_SIZE;
// Decrement data size
DATASIZE -= EP0_PACKET_SIZE;
// Increment data sent counter
DATASENT += EP0_PACKET_SIZE;
}
else
{
// read bytes from FIFO
// // Report type: 0x01 input, 0x02 output, 0x03 feature report.
if( system.usbp0_data.wait_out_report && (system.usbp0_data.report_type==PC_SET_REPORT_2) && (system.usbp0_data.report_id==PC_SET_REPORT_1) )
{
system.usbp0_data.wait_out_report=0;
DATAPTR = & EP0_ADDR_DEF;
if(PC_SET_REPORT_1 == DATAPTR[0])
{
DATAPTR++;
DATASIZE--;
}
for ( system.usbp0_data.report_cnt = 0; // read num = NO fifo_data;
system.usbp0_data.report_cnt< DATASIZE;
system.usbp0_data.report_cnt ++ )
{
system.usbp0_data.set_report_data[system.usbp0_data.report_cnt] = DATAPTR[system.usbp0_data.report_cnt];
}
system.usbp0_data.aceept = 1;
system.usbp0_data.reday_report_flag = 0;
set_report_status_phace=1;
}
else
{
fifo_Read (EP_0, DATASIZE, (UINT8 * ) DATAPTR);
// KEYBOARD_OUT_REPORT
if (system.usbp0_data.wait_out_report)
{
numlock_sta = system.usbp0_data.out_report[0] & 0x01;
system.usbp0_data.out_report_index = DATASIZE;
system.usbp0_data.wait_out_report=0;
set_report_status_phace=1;
}
}
set_Wait_Status ( ); // set Endpoint to EP_WAIT_STATUS
}
if ( (DATASENT == setup.wLength.i) && (set_report_status_phace==0) )
{
set_Wait_Status ( );
}
set_report_status_phace=0;
}
if (EP_STATE[0] == EP_TX) // See if endpoint should transmit
{ // In Token, device->PC
if ((DATASIZE == 0))// || (DATASENT == setup.wLength.i)) // when all data has been sent over
{
set_Wait_Status ( );
}
else
{
CFG_EP0_1 = 0xc0;
if (DATASIZE >= EP0_PACKET_SIZE)
{
// Break Data into multiple packets if larger than Max Packet
fifo_Write (EP_0, EP0_PACKET_SIZE, (unsigned char*)DATAPTR);
// Advance data pointer
DATAPTR += EP0_PACKET_SIZE;
// Decrement data size
DATASIZE -= EP0_PACKET_SIZE;
// Increment data sent counter
DATASENT += EP0_PACKET_SIZE;
}
else
{
// If data is less than Max Packet size or zero
fifo_Write (EP_0, DATASIZE, (unsigned char*)DATAPTR);
// Increment data sent counter
DATASENT += DATASIZE;
// Decrement data size
DATASIZE = 0;
USBINT0 = 0xc0;
EP_RDY = 0x01 ;
// when usb_reset and send ok inttrupt (USBINT0&0x48)!=0 // when setup come (EP_STATUS & 0x01) will set
while(((USBINT0 & 0x4C) == 0) && ((EP_STATUS_OUT & 0x01) == 0)); //wait usb_reset, suspend or sending end interrupt happend, wait setup come
if((USBINT0&0x40) !=0)
{
CFG_EP0_1 =0x40;
EP_RDY = 0x01 ;
USBINT0 = 0xc0;
EP_STATE[0]= EP_WAIT_STATUS;
}
wait_tx_status = 1;
system.usbp0_data.wait_out_report=0;
}
}
}
if (system.usbp0_data.wait_out_report==0 && wait_tx_status ==0)
{
USBINT0 = 0xc0;
// set_ep_rdy(EP_0); // set ready to receive or send next packet
EP_RDY = 0x01 ;
}
}
void calc_process(void)
{
struct calc_data *c = &mavdata.calcs;
struct flight_stats *s = &mavdata.stats;
/* calcs for home */
float dlat = c->home_lat - mavdata.gps_lat;
float dlon = c->home_lon - mavdata.gps_lon;
float cos_lat = cos(DEG2RAD(fabs(c->home_lat)));
/* calculate aprox home distance */
float dlon_cos = dlon * cos_lat;
float dist = DEG2RAD(EARTH_AVG_RADIUS) * sqrt(dlat*dlat + dlon_cos*dlon_cos);
/* home initial bearing */
float dlat_cos = dlat / cos_lat;
float bearing = 90 + RAD2DEG(atan2(dlat_cos, -dlon));
/* return bearing */
bearing += 180;
/* relative return bearing */
bearing -= mavdata.vfr_hud.heading;
if(bearing < 0)
bearing += 360;
c->home_distance = (unsigned int) dist;
c->home_direction = (unsigned int) bearing;
if (c->has_home < (SLOW_TICKS_PER_SECOND * 5)) {
/* get gps lat/lon within 5 sec of having 2D fix */
if (mavdata.gps_fix_type > 1) {
c->has_home++;
} else {
c->has_home = 0;
}
c->home_lat = mavdata.gps_lat;
c->home_lon = mavdata.gps_lon;
} else if (c->has_home < (SLOW_TICKS_PER_SECOND * 10)){
/* set home altitude when altitude is steady (delta < 1m) for 5 sec */
if (abs((long) c->home_altitude - (long) mavdata.vfr_hud.alt) < 1) {
c->has_home++;
} else {
c->has_home = SLOW_TICKS_PER_SECOND * 5;
}
c->home_altitude = (unsigned int) mavdata.vfr_hud.alt;
}
/* guess when the flight starts */
if ((s->flight_start == 0) && \
(mavdata.vfr_hud.throttle > 10) && \
(c->has_home) && (c->home_altitude > 10) && \
(mavdata.vfr_hud.airspeed > 8)) {
s->flight_end = s->flight_start = get_uptime();
}
/* guess when landing occurs */
if ((s->flight_start != 0) && \
((mavdata.vfr_hud.throttle > 3) || \
(mavdata.vfr_hud.airspeed > 3) || \
(c->home_altitude > 10))) {
s->flight_end = get_uptime();
}
/* total distance covered accumulator */
if (c->has_home) {
s->total_distance += mavdata.vfr_hud.groundspeed * SLOW_PROCESS_TICK / 1000;
}
/* store maximum values */
if (s->flight_start != 0) {
SET_MAX(s->max_home_distance, c->home_distance);
SET_MAX(s->max_airspeed, mavdata.vfr_hud.airspeed);
SET_MAX(s->max_groundspeed, mavdata.vfr_hud.groundspeed);
SET_MAX(s->max_home_altitude, c->home_altitude);
SET_MAX(s->max_widspeed, mavdata.wind_speed);
}
}
