major rewrite of the G27 arduino SW

- add configurable median filter for axis signals
- replace all configuration preprocessor switches with a calibration
structure stored and loaded from EEPROM
- add more configuration options (invert for all axis, replace reverse
button with the right red button)
- replace ASCII debug serial output with much more efficient binary
serial output
- add a supporting GUI for calibration and configuration
This commit is contained in:
n-e-y-s 2020-06-15 15:06:05 +02:00
parent 3b0fe5431d
commit 689bf7b71c
7 changed files with 1142 additions and 209 deletions

3
.gitignore vendored Normal file
View File

@ -0,0 +1,3 @@
.idea
*.egg*
__pycache__

View File

@ -5,31 +5,9 @@
// http://www.isrtv.com/forums/topic/13189-diy-g25-shifter-interface-with-h-pattern-sequential-and-handbrake-modes/
#include <HID.h>
#include <EEPROM.h>
#include "./src/G27PedalsShifter.h"
// comment either out to disable
#define USE_PEDALS
#define USE_SHIFTER
// for debugging, gives serial output rather than working as a joystick
//#define DEBUG true
#if defined(DEBUG)
#define DEBUG_PEDALS true
#define DEBUG_SHIFTER true
#endif
//#define DEBUG_PEDALS true
//#define DEBUG_SHIFTER true
// red for brake, green for gas, blue for clutch
//#define PEDAL_COLORS true
// for load-cell users and Australians
//#define INVERT_BRAKE true
// use static thresholds rather than on-the-fly calibration
//#define STATIC_THRESHOLDS true
#include "./src/Filter.h"
// LED PINS
#define RED_PIN 3
@ -97,38 +75,127 @@
#define OUTPUT_RED_CENTERRIGHT 17
#define OUTPUT_RED_RIGHT 18
// SHIFTER AXIS THRESHOLDS
#define SHIFTER_XAXIS_12 290 //Gears 1,2
#define SHIFTER_XAXIS_56 600 //Gears 5,6, R
#define SHIFTER_YAXIS_135 750 //Gears 1,3,5
#define SHIFTER_YAXIS_246 290 //Gears 2,4,6, R
// PEDAL AXIS THRESHOLDS
#define MIN_GAS 27
#define MAX_GAS 925
#define MIN_BRAKE 26
#define MAX_BRAKE 845
#define MIN_CLUTCH 45
#define MAX_CLUTCH 932
// MISC.
#define MAX_AXIS 1023
#define SIGNAL_SETTLE_DELAY 10
#define CALIB_DATA_MAGIC_NUMBER 0x27CA11B1 // change this when the struct definition changes
#define FLAG_INVERT_BRAKE 0x1 // invert brake pedal
#define FLAG_INVERT_GAS 0x2 // invert gas pedal
#define FLAG_INVERT_CLUTCH 0x4 // invert clutch pedal
#define FLAG_REVERSE_RIGHT_RED 0x8 // use right red button for reverse gear instead of the reverse sensor
typedef struct CalibData
{
/* magic number, used for testing valid EEPROM content */
uint32_t calibID;
/* bool whether to automatically calibrate the pedals at a power cycle or use a static calibration */
uint8_t pedals_auto_calib;
/* flags, see the FLAG_* defines for the meaning of the bits */
uint8_t flags;
/* bool whether to use the pedals */
uint8_t use_pedals;
/* bool whether to use the shifter */
uint8_t use_shifter;
/* size of median filter to filter pedal values */
uint8_t pedal_median_size;
/* size of median filter to filter shifter values */
uint8_t shifter_median_size;
/* only meaningful if pedals_auto_calib is 0 */
uint16_t gasMin, gasMax, brakeMin, brakeMax, clutchMin, clutchMax;
/* defines the neutral zone of the y shifter hysteresis */
uint16_t shifter_y_neutralMin, shifter_y_neutralMax;
/* defines the 246R zone of the y shifter hysteresis, should be less than shifter_y_neutralMin */
uint16_t shifter_y_246R_gearZone;
/* defines the 135 zone of the y shifter hysteresis, should be greater than shifter_y_neutralMax */
uint16_t shifter_y_135_gearZone;
/* threshold for gears 1 and 2 of the x shifter, should be less than shifter_x_56 */
uint16_t shifter_x_12;
/* threshold for gears 5,6 and R of the x shifter, should be greater than shifter_x_12 */
uint16_t shifter_x_56;
} CalibData;
/* this structure holds the calibration data stored in EEPROM */
typedef struct calibration
{
/* the actual calibration struct */
CalibData data;
/* CRC checksum */
uint32_t crc;
} Calibration;
static Calibration calibDefault = {
{
CALIB_DATA_MAGIC_NUMBER,
1, /* pedals auto calib */
0, /* flags, everything on default */
1, /* use pedals */
1, /* use shifter */
0, /* pedals median size */
0, /* shifter median size */
0,0,0,0,0,0, /* calibrated pedal values */
1024/2 - 1024/10, 1024/2 + 1024/10, /* shifter_y_neutral */
1024/2 - 1024/3, 1024/2 + 1024/3, /* shifter_y_gearZone */
1024/3, 1024*2/3 /* shifter_x */
},
0 /* crc will be set when written to EEPROM */
};
static Calibration calibration = calibDefault;
unsigned long crc(uint8_t *buffer, uint16_t length) {
const unsigned long crc_table[16] = {
0x00000000, 0x1db71064, 0x3b6e20c8, 0x26d930ac,
0x76dc4190, 0x6b6b51f4, 0x4db26158, 0x5005713c,
0xedb88320, 0xf00f9344, 0xd6d6a3e8, 0xcb61b38c,
0x9b64c2b0, 0x86d3d2d4, 0xa00ae278, 0xbdbdf21c
};
unsigned long crc = ~0L;
for (int index = 0 ; index < length ; ++index) {
crc = crc_table[(crc ^ buffer[index]) & 0x0f] ^ (crc >> 4);
crc = crc_table[(crc ^ (buffer[index] >> 4)) & 0x0f] ^ (crc >> 4);
crc = ~crc;
}
return crc;
}
static int crcError = 0;
static int magicNumberError = 0;
static int lastCrcFromEEPROM = 0;
static int lastCrcFromContents = 0;
void loadEEPROM()
{
Calibration fromEEPROM;
EEPROM.get(0, fromEEPROM);
uint32_t eeprom_crc = crc((uint8_t*)&fromEEPROM.data, sizeof(fromEEPROM.data));
lastCrcFromEEPROM = fromEEPROM.crc;
lastCrcFromContents = eeprom_crc;
if( eeprom_crc == fromEEPROM.crc && fromEEPROM.data.calibID == CALIB_DATA_MAGIC_NUMBER )
{
calibration = fromEEPROM;
} else
{
crcError += eeprom_crc != fromEEPROM.crc;
magicNumberError += fromEEPROM.data.calibID != CALIB_DATA_MAGIC_NUMBER;
calibration = calibDefault;
}
}
// PEDAL CODE
typedef struct pedal {
typedef struct Pedal {
byte pin;
int min, max, cur, axis;
};
} Pedal;
typedef struct pedal Pedal;
static Pedal* gasPedal = 0;
static Pedal* brakePedal = 0;
static Pedal* clutchPedal = 0;
void* gasPedal;
void* brakePedal;
void* clutchPedal;
int axisValue(void* in) {
Pedal* input = (Pedal*)in;
int axisValue(struct Pedal* input) {
int physicalRange = input->max - input->min;
if (physicalRange == 0) {
@ -146,39 +213,21 @@ int axisValue(void* in) {
return result;
}
void processPedal(void* in) {
Pedal* input = (Pedal*)in;
void processPedal(struct Pedal* input, SignalFilter *flt, uint8_t filterSize)
{
input->cur = apply_filter(flt, filterSize, analogRead(input->pin));
input->cur = analogRead(input->pin);
#if !defined(STATIC_THRESHOLDS)
// calibrate, we want the highest this pedal has been
input->max = input->cur > input->max ? input->cur : input->max;
// same for lowest, but bottom out at current value rather than 0
input->min = input->min == 0 || input->cur < input->min ? input->cur : input->min;
#endif
if(calibration.data.pedals_auto_calib)
{
// calibrate, we want the highest this pedal has been
input->max = input->cur > input->max ? input->cur : input->max;
// same for lowest, but bottom out at current value rather than 0
input->min = input->min == 0 || input->cur < input->min ? input->cur : input->min;
}
input->axis = axisValue(input);
}
void describePedal(char* name, char* axisName, void* in) {
Pedal* input = (Pedal*)in;
Serial.print("\nPIN: ");
Serial.print(input->pin);
Serial.print(" ");
Serial.print(name);
Serial.print(": ");
Serial.print(input->cur);
Serial.print(" MIN: ");
Serial.print(input->min);
Serial.print(" MAX: ");
Serial.print(input->max);
Serial.print(" ");
Serial.print(axisName);
Serial.print(" VALUE: ");
Serial.print(input->axis);
}
void setXAxis(void* in) {
Pedal* input = (Pedal*)in;
G27.setXAxis(input->axis);
@ -194,14 +243,6 @@ void setZAxis(void* in) {
G27.setZAxis(input->axis);
}
void pedalColor(void* inGas, void* inBrake, void* inClutch){
Pedal* gas = (Pedal*)inGas;
Pedal* brake = (Pedal*)inBrake;
Pedal* clutch = (Pedal*)inClutch;
setColor(brake->axis + MAX_AXIS, gas->axis + MAX_AXIS, clutch->axis + MAX_AXIS);
}
// SHIFTER CODE
int buttonTable[] = {
// first four are unused
@ -229,25 +270,12 @@ void getButtonStates(int *ret) {
waitForSignalToSettle();
digitalWrite(SHIFTER_MODE_PIN, HIGH); // Switch to serial mode: one data bit is output on each clock falling edge
#if defined(DEBUG_SHIFTER)
Serial.print("\nBUTTON STATES:");
#endif
for(int i = 0; i < 16; ++i) { // Iteration over both 8 bit registers
digitalWrite(SHIFTER_CLOCK_PIN, LOW); // Generate clock falling edge
waitForSignalToSettle();
ret[i] = digitalRead(SHIFTER_DATA_PIN);
#if defined(DEBUG_SHIFTER)
if (!(i % 4)) Serial.print("\n");
Serial.print(" button");
if (i < 10) Serial.print(0);
Serial.print(i);
Serial.print(" = ");
Serial.print(ret[i]);
#endif
digitalWrite(SHIFTER_CLOCK_PIN, HIGH); // Generate clock rising edge
waitForSignalToSettle();
}
@ -259,32 +287,93 @@ void getShifterPosition(int *ret) {
}
int getCurrentGear(int shifterPosition[], int btns[]) {
int gear = 0; // default to neutral
static int gear = 0; // default to neutral
int x = shifterPosition[0], y = shifterPosition[1];
if (y < calibration.data.shifter_y_neutralMax && y > calibration.data.shifter_y_neutralMin)
{
gear = 0;
}
if (x < SHIFTER_XAXIS_12) // Shifter on the left?
if (y > calibration.data.shifter_y_135_gearZone )
{
if( x <= calibration.data.shifter_x_12 )
{
if (y > SHIFTER_YAXIS_135) gear = 1; // 1st gear
if (y < SHIFTER_YAXIS_246) gear = 2; // 2nd gear
}
else if (x > SHIFTER_XAXIS_56) // Shifter on the right?
if( gear != 3 ) /* avoid toggles between neighboring gears */
{
gear = 1;
}
} else if ( x >= calibration.data.shifter_x_56 )
{
if (y > SHIFTER_YAXIS_135) gear = 5; // 5th gear
if (y < SHIFTER_YAXIS_246) gear = 6; // 6th gear
}
else // Shifter is in the middle
if( gear != 3 ) /* avoid toggles between neighboring gears */
{
gear = 5;
}
} else
{
if (y > SHIFTER_YAXIS_135) gear = 3; // 3rd gear
if (y < SHIFTER_YAXIS_246) gear = 4; // 4th gear
if( gear != 1 && gear != 5 ) /* avoid toggles between neighboring gears */
{
gear = 3;
}
}
if (gear != 6) btns[BUTTON_REVERSE] = 0; // Reverse gear is allowed only on 6th gear position
if (btns[BUTTON_REVERSE] == 1) gear = 7; // Reverse is 7th gear (for the sake of argument)
}
if (y < calibration.data.shifter_y_246R_gearZone )
{
if( x <= calibration.data.shifter_x_12 )
{
if( gear != 4 ) /* avoid toggles between neighboring gears */
{
gear = 2;
}
} else if ( x >= calibration.data.shifter_x_56 )
{
uint8_t reverse = (calibration.data.flags & FLAG_REVERSE_RIGHT_RED) ? btns[BUTTON_RED_RIGHT] : btns[BUTTON_REVERSE];
if(reverse)
{
if( gear != 4 && gear != 6 ) /* avoid toggles between neighboring gears */
{
gear = 7;
}
} else
{
if( gear != 4 && gear != 7) /* avoid toggles between neighboring gears */
{
gear = 6;
}
}
} else
{
if( gear != 2 && gear != 6 && gear != 7) /* avoid toggles between neighboring gears */
{
gear = 4;
}
}
}
// reset button if we are not in reverse gear (this was there in original code, not sure if it's needed)
if (gear != 7) btns[BUTTON_REVERSE] = 0;
return gear;
}
typedef struct DebugStruct
{
uint8_t sizeInBytes;
uint8_t calibButton;
int16_t axisValues[5];
CalibData calib;
uint16_t numCrcErrors;
uint16_t numMagicNumErrors;
uint32_t profiling[4];
uint16_t out_axis[3];
uint32_t out_buttons;
} DebugStruct;
static DebugStruct debug = {0};
void setButtonStates(int buttons[], int gear) {
// SHIFTER CODE
debug.out_buttons = 0;
// release virtual buttons for all gears
for (byte i = 0; i < 7; ++i) {
G27.setButton(i, LOW);
@ -292,72 +381,187 @@ void setButtonStates(int buttons[], int gear) {
if (gear > 0) {
G27.setButton(gear - 1, HIGH);
debug.out_buttons |= 1 << (gear - 1);
}
for (byte i = BUTTON_RED_CENTERRIGHT; i <= BUTTON_DPAD_TOP; ++i) {
G27.setButton(buttonTable[i], buttons[i]);
if( buttons[i] )
{
debug.out_buttons |= 1 << buttonTable[i];
}
}
}
void describeButtonStates(int buttons[], int shifterPosition[], int gear) {
Serial.print("\nSHIFTER X: ");
Serial.print(shifterPosition[0]);
Serial.print(" Y: ");
Serial.print(shifterPosition[1]);
#define CALIB_MIN(calibValue, curValue, minValue) if( calibValue < 0 || curValue < minValue ) { calibValue = minValue = curValue; }
#define CALIB_MAX(calibValue, curValue, maxValue) if( calibValue < 0 || curValue > maxValue ) { calibValue = maxValue = curValue; }
#define CALIB_RANGE(calibValue, curValue, minValue, maxValue) \
{ if( calibValue < 0 ) { calibValue = minValue = maxValue = curValue; } \
CALIB_MIN(calibValue, curValue, minValue); \
CALIB_MAX(calibValue, curValue, maxValue); }
Serial.print(" GEAR: ");
Serial.print(gear);
Serial.print(" REVERSE: ");
Serial.print(buttons[BUTTON_REVERSE]);
void calib(struct Pedal *gas, Pedal *brake, Pedal *clutch, int shifter_X, int shifter_Y, int calibButton)
{
static uint8_t printMode = 0;
static enum COMMANDS {
IDLE = 'i',
RECORD_Y_SHIFTER_135 = 'u',
RECORD_Y_SHIFTER_246 = 'b',
RECORD_Y_SHIFTER_NEUTRAL = 'n',
RECORD_X_SHIFTER_12 = 'l',
RECORD_X_SHIFTER_56 = 'r',
RECORD_GAS = 'G',
RECORD_BRAKE = 'B',
RECORD_CLUTCH = 'C',
SET_PEDAL_AUTO_CALIBRATE = 'P',
RESET_PEDAL_AUTO_CALIBRATE = 'p',
SET_PEDALS_ENABLED = 'E',
RESET_PEDALS_ENABLED = 'e',
SET_SHIFTER_ENABLED = 'S',
RESET_SHIFTER_ENABLED = 's',
SET_GAS_INVERTED = 'Y',
RESET_GAS_INVERTED = 'y',
SET_BRAKE_INVERTED = 'X',
RESET_BRAKE_INVERTED = 'x',
SET_CLUTCH_INVERTED = 'Z',
RESET_CLUTCH_INVERTED = 'z',
SET_REVERSE_RIGHT_RED = 'Q',
RESET_REVERSE_RIGHT_RED = 'q',
SET_PEDAL_FILTSIZE_OFF = '0',
SET_PEDAL_FILTSIZE_3 = '3',
SET_PEDAL_FILTSIZE_5 = '5',
SET_PEDAL_FILTSIZE_7 = '7',
SET_PEDAL_FILTSIZE_9 = '9',
SET_PEDAL_FILTSIZE_15 = 'f',
SET_SHIFTER_FILTSIZE_OFF = '1',
SET_SHIFTER_FILTSIZE_3 = '2',
SET_SHIFTER_FILTSIZE_5 = '4',
SET_SHIFTER_FILTSIZE_7 = '6',
SET_SHIFTER_FILTSIZE_9 = '8',
SET_SHIFTER_FILTSIZE_15 = 'F',
SET_PRINT_MODE = 'O',
RESET_PRINT_MODE = 'o',
STORE_CALIB = 'w',
DEFAULT_CALIB = 'c',
EEPROM_CALIB = 'U'
} currentMode = IDLE;
static int calibValue = -1;
if (Serial.available() > 0)
{
char rx_byte = Serial.read();
switch (rx_byte)
{
case IDLE:
case RECORD_Y_SHIFTER_135:
case RECORD_Y_SHIFTER_246:
case RECORD_Y_SHIFTER_NEUTRAL:
case RECORD_X_SHIFTER_12:
case RECORD_X_SHIFTER_56:
case RECORD_GAS:
case RECORD_BRAKE:
case RECORD_CLUTCH: calibValue = -1; currentMode = (COMMANDS)rx_byte; break;
case SET_PEDAL_AUTO_CALIBRATE: calibration.data.pedals_auto_calib = 1; break;
case RESET_PEDAL_AUTO_CALIBRATE: calibration.data.pedals_auto_calib= 0; break;
case SET_PEDALS_ENABLED: calibration.data.use_pedals = 1; break;
case RESET_PEDALS_ENABLED: calibration.data.use_pedals = 0; break;
case SET_SHIFTER_ENABLED: calibration.data.use_shifter = 1; break;
case RESET_SHIFTER_ENABLED: calibration.data.use_shifter = 0; break;
case SET_GAS_INVERTED: calibration.data.flags |= FLAG_INVERT_GAS; break;
case RESET_GAS_INVERTED: calibration.data.flags &= ~FLAG_INVERT_GAS; break;
case SET_BRAKE_INVERTED: calibration.data.flags |= FLAG_INVERT_BRAKE; break;
case RESET_BRAKE_INVERTED: calibration.data.flags &= ~FLAG_INVERT_BRAKE; break;
case SET_CLUTCH_INVERTED: calibration.data.flags |= FLAG_INVERT_CLUTCH; break;
case RESET_CLUTCH_INVERTED: calibration.data.flags &= ~FLAG_INVERT_CLUTCH; break;
case SET_REVERSE_RIGHT_RED: calibration.data.flags |= FLAG_REVERSE_RIGHT_RED; break;
case RESET_REVERSE_RIGHT_RED: calibration.data.flags &= ~FLAG_REVERSE_RIGHT_RED; break;
case SET_PEDAL_FILTSIZE_OFF: calibration.data.pedal_median_size = 0; break;
case SET_PEDAL_FILTSIZE_3: calibration.data.pedal_median_size = 3; break;
case SET_PEDAL_FILTSIZE_5: calibration.data.pedal_median_size = 5; break;
case SET_PEDAL_FILTSIZE_7: calibration.data.pedal_median_size = 9; break;
case SET_PEDAL_FILTSIZE_9: calibration.data.pedal_median_size = 15; break;
case SET_PEDAL_FILTSIZE_15: calibration.data.pedal_median_size = 49; break;
case SET_SHIFTER_FILTSIZE_OFF: calibration.data.shifter_median_size = 0; break;
case SET_SHIFTER_FILTSIZE_3: calibration.data.shifter_median_size = 3; break;
case SET_SHIFTER_FILTSIZE_5: calibration.data.shifter_median_size = 5; break;
case SET_SHIFTER_FILTSIZE_7: calibration.data.shifter_median_size = 9; break;
case SET_SHIFTER_FILTSIZE_9: calibration.data.shifter_median_size = 15; break;
case SET_SHIFTER_FILTSIZE_15: calibration.data.shifter_median_size = 49; break;
case SET_PRINT_MODE: printMode = 1; break;
case RESET_PRINT_MODE: printMode = 0;; break;
Serial.print(" RED BUTTONS:");
if (buttons[BUTTON_RED_LEFT]) {
Serial.print(" 1");
case STORE_CALIB:
calibration.crc = crc((uint8_t*)&calibration.data, sizeof(calibration.data));
EEPROM.put(0, calibration);
break;
case DEFAULT_CALIB:
calibration = calibDefault;
break;
case EEPROM_CALIB:
loadEEPROM();
break;
}
}
if (buttons[BUTTON_RED_CENTERLEFT]) {
Serial.print(" 2");
if(printMode)
{
debug.sizeInBytes = sizeof(debug);
debug.calibButton = calibButton;
debug.axisValues[0] = shifter_X;
debug.axisValues[1] = shifter_Y;
debug.axisValues[2] = gas->cur;
debug.axisValues[3] = brake->cur;
debug.axisValues[4] = clutch->cur;
debug.calib = calibration.data;
debug.numCrcErrors = crcError;
debug.numMagicNumErrors = magicNumberError;
debug.profiling[3] = micros();
uint16_t s = sizeof(debug);
Serial.write((const byte*)&debug, sizeof(debug));
debug.profiling[0] = debug.profiling[3];
}
if (buttons[BUTTON_RED_CENTERLEFT]) {
Serial.print(" 3");
if(calibButton)
{
switch(currentMode)
{
case RECORD_Y_SHIFTER_135:
CALIB_MIN(calibValue, shifter_Y, calibration.data.shifter_y_135_gearZone);
break;
case RECORD_Y_SHIFTER_246:
CALIB_MAX(calibValue, shifter_Y, calibration.data.shifter_y_246R_gearZone);
break;
case RECORD_Y_SHIFTER_NEUTRAL:
CALIB_RANGE(calibValue, shifter_Y, calibration.data.shifter_y_neutralMin, calibration.data.shifter_y_neutralMax);
break;
case RECORD_X_SHIFTER_12:
CALIB_MAX(calibValue, shifter_X, calibration.data.shifter_x_12);
break;
case RECORD_X_SHIFTER_56:
CALIB_MIN(calibValue, shifter_X, calibration.data.shifter_x_56);
break;
}
}
if (buttons[BUTTON_RED_RIGHT]) {
Serial.print(" 4");
}
Serial.print(" BLACK BUTTONS:");
if (buttons[BUTTON_BLACK_LEFT]) {
Serial.print(" LEFT");
}
if (buttons[BUTTON_BLACK_TOP]) {
Serial.print(" TOP");
}
if (buttons[BUTTON_BLACK_BOTTOM]) {
Serial.print(" BOTTOM");
}
if (buttons[BUTTON_BLACK_RIGHT]) {
Serial.print(" RIGHT");
}
Serial.print(" D-PAD:");
if (buttons[BUTTON_DPAD_LEFT]) {
Serial.print(" LEFT");
}
if (buttons[BUTTON_DPAD_TOP]) {
Serial.print(" UP");
}
if (buttons[BUTTON_DPAD_BOTTOM]) {
Serial.print(" DOWN");
}
if (buttons[BUTTON_DPAD_RIGHT]) {
Serial.print(" RIGHT");
switch(currentMode)
{
case RECORD_GAS:
CALIB_RANGE(calibValue, gas->cur, calibration.data.gasMin, calibration.data.gasMax);
break;
case RECORD_BRAKE:
CALIB_RANGE(calibValue, brake->cur, calibration.data.brakeMin, calibration.data.brakeMax);
break;
case RECORD_CLUTCH:
CALIB_RANGE(calibValue, clutch->cur, calibration.data.clutchMin, calibration.data.clutchMax);
break;
}
}
void setup() {
Serial.begin(38400);
#if !defined(DEBUG_PEDALS) && !defined(DEBUG_SHIFTER)
G27.begin(false);
#endif
Serial.begin(460800);
// lights
pinMode(RED_PIN, OUTPUT);
@ -380,73 +584,89 @@ void setup() {
brake->pin = BRAKE_PIN;
clutch->pin = CLUTCH_PIN;
#if defined(STATIC_THRESHOLDS)
gas->min = MIN_GAS;
gas->max = MAX_GAS;
loadEEPROM();
brake->min = MIN_BRAKE;
brake->max = MAX_BRAKE;
if( !calibration.data.pedals_auto_calib )
{
gas->min = calibration.data.gasMin;
gas->max = calibration.data.gasMax;
clutch->min = MIN_CLUTCH;
clutch->max = MAX_CLUTCH;
#endif
brake->min = calibration.data.brakeMin;
brake->max = calibration.data.brakeMax;
clutch->min = calibration.data.clutchMin;
clutch->max = calibration.data.clutchMax;
} else
{
gas->min = brake->min = clutch->min = MAX_AXIS;
gas->max = brake->max = clutch->max = 0;
}
gasPedal = gas;
brakePedal = brake;
clutchPedal = clutch;
}
SignalFilter signalFilters[5];
void loop() {
debug.profiling[2] = micros();
// pedals
processPedal(gasPedal);
processPedal(brakePedal);
processPedal(clutchPedal);
processPedal(gasPedal, &signalFilters[0], calibration.data.pedal_median_size);
processPedal(brakePedal, &signalFilters[1], calibration.data.pedal_median_size);
processPedal(clutchPedal, &signalFilters[2], calibration.data.pedal_median_size);
#if defined(INVERT_BRAKE)
Pedal* brake = (Pedal*)brakePedal;
brake->axis = map(brake->axis, 0, MAX_AXIS, MAX_AXIS, 0);
#endif
#if defined(DEBUG_PEDALS)
describePedal("GAS", "X", gasPedal);
describePedal("BRAKE", "Y", brakePedal);
describePedal("CLUTCH", "Z", clutchPedal);
#elif defined(USE_PEDALS)
setXAxis(gasPedal);
setYAxis(brakePedal);
setZAxis(clutchPedal);
#endif
#if defined(PEDAL_COLORS)
pedalColor(gasPedal, brakePedal, clutchPedal);
#endif
if(calibration.data.flags & FLAG_INVERT_GAS )
{
Pedal* gas = (Pedal*)gasPedal;
gas->axis = map(gas->axis, 0, MAX_AXIS, MAX_AXIS, 0);
}
if(calibration.data.flags & FLAG_INVERT_BRAKE )
{
Pedal* brake = (Pedal*)brakePedal;
brake->axis = map(brake->axis, 0, MAX_AXIS, MAX_AXIS, 0);
}
if(calibration.data.flags & FLAG_INVERT_CLUTCH )
{
Pedal* clutch = (Pedal*)clutchPedal;
clutch->axis = map(clutch->axis, 0, MAX_AXIS, MAX_AXIS, 0);
}
if(calibration.data.use_pedals)
{
setXAxis(gasPedal);
setYAxis(brakePedal);
setZAxis(clutchPedal);
debug.out_axis[0] = gasPedal->axis;
debug.out_axis[1] = brakePedal->axis;
debug.out_axis[2] = clutchPedal->axis;
} else
{
debug.out_axis[0] = 0xffff;
debug.out_axis[1] = 0xffff;
debug.out_axis[2] = 0xffff;
}
// shifter
int buttonStates[16];
getButtonStates(buttonStates);
int shifterPosition[2];
getShifterPosition(shifterPosition);
shifterPosition[0] = apply_filter(&signalFilters[3], calibration.data.shifter_median_size, shifterPosition[0]);
shifterPosition[1] = apply_filter(&signalFilters[4], calibration.data.shifter_median_size, shifterPosition[1]);
int gear = getCurrentGear(shifterPosition, buttonStates);
#if defined(DEBUG_SHIFTER)
describeButtonStates(buttonStates, shifterPosition, gear);
#elif defined(USE_SHIFTER)
setButtonStates(buttonStates, gear);
#endif
if(calibration.data.use_shifter)
{
setButtonStates(buttonStates, gear);
} else
{
debug.out_buttons = 0xffffffff;
}
#if !defined(DEBUG_SHIFTER) || !defined(DEBUG_PEDALS)
calib(gasPedal, brakePedal, clutchPedal, shifterPosition[0], shifterPosition[1], buttonStates[BUTTON_RED_LEFT]);
G27.sendState();
#endif
#if defined(DEBUG_PEDALS) || defined(DEBUG_SHIFTER)
Serial.print("\n----------------------------------------------------------------------------");
// slow the output down a bit
delay(500);
#endif
}
void setColor(int red, int green, int blue) {
analogWrite(RED_PIN, red);
analogWrite(GREEN_PIN, green);
analogWrite(BLUE_PIN, blue);
debug.profiling[1] = micros();
}

View File

@ -0,0 +1,100 @@
#include "Filter.h"
uint16_t apply_filter(SignalFilter *filter, uint8_t filterSize, uint16_t value)
{
uint8_t i, idxMaxInLow = 0, idxMinInHigh = 0;
uint8_t filterSizeDiv2;
uint16_t v, vMaxInLow, vMinInHigh;
if( filterSize <= 1 )
{
return value;
}
if( filterSize > MAX_FILTER_SIZE )
{
filterSize = MAX_FILTER_SIZE;
}
filterSize = (filterSize/2)*2+1; /* assert sanity of filter size */
filterSizeDiv2 = filterSize/2;
if( filterSize != filter->filterSize )
{
/* initialize filter */
filter->filterSize = filterSize;
filter->ringBufferIdx = 0;
filter->medianIdx = filterSizeDiv2;
filter->idxMaxInLow = 0;
filter->idxMinInHigh = 0;
for(i = 0; i < filterSize; i++)
{
filter->values[i] = value;
if( i < filterSizeDiv2 )
{
filter->lowIdx[i] = i;
}
if( i > filterSizeDiv2 )
{
filter->highIdx[i-filterSizeDiv2-1] = i;
}
}
}
/* remove the last item */
v = filter->values[filter->ringBufferIdx];
if( v <= filter->values[filter->medianIdx] || filter->idxMaxInLow == 255 )
{
for(i = 0; i < filterSizeDiv2; i++)
{
if( filter->ringBufferIdx == filter->lowIdx[i] )
{
filter->lowIdx[i] = filter->medianIdx;
}
if( filter->values[filter->lowIdx[i]] > filter->values[filter->lowIdx[idxMaxInLow]] )
{
idxMaxInLow = i;
}
}
filter->idxMaxInLow = idxMaxInLow;
}
idxMaxInLow = filter->idxMaxInLow;
if( v >= filter->values[filter->medianIdx] || filter->idxMinInHigh == 255 )
{
for(i = 0; i < filterSizeDiv2; i++)
{
if( filter->ringBufferIdx == filter->highIdx[i] )
{
filter->highIdx[i] = filter->medianIdx;
}
if( filter->values[filter->highIdx[i]] < filter->values[filter->highIdx[idxMinInHigh]] )
{
idxMinInHigh = i;
}
}
filter->idxMinInHigh = idxMinInHigh;
}
idxMinInHigh = filter->idxMinInHigh;
/* the median index is now free */
vMaxInLow = filter->values[filter->lowIdx[idxMaxInLow]];
vMinInHigh = filter->values[filter->highIdx[idxMinInHigh]];
filter->values[filter->ringBufferIdx] = value;
if( value < vMaxInLow )
{
uint8_t tmp = filter->lowIdx[idxMaxInLow];
filter->lowIdx[idxMaxInLow] = filter->ringBufferIdx;
filter->medianIdx = tmp;
filter->idxMaxInLow = 255;
} else if( value > vMinInHigh )
{
uint8_t tmp = filter->highIdx[idxMinInHigh];
filter->highIdx[idxMinInHigh] = filter->ringBufferIdx;
filter->medianIdx = tmp;
filter->idxMinInHigh = 255;
} else
{
filter->medianIdx = filter->ringBufferIdx;
}
filter->ringBufferIdx++;
if( filter->ringBufferIdx >= filterSize )
{
filter->ringBufferIdx = 0;
}
return filter->values[filter->medianIdx];
}

View File

@ -0,0 +1,22 @@
#ifndef MEDIAN_FILTER_H
#define MEDIAN_FILTER_H
#include <stdint.h>
#define MAX_FILTER_SIZE 49
typedef struct SignalFilter
{
uint16_t values[MAX_FILTER_SIZE];
uint8_t ringBufferIdx;
uint8_t filterSize;
uint8_t lowIdx[MAX_FILTER_SIZE/2];
uint8_t highIdx[MAX_FILTER_SIZE/2];
uint8_t idxMaxInLow;
uint8_t idxMinInHigh;
uint8_t medianIdx;
} SignalFilter;
uint16_t apply_filter(SignalFilter *filter, uint8_t filterSize, uint16_t value);
#endif

View File

@ -0,0 +1,95 @@
import ctypes as ct
MAX_FILTER_SIZE = 49
class SignalFilter(ct.Structure):
_fields_ = [
("values", ct.c_uint16*MAX_FILTER_SIZE),
("ringBufferIdx", ct.c_uint8),
("filterSize", ct.c_uint8),
("lowIdx", ct.c_uint8*(MAX_FILTER_SIZE//2)),
("highIdx", ct.c_uint8*(MAX_FILTER_SIZE//2)),
("idxMinInHigh", ct.c_uint8),
("idxMaxInLow", ct.c_uint8),
]
def apply_filter(filter, filterSize, value):
idxMaxInLow = 0
idxMinInHigh = 0
if filterSize <= 1:
return value
if filterSize > MAX_FILTER_SIZE:
filterSize = MAX_FILTER_SIZE
filterSize = (filterSize//2)*2+1 #/* assert sanity of filter size */
filterSizeDiv2 = filterSize//2
if filterSize != filter.filterSize:
#/* initialize filter */
filter.filterSize = filterSize
filter.ringBufferIdx = 0
filter.medianIdx = filterSizeDiv2
filter.idxMaxInLow = 0
filter.idxMinInHigh = 0
for i in range(filterSize):
filter.values[i] = value
if i < filterSizeDiv2:
filter.lowIdx[i] = i
if i > filterSizeDiv2:
filter.highIdx[i-filterSizeDiv2-1] = i
# /* remove the last item */
print(list(filter.lowIdx)[:filterSizeDiv2], list(filter.highIdx)[:filterSizeDiv2], filter.medianIdx, list(filter.values)[:filterSize], filter.ringBufferIdx, filter.idxMaxInLow, filter.idxMinInHigh, value)
v = filter.values[filter.ringBufferIdx]
if v <= filter.values[filter.medianIdx] or filter.idxMaxInLow == 255:
for i in range(filterSizeDiv2): #for(i = 0; i < filterSizeDiv2; i++)
if filter.ringBufferIdx == filter.lowIdx[i]:
filter.lowIdx[i] = filter.medianIdx
if filter.values[filter.lowIdx[i]] > filter.values[filter.lowIdx[idxMaxInLow]]:
idxMaxInLow = i
filter.idxMaxInLow = idxMaxInLow
idxMaxInLow = filter.idxMaxInLow
if v >= filter.values[filter.medianIdx] or filter.idxMinInHigh == 255:
for i in range(filterSizeDiv2): #for(i = 0; i < filterSizeDiv2; i++)
if filter.ringBufferIdx == filter.highIdx[i]:
filter.highIdx[i] = filter.medianIdx
if filter.values[filter.highIdx[i]] < filter.values[filter.highIdx[idxMinInHigh]]:
idxMinInHigh = i
filter.idxMinInHigh = idxMinInHigh
idxMinInHigh = filter.idxMinInHigh
#/* the median index is now free */
vMaxInLow = filter.values[filter.lowIdx[idxMaxInLow]]
vMinInHigh = filter.values[filter.highIdx[idxMinInHigh]]
filter.values[filter.ringBufferIdx] = value
if value < vMaxInLow:
tmp = filter.lowIdx[idxMaxInLow]
filter.lowIdx[idxMaxInLow] = filter.ringBufferIdx
filter.medianIdx = tmp
filter.idxMaxInLow = 255
elif value > vMinInHigh:
tmp = filter.highIdx[idxMinInHigh]
filter.highIdx[idxMinInHigh] = filter.ringBufferIdx
filter.medianIdx = tmp
filter.idxMinInHigh = 255
else:
filter.medianIdx = filter.ringBufferIdx
print(list(filter.lowIdx)[:filterSizeDiv2], list(filter.highIdx)[:filterSizeDiv2], filter.medianIdx, list(filter.values)[:filterSize], filter.ringBufferIdx, filter.idxMaxInLow, filter.idxMinInHigh, value)
assert sorted([filter.medianIdx] + list(filter.lowIdx)[:filterSizeDiv2] + list(filter.highIdx)[:filterSizeDiv2]) == sorted(np.arange(filterSize, dtype=np.uint8))
#low = np.array(filter.values)[np.array(filter.lowIdx[:filterSizeDiv2])]
#high = np.array(filter.values)[np.array(filter.highIdx[:filterSizeDiv2])]
#assert np.all(low[filter.idxMaxInLow] >= low)
#assert np.all(high[filter.idxMinInHigh] <= high)
filter.ringBufferIdx += 1
if filter.ringBufferIdx >= filterSize:
filter.ringBufferIdx = 0
return filter.values[filter.medianIdx]
if __name__ == "__main__":
import numpy as np
for filterSize in [0, 3, 5, 9, 15, 49]:
filter = SignalFilter()
numbers = np.random.randint(0, 1023, size=10000, dtype=np.uint16)
for i,n in enumerate(numbers):
m = apply_filter(filter, filterSize, n)
if i >= filterSize:
if filterSize > 0:
assert m == np.median(numbers[i-filterSize+1:i+1])
else:
assert m == n

View File

@ -0,0 +1,480 @@
import struct
import ctypes as ct
import sys
import time
import traceback
from PySide2.QtCore import QRectF, Qt, QObject, QThread, Signal, QMutex, QMutexLocker, QTimer, QSignalBlocker
from PySide2.QtGui import QBrush, QPen, QColor
from PySide2.QtWidgets import (QApplication, QWidget, QGroupBox, QLabel, QGridLayout, QVBoxLayout, QRadioButton,
QCheckBox, QComboBox, QPushButton)
import serial
from serial.tools import list_ports
import pyqtgraph as pg
import numpy as np
# all joystick inputs seem to have issues with the more than 16 buttons present on G27 shifter.
# therefore, we just use inputs to connect to the device and all other stuff is transferred via serial
# port.
import inputs
FLAG_INVERT_BRAKE = 0x1
FLAG_INVERT_GAS = 0x2
FLAG_INVERT_CLUTCH = 0x4
FLAG_REVERSE_RIGHT_RED = 0x8
# following structures need to be synchronized with Arduino C Code
class CalibData(ct.Structure):
_pack_ = 2
_fields_ = [
("calibID", ct.c_uint32),
("pedals_auto_calib", ct.c_uint8),
("flags", ct.c_uint8),
("use_pedals", ct.c_uint8),
("use_shifter", ct.c_uint8),
("pedals_median_size", ct.c_uint8),
("shifter_median_size", ct.c_uint8),
("gasMin", ct.c_uint16),
("gasMax", ct.c_uint16),
("brakeMin", ct.c_uint16),
("brakeMax", ct.c_uint16),
("clutchMin", ct.c_uint16),
("clutchMax", ct.c_uint16),
("shifter_y_neutralMin", ct.c_uint16),
("shifter_y_neutralMax", ct.c_uint16),
("shifter_y_246R_gearZone", ct.c_uint16),
("shifter_y_135_gearZone", ct.c_uint16),
("shifter_x_12", ct.c_uint16),
("shifter_x_56", ct.c_uint16),
]
class DebugStruct(ct.Structure):
_pack_ = 2
_fields_ = [
("sizeInBytes", ct.c_uint8),
("calibButton", ct.c_uint8),
("axisValues", ct.c_uint16*5),
("calib", CalibData),
("numCrcErrors", ct.c_uint16),
("numMagicNumErrors", ct.c_uint16),
("profiling", ct.c_uint32*4),
("out_axis", ct.c_uint16*3),
("out_buttons", ct.c_uint32)
]
class Values:
def __init__(self, history_time_s = 1.0):
self.mutex = QMutex()
self.history_time_ns = np.int64(history_time_s*1e9)
self.times = np.zeros((0,), dtype=np.int64)
self.gas = np.zeros((0,), dtype=np.uint16)
self.brake = np.zeros((0,), dtype=np.uint16)
self.clutch = np.zeros((0,), dtype=np.uint16)
self.sx = np.zeros((0,), dtype=np.uint16)
self.sy = np.zeros((0,), dtype=np.uint16)
self.calib = CalibData()
def update(self, dbg): # options
ml = QMutexLocker(self.mutex)
self.times = np.append(self.times, [time.perf_counter_ns()])
v = self.times >= self.times[-1] - self.history_time_ns
self.times = self.times[v]
self.gas = np.append(self.gas, [dbg.axisValues[2]])[v]
self.brake = np.append(self.brake, [dbg.axisValues[3]])[v]
self.clutch = np.append(self.clutch, [dbg.axisValues[4]])[v]
self.sx = np.append(self.sx, [dbg.axisValues[0]])[v]
self.sy = np.append(self.sy, [dbg.axisValues[1]])[v]
self.calib = dbg.calib
def add(self, other):
ml = QMutexLocker(other.mutex)
ml2 = QMutexLocker(self.mutex)
self.times = np.append(self.times, other.times)
self.gas = np.append(self.gas, other.gas)
self.brake = np.append(self.brake, other.brake)
self.clutch = np.append(self.clutch, other.clutch)
self.sx = np.append(self.sx, other.sx)
self.sy = np.append(self.sy, other.sy)
self.calib = other.calib
other.times = np.resize(other.times, (0,))
other.gas = np.resize(other.gas, (0,))
other.brake = np.resize(other.brake, (0,))
other.clutch = np.resize(other.clutch, (0,))
other.sx = np.resize(other.sx, (0,))
other.sy = np.resize(other.sy, (0,))
v = self.times >= self.times[-1] - self.history_time_ns
self.times = self.times[v]
self.gas = self.gas[v]
self.brake = self.brake[v]
self.clutch = self.clutch[v]
self.sx = self.sx[v]
self.sy = self.sy[v]
class G27CalibGui(QWidget):
sendModeCmd = Signal(object)
setObjectCmd = Signal(object)
def __init__(self):
super().__init__()
self.tl_layout = QGridLayout(self)
rbtn_values = QGroupBox("Calibration Values", self)
vbox = QVBoxLayout(rbtn_values)
self.mode_btns = [
QRadioButton(s, rbtn_values) for s in
["Idle", "Shifter neutral zone", "Shifter 135 zone", "Shifter 246R zone", "Shifter 12 zone",
"Shifter 56 zone", "Gas pedal (if not auto-calibrated)", "Brake pedal (if not auto-calibrated)",
"Clutch pedal (if not auto-calibrated)"]]
self.mode_btns[0].setChecked(True)
self.mode_cmds = [b"i", b"n", b"u", b"b", b"l", b"r", b"G", b"B", b"C"]
self.help_cmds = [
"First decide about the filtering needed (in middle panel). If there is a lot of noise in the signal, "
"maybe because of old pots, you might want to use a multi-sample median filter. Note that this filter "
"introduces some delay, so you want to keep the numbers reasonable low. Afterwards, iterate through "
"the calibration values in the left panel.\n\n"
"When you're done, you can save the calibration in the right panel to the EEPROM.",
"Put the shifter in neutral position. Afterwards, keep the left red button pressed and move the "
"shifter while still in neutral. When reaching the calibrated area, the gear will be set to neutral.",
"Put the shifter in 3rd gear. Afterwards keep the left red button pressed and move the shifter "
"while still in 3rd gear. When done, release the red button. Optionally repeat for gears 1st and 5th.",
"Put the shifter in 4th gear. Afterwards keep the left red button pressed and move the shifter "
"while still in 4th gear. When done, release the red button. Optionally repeat for gears 2nd and 6th.",
"Put the shifter in 1st gear. Afterwards keep the left red button pressed and move the shifter "
"while still in 1st gear. When done, release the red button. Optionally repeat for 2nd gear.",
"Put the shifter in 5th gear. Afterwards keep the left red button pressed and move the shifter "
"while still in 5th gear. When done, release the red button. Optionally repeat for 6th gear.",
"Move the gas pedal multiple times between lowest and highest position.",
"Move the brake pedal multiple times between lowest and highest position.",
"Move the clutch pedal multiple times between lowest and highest position."
]
for b in self.mode_btns:
vbox.addWidget(b)
b.toggled.connect(self.modeChanged)
self.tl_layout.addWidget(rbtn_values, 0,0, 1, 1)
cbtn_options = QGroupBox("Options", self)
grid = QGridLayout(cbtn_options)
self.option_btns = [
QCheckBox(s, cbtn_options) for s in
["Enable pedal auto-calibration",
"Invert gas/throttle pedal",
"Invert brake pedal",
"Invert clutch pedal",
"Use right red button for reverse instead pushing the gear",
"Enable pedals",
"Enable shifter"]
] + [QComboBox(), QComboBox()]
self.option_cmds = [(b"p", b"P"),
(b"y", b"Y"),
(b"x", b"X"),
(b"z", b"Z"),
(b"q", b"Q"),
(b"e", b"E"),
(b"s", b"S"),
(b"0", b"3", b"5", b"7", b"9", b"f"),
(b"1", b"2", b"4", b"6", b"8", b"F"),
]
for (idx, name) in [(-2, "pedals"), (-1, "shifter")]:
for size in [0,3,5,9,15,49]:
self.option_btns[idx].addItem("off" if size == 0 else (str(size) + "-median"), size)
for i,b in enumerate(self.option_btns):
n_checkboxes = len(self.option_cmds) - 2
if i < n_checkboxes:
grid.addWidget(b, i, 0, 1, 2)
else:
grid.addWidget(QLabel("Pedal filter" if i == n_checkboxes else "Shifter filter", parent=self), i, 0)
grid.addWidget(b, i, 1)
if isinstance(b, QCheckBox):
b.toggled.connect(self.optionChanged)
else:
b.currentIndexChanged.connect(self.optionChanged)
self.tl_layout.addWidget(cbtn_options, 0, 1, 1, 1)
cbtn_persistent = QGroupBox("Persistency")
self.persistent_btns = [QPushButton(s, parent=self) for s in ["Save to EEPROM", "Load from EEPROM", "Defaults"]]
self.persistent_cmds = [b"w", b"U", b"c"]
vbox = QVBoxLayout(cbtn_persistent)
for i,b in enumerate(self.persistent_btns):
vbox.addWidget(b)
b.clicked.connect(self.persistent_cmd)
self.tl_layout.addWidget(cbtn_persistent, 0, 2, 1, 1)
self.helpArea = QLabel(self.help_cmds[0], self)
self.helpArea.setWordWrap(True)
f = self.helpArea.font()
f.setBold(True)
f.setPointSizeF(f.pointSizeF()*1.5)
self.helpArea.setFont(f)
self.tl_layout.addWidget(self.helpArea, 0, 3, 1, 1)
shifter_label = QLabel("Shifter")
self.tl_layout.addWidget(shifter_label, 1,2,1,2,alignment=Qt.AlignHCenter)
shifter_plot = pg.PlotWidget(parent=self)
shifter_plot.setMouseEnabled(x=False,y=False)
self.tl_layout.addWidget(shifter_plot, 2,2, 1,2)
shifter_plot.setRange(QRectF(0,0,1023,1023), padding=0)
pi = shifter_plot.getPlotItem()
self.shifter_neutral_plot = pg.LinearRegionItem(values = (400, 600), orientation='horizontal', movable=False,
brush = QBrush(QColor(200,50,50,50)),
pen=QPen(QColor(200,50,50)))
shifter_plot.addItem(self.shifter_neutral_plot)
self.shifter_135_plot = pg.LinearRegionItem(values = (800, 1024), orientation='horizontal', movable=False,
brush = QBrush(QColor(50,200,50,50)),
pen = QPen(QColor(50,200,50)))
shifter_plot.addItem(self.shifter_135_plot)
self.shifter_246_plot = pg.LinearRegionItem(values = (0, 200), orientation='horizontal', movable=False,
brush = QBrush(QColor(50,50,200,50)),
pen = QPen(QColor(50,50,200)))
shifter_plot.addItem(self.shifter_246_plot)
self.shifter_12_plot = pg.LinearRegionItem(values = (0, 200), orientation='vertical', movable=False,
brush = QBrush(QColor(50,200,200,50)),
pen = QPen(QColor(50,200,200)))
shifter_plot.addItem(self.shifter_12_plot)
self.shifter_56_plot = pg.LinearRegionItem(values = (800, 1024), orientation='vertical', movable=False,
brush = QBrush(QColor(200,200,50,50)),
pen = QPen(QColor(200,200,50)))
shifter_plot.addItem(self.shifter_56_plot)
self.shifter_pos_plot = pi.plot(x=[512], y=[512], symbol='+', pen=None)
pedals_label = QLabel("Pedals")
self.tl_layout.addWidget(pedals_label, 1,0,1,2,alignment=Qt.AlignHCenter)
pedals_plot = pg.PlotWidget(parent=self)
pedals_plot.setMouseEnabled(x=False,y=False)
pedals_plot.setRange(QRectF(-1,0,4,1023), padding=0)
pedals_plot.getAxis('bottom').setTicks([[(0.0,"Gas/Throttle"), (1.0,"Brake"), (2.0,"Clutch")],[]])
self.tl_layout.addWidget(pedals_plot,2,0,1,2)
self.pedals_pos_plot = pg.BarGraphItem(x=np.arange(3), height=[512,512,512], width=0.5)
self.pedals_gas_calib_plot = pedals_plot.getPlotItem().plot(x=[0], y=[0], pen=QPen(QColor(255,0,0),width=0.1), brush=QBrush(Qt.NoBrush))
self.pedals_brake_calib_plot = pedals_plot.getPlotItem().plot(x=[0], y=[0], pen=QPen(QColor(0,255,0)))
self.pedals_clutch_calib_plot = pedals_plot.getPlotItem().plot(x=[0], y=[0], pen=QPen(QColor(0,0,255)))
self.pedals_pos_plot2 = pedals_plot.getPlotItem().plot(x=[], y=[], symbol='+', pen=None)
pedals_plot.addItem(self.pedals_pos_plot)
self.status_line = QLabel(self)
self.tl_layout.addWidget(self.status_line,3,0,1,2)
self.status_line2 = QLabel(self)
self.tl_layout.addWidget(self.status_line2,3,2,1,2)
self.currentVals = Values()
def modeChanged(self, toggled):
if toggled:
o = self.sender()
idx = self.mode_btns.index(o)
self.sendModeCmd.emit(self.mode_cmds[idx])
self.helpArea.setText(self.help_cmds[idx])
def optionChanged(self, option):
o = self.sender()
idx = self.option_btns.index(o)
self.sendModeCmd.emit(self.option_cmds[idx][int(option)])
def persistent_cmd(self):
o = self.sender()
idx = self.persistent_btns.index(o)
self.sendModeCmd.emit(self.persistent_cmds[idx])
def newVals(self, values, dbg):
updateRate = 0.040*1e9 # 25 Hz
if len(self.currentVals.times) == 0 or (len(values.times) > 0 and
values.times[-1] > self.currentVals.times[-1] + updateRate): # each 10ms
self.currentVals.add(values)
values = self.currentVals
self.pedals_pos_plot.setOpts(height=[values.gas[-1], values.brake[-1], values.clutch[-1]])
self.pedals_pos_plot2.setData(x=np.concatenate(([-0.35]*len(values.gas), [0.65]*len(values.brake), [1.65]*len(values.clutch))),
y=np.concatenate((values.gas, values.brake, values.clutch)))
self.pedals_gas_calib_plot.setPen(QPen(QColor(255,0,0),width=0))
self.pedals_gas_calib_plot.setData(x=[-0.0, -0.0, +0.0, +0.0, -0.0],
y=[values.calib.gasMin, values.calib.gasMax, values.calib.gasMax, values.calib.gasMin, values.calib.gasMin])
self.pedals_brake_calib_plot.setData(x=[1.0,1.0,1.0,1.0,1.0],
y=[values.calib.brakeMin, values.calib.brakeMax, values.calib.brakeMax, values.calib.brakeMin, values.calib.brakeMin])
self.pedals_clutch_calib_plot.setData(x=[2.0,2.0,2.0,2.0,2.0],
y=[values.calib.clutchMin, values.calib.clutchMax, values.calib.clutchMax, values.calib.clutchMin, values.calib.clutchMin])
self.shifter_pos_plot.setData(x=values.sx, y=values.sy)
self.shifter_neutral_plot.setRegion((values.calib.shifter_y_neutralMin, values.calib.shifter_y_neutralMax))
self.shifter_135_plot.setRegion((values.calib.shifter_y_135_gearZone, 1024))
self.shifter_246_plot.setRegion((0, values.calib.shifter_y_246R_gearZone))
self.shifter_12_plot.setRegion((0, values.calib.shifter_x_12))
self.shifter_56_plot.setRegion((values.calib.shifter_x_56, 1024))
blockers = []
for b in self.option_btns:
blockers.append(QSignalBlocker(b))
self.option_btns[0].setChecked(values.calib.pedals_auto_calib)
self.option_btns[1].setChecked((values.calib.flags & FLAG_INVERT_GAS) != 0)
self.option_btns[2].setChecked((values.calib.flags & FLAG_INVERT_BRAKE) != 0)
self.option_btns[3].setChecked((values.calib.flags & FLAG_INVERT_CLUTCH) != 0)
self.option_btns[4].setChecked((values.calib.flags & FLAG_REVERSE_RIGHT_RED) != 0)
self.option_btns[5].setChecked(values.calib.use_pedals)
self.option_btns[6].setChecked(values.calib.use_shifter)
self.option_btns[7].setCurrentIndex(self.option_btns[7].findData(values.calib.pedals_median_size))
self.option_btns[8].setCurrentIndex(self.option_btns[8].findData(values.calib.shifter_median_size))
prof = "Total runtime: %9.2f ms | prof[0->1]: %9.2f ms | prof[1->2]: %9.2f ms | prof[2->3]: %9.2f ms | FPS: %04d" % (
(dbg.profiling[-1] - dbg.profiling[0])*1e-3,
(dbg.profiling[1] - dbg.profiling[0])*1e-3,
(dbg.profiling[2] - dbg.profiling[1])*1e-3,
(dbg.profiling[3] - dbg.profiling[2])*1e-3,
((len(values.times)*1e9 / (values.times[-1] - values.times[0])) if len(values.times) > 1 else "N/A"),
)
self.status_line.setText(prof)
# output values
s = ""
if dbg.out_axis[0] != 0xffff:
s += "A0=%04d A1=%04d A2=%04d " % tuple(dbg.out_axis)
else:
s += "A0=N/A A1=N/A A2=N/A "
if dbg.out_buttons != 0xffffffff:
bstr="{:019b}".format(dbg.out_buttons)
s += "ShifterBtn: " + bstr[:-7]
gear = bstr[-7:][::-1]
if "1" in gear:
gear = gear.index("1") + 1
else:
gear = 0
s += " ShifterGear=%d" % (gear)
else:
s += "Shifter=N/A"
self.status_line2.setText(s)
class Collector(QObject):
valuesChanged = Signal(object, object)
def __init__(self, portName):
super().__init__()
self.portName = portName
self.thread = QThread()
self.moveToThread(self.thread)
self.thread.started.connect(self.create)
self.thread.start()
def sendModeCmd(self, cmd):
self.serialPort.write(cmd)
#print("Sent CMD: ", cmd)
def create(self):
try:
self.values = Values()
self.serialPort = serial.Serial(self.portName, baudrate=460800, timeout=1) #115200, timeout=1)
self.serialPort.write(b'O')
self.n = 0
self.tstart = time.perf_counter_ns()
self.timer = QTimer()
self.timer.timeout.connect(self.readFromSerial, Qt.QueuedConnection)
self.timer.setInterval(0)
self.timer.start()
except Exception as e:
traceback.print_exc()
def readFromSerial(self):
try:
buf = self.serialPort.read(1)
numBytes, = struct.unpack("B", buf)
while len(buf) < numBytes-1:
buf += self.serialPort.read(numBytes-1)
dbg = DebugStruct.from_buffer_copy(buf)
self.n += len(buf)+2
if 0:
print("read %d bytes in %.2f seconds: bits/second=%.1f" %
(self.n,
(time.perf_counter_ns() - self.tstart)*1e-9,
self.n * 8 / ((time.perf_counter_ns() - self.tstart)*1e-9)))
if 0:
print("Profiling 01: %.1f ms 12: %.1f ms 23: %.1f ms 03: %.1f ms" %
(
(items[-3] - items[-4])*1e-3,
(items[-2] - items[-3])*1e-3,
(items[-1] - items[-2])*1e-3,
(items[-1] - items[-4])*1e-3,
))
self.values.update(dbg)
self.valuesChanged.emit(self.values, dbg)
except Exception as e:
traceback.print_exc()
class JoystickSink(QObject):
def __init__(self, jsdev):
super().__init__()
self.jsdev = jsdev
self.thread = QThread()
self.moveToThread(self.thread)
self.thread.started.connect(self.create)
self.thread.start()
def create(self):
self.timer = QTimer()
self.timer.timeout.connect(self.readFromDevice, Qt.QueuedConnection)
self.timer.setInterval(0)
self.timer.start()
def readFromDevice(self):
try:
events = self.jsdev.read()
except:
pass
def main():
app = QApplication(sys.argv)
vars = {}
def createGui():
nonlocal vars
gui = G27CalibGui()
gui.setMinimumSize(1024, 700)
gui.setWindowTitle("G27 Pedalsand Shifter")
coll = Collector(vars["tty"])
coll.valuesChanged.connect(gui.newVals)
js = JoystickSink(vars["jsdev"])
gui.sendModeCmd.connect(coll.sendModeCmd)
gui.show()
if main_widget is not None: main_widget.hide()
vars["gui"] = gui
vars["coll"] = coll
vars["js"] = js
if len(list_ports.comports()) != 1 or len(inputs.devices.gamepads) != 1:
main_widget = QWidget()
main_widget.setWindowTitle("G27 Pedalsand Shifter - serial port")
layout = QGridLayout(main_widget)
layout.addWidget(QLabel("Select Arduino COM port:", main_widget),0,0)
ttyCombo = QComboBox(main_widget)
layout.addWidget(ttyCombo,0,1)
layout.addWidget(QLabel("Select G27 joystick port:", main_widget), 1, 0)
jsCombo = QComboBox(main_widget)
layout.addWidget(jsCombo,1,1)
def refresh():
ttyCombo.clear()
n = 0
for p in list_ports.comports():
ttyCombo.addItem(p.device)
n += 1
k = 0
jsCombo.clear()
for i,d in enumerate(inputs.devices.gamepads):
jsCombo.addItem(d.name, userData=d)
k += 1
btnStart.setEnabled(n > 0 and k > 0)
def startgui():
idx = ttyCombo.currentIndex()
vars["tty"] = ttyCombo.itemText(ttyCombo.currentIndex())
vars["jsdev"] = jsCombo.itemData(jsCombo.currentIndex())
createGui()
btnRefresh = QPushButton("Refresh Devices")
btnRefresh.clicked.connect(refresh)
layout.addWidget(btnRefresh, 0, 2)
btnStart = QPushButton("Start")
btnStart.clicked.connect(startgui)
refresh()
layout.addWidget(btnStart, 1, 2)
main_widget.show()
else:
main_widget = None
vars["tty"] = list_ports.comports()[0].device
vars["jsdev"] = inputs.devices.gamepads[0]
createGui()
return app.exec_()
if __name__ == "__main__":
main()

13
setup.py Normal file
View File

@ -0,0 +1,13 @@
#!/usr/bin/env python
from distutils.core import setup
setup(name='G27_Pedals_and_Shifter_GUI',
version='0.1',
description='Calibration GUI for the G27 Arduino adapter.',
author='functionreturnfunction,NEYS',
url='https://github.com/n-e-y-s/G27_Pedals_and_Shifter_GUI',
packages=['g27_pedals_and_shifter_gui'],
install_requires=['PySide2>=5.12.0', 'pyqtgraph>=0.11.0', 'pyserial>=3.4', "numpy>=1.16.2", "inputs>=0.5"],
entry_points = {"gui_scripts" : ['g27calib = g27_pedals_and_shifter_gui.calib:main']},
)