0920
Embed:
(wiki syntax)
Show/hide line numbers
DualShock.cpp
00001 #include "DualShock.h" 00002 00003 tDSParm hDS; 00004 static uint8_t recv_data, check = 0; 00005 static Serial* ds_serial; 00006 static float DimensionlessAndBacklash(int8_t dat); 00007 static uint8_t getsum(void *buf, size_t size); 00008 00009 void ReStartDS(void) { 00010 check = 0; 00011 recv_data = (*ds_serial).getc(); 00012 } 00013 00014 uint8_t InitDS(Serial* f_serial) { 00015 00016 hDS.BUTTON.ButtonData = 0; 00017 hDS.ANALOG.LX = 0; 00018 hDS.ANALOG.LY = 0; 00019 hDS.ANALOG.RX = 0; 00020 hDS.ANALOG.RY = 0; 00021 //huartDS = huart; 00022 ds_serial = f_serial; 00023 00024 printf("DualShock connecting check...\r\n"); 00025 ReStartDS(); 00026 printf("DualShock connect successful...\r\n"); 00027 return 0; 00028 } 00029 //&getDSdata 00030 void getDSdata(void) { 00031 static uint8_t dat[8] = {0}; 00032 00033 recv_data = (*ds_serial).getc(); 00034 dat[check] = recv_data; 00035 switch (check) { 00036 case 0: 00037 if (dat[0] == 0x80) { 00038 check++; 00039 } 00040 break; 00041 case 7: 00042 if (dat[7] == (getsum(dat, 7) & 0x7f)) { 00043 hDS.BUTTON.ButtonHighData = dat[1]; 00044 hDS.BUTTON.ButtonLowData = dat[2]; 00045 hDS.ANALOG.LX = DimensionlessAndBacklash(dat[3]); 00046 hDS.ANALOG.LY = DimensionlessAndBacklash(dat[4]); 00047 hDS.ANALOG.RX = DimensionlessAndBacklash(dat[5]); 00048 hDS.ANALOG.RY = DimensionlessAndBacklash(dat[6]); 00049 } 00050 dat[0] = 0; 00051 check = 0; 00052 break; 00053 default: 00054 check++; 00055 break; 00056 } 00057 } 00058 00059 static float DimensionlessAndBacklash(int8_t dat) { 00060 float val; 00061 00062 if ((dat < BACKLASH) && (dat > -BACKLASH)) { 00063 dat = 0; 00064 } else { 00065 dat += (dat > 0) ? (-BACKLASH) : (BACKLASH); 00066 } 00067 val = (float) dat / (float) (128 - BACKLASH); 00068 return val; 00069 } 00070 00071 static uint8_t getsum(void *buf, size_t size) { 00072 const uint8_t *p = (uint8_t*) buf; 00073 uint8_t ret = 0; 00074 00075 for (; size; size--) { 00076 ret += *p++; 00077 } 00078 return ret; 00079 }
Generated on Mon Jul 18 2022 11:21:59 by
1.7.2