mirror of
https://github.com/n-e-y-s/G27_Pedals_and_Shifter.git
synced 2024-08-30 18:22:10 +00:00
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
parent
3b0fe5431d
commit
689bf7b71c
3
.gitignore
vendored
Normal file
3
.gitignore
vendored
Normal file
@ -0,0 +1,3 @@
|
|||||||
|
.idea
|
||||||
|
*.egg*
|
||||||
|
__pycache__
|
||||||
@ -5,31 +5,9 @@
|
|||||||
// http://www.isrtv.com/forums/topic/13189-diy-g25-shifter-interface-with-h-pattern-sequential-and-handbrake-modes/
|
// http://www.isrtv.com/forums/topic/13189-diy-g25-shifter-interface-with-h-pattern-sequential-and-handbrake-modes/
|
||||||
|
|
||||||
#include <HID.h>
|
#include <HID.h>
|
||||||
|
#include <EEPROM.h>
|
||||||
#include "./src/G27PedalsShifter.h"
|
#include "./src/G27PedalsShifter.h"
|
||||||
|
#include "./src/Filter.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
|
|
||||||
|
|
||||||
// LED PINS
|
// LED PINS
|
||||||
#define RED_PIN 3
|
#define RED_PIN 3
|
||||||
@ -97,38 +75,127 @@
|
|||||||
#define OUTPUT_RED_CENTERRIGHT 17
|
#define OUTPUT_RED_CENTERRIGHT 17
|
||||||
#define OUTPUT_RED_RIGHT 18
|
#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.
|
// MISC.
|
||||||
#define MAX_AXIS 1023
|
#define MAX_AXIS 1023
|
||||||
#define SIGNAL_SETTLE_DELAY 10
|
#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
|
// PEDAL CODE
|
||||||
typedef struct pedal {
|
typedef struct Pedal {
|
||||||
byte pin;
|
byte pin;
|
||||||
int min, max, cur, axis;
|
int min, max, cur, axis;
|
||||||
};
|
} Pedal;
|
||||||
|
|
||||||
typedef struct pedal Pedal;
|
static Pedal* gasPedal = 0;
|
||||||
|
static Pedal* brakePedal = 0;
|
||||||
|
static Pedal* clutchPedal = 0;
|
||||||
|
|
||||||
void* gasPedal;
|
int axisValue(struct Pedal* input) {
|
||||||
void* brakePedal;
|
|
||||||
void* clutchPedal;
|
|
||||||
|
|
||||||
int axisValue(void* in) {
|
|
||||||
Pedal* input = (Pedal*)in;
|
|
||||||
|
|
||||||
int physicalRange = input->max - input->min;
|
int physicalRange = input->max - input->min;
|
||||||
if (physicalRange == 0) {
|
if (physicalRange == 0) {
|
||||||
@ -146,39 +213,21 @@ int axisValue(void* in) {
|
|||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
void processPedal(void* in) {
|
void processPedal(struct Pedal* input, SignalFilter *flt, uint8_t filterSize)
|
||||||
Pedal* input = (Pedal*)in;
|
{
|
||||||
|
input->cur = apply_filter(flt, filterSize, analogRead(input->pin));
|
||||||
|
|
||||||
input->cur = analogRead(input->pin);
|
if(calibration.data.pedals_auto_calib)
|
||||||
|
{
|
||||||
#if !defined(STATIC_THRESHOLDS)
|
// calibrate, we want the highest this pedal has been
|
||||||
// calibrate, we want the highest this pedal has been
|
input->max = input->cur > input->max ? input->cur : input->max;
|
||||||
input->max = input->cur > input->max ? input->cur : input->max;
|
// same for lowest, but bottom out at current value rather than 0
|
||||||
// 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->min = input->min == 0 || input->cur < input->min ? input->cur : input->min;
|
}
|
||||||
#endif
|
|
||||||
|
|
||||||
input->axis = axisValue(input);
|
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) {
|
void setXAxis(void* in) {
|
||||||
Pedal* input = (Pedal*)in;
|
Pedal* input = (Pedal*)in;
|
||||||
G27.setXAxis(input->axis);
|
G27.setXAxis(input->axis);
|
||||||
@ -194,14 +243,6 @@ void setZAxis(void* in) {
|
|||||||
G27.setZAxis(input->axis);
|
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
|
// SHIFTER CODE
|
||||||
int buttonTable[] = {
|
int buttonTable[] = {
|
||||||
// first four are unused
|
// first four are unused
|
||||||
@ -229,25 +270,12 @@ void getButtonStates(int *ret) {
|
|||||||
waitForSignalToSettle();
|
waitForSignalToSettle();
|
||||||
digitalWrite(SHIFTER_MODE_PIN, HIGH); // Switch to serial mode: one data bit is output on each clock falling edge
|
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
|
for(int i = 0; i < 16; ++i) { // Iteration over both 8 bit registers
|
||||||
digitalWrite(SHIFTER_CLOCK_PIN, LOW); // Generate clock falling edge
|
digitalWrite(SHIFTER_CLOCK_PIN, LOW); // Generate clock falling edge
|
||||||
waitForSignalToSettle();
|
waitForSignalToSettle();
|
||||||
|
|
||||||
ret[i] = digitalRead(SHIFTER_DATA_PIN);
|
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
|
digitalWrite(SHIFTER_CLOCK_PIN, HIGH); // Generate clock rising edge
|
||||||
waitForSignalToSettle();
|
waitForSignalToSettle();
|
||||||
}
|
}
|
||||||
@ -259,32 +287,93 @@ void getShifterPosition(int *ret) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
int getCurrentGear(int shifterPosition[], int btns[]) {
|
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];
|
int x = shifterPosition[0], y = shifterPosition[1];
|
||||||
|
|
||||||
if (x < SHIFTER_XAXIS_12) // Shifter on the left?
|
if (y < calibration.data.shifter_y_neutralMax && y > calibration.data.shifter_y_neutralMin)
|
||||||
{
|
{
|
||||||
if (y > SHIFTER_YAXIS_135) gear = 1; // 1st gear
|
gear = 0;
|
||||||
if (y < SHIFTER_YAXIS_246) gear = 2; // 2nd gear
|
}
|
||||||
}
|
|
||||||
else if (x > SHIFTER_XAXIS_56) // Shifter on the right?
|
|
||||||
{
|
|
||||||
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 (y > SHIFTER_YAXIS_135) gear = 3; // 3rd gear
|
|
||||||
if (y < SHIFTER_YAXIS_246) gear = 4; // 4th gear
|
|
||||||
}
|
|
||||||
|
|
||||||
if (gear != 6) btns[BUTTON_REVERSE] = 0; // Reverse gear is allowed only on 6th gear position
|
if (y > calibration.data.shifter_y_135_gearZone )
|
||||||
if (btns[BUTTON_REVERSE] == 1) gear = 7; // Reverse is 7th gear (for the sake of argument)
|
{
|
||||||
|
if( x <= calibration.data.shifter_x_12 )
|
||||||
|
{
|
||||||
|
if( gear != 3 ) /* avoid toggles between neighboring gears */
|
||||||
|
{
|
||||||
|
gear = 1;
|
||||||
|
}
|
||||||
|
} else if ( x >= calibration.data.shifter_x_56 )
|
||||||
|
{
|
||||||
|
if( gear != 3 ) /* avoid toggles between neighboring gears */
|
||||||
|
{
|
||||||
|
gear = 5;
|
||||||
|
}
|
||||||
|
} else
|
||||||
|
{
|
||||||
|
if( gear != 1 && gear != 5 ) /* avoid toggles between neighboring gears */
|
||||||
|
{
|
||||||
|
gear = 3;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
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;
|
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) {
|
void setButtonStates(int buttons[], int gear) {
|
||||||
|
// SHIFTER CODE
|
||||||
|
debug.out_buttons = 0;
|
||||||
|
|
||||||
// release virtual buttons for all gears
|
// release virtual buttons for all gears
|
||||||
for (byte i = 0; i < 7; ++i) {
|
for (byte i = 0; i < 7; ++i) {
|
||||||
G27.setButton(i, LOW);
|
G27.setButton(i, LOW);
|
||||||
@ -292,72 +381,187 @@ void setButtonStates(int buttons[], int gear) {
|
|||||||
|
|
||||||
if (gear > 0) {
|
if (gear > 0) {
|
||||||
G27.setButton(gear - 1, HIGH);
|
G27.setButton(gear - 1, HIGH);
|
||||||
|
debug.out_buttons |= 1 << (gear - 1);
|
||||||
}
|
}
|
||||||
|
|
||||||
for (byte i = BUTTON_RED_CENTERRIGHT; i <= BUTTON_DPAD_TOP; ++i) {
|
for (byte i = BUTTON_RED_CENTERRIGHT; i <= BUTTON_DPAD_TOP; ++i) {
|
||||||
G27.setButton(buttonTable[i], buttons[i]);
|
G27.setButton(buttonTable[i], buttons[i]);
|
||||||
|
if( buttons[i] )
|
||||||
|
{
|
||||||
|
debug.out_buttons |= 1 << buttonTable[i];
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void describeButtonStates(int buttons[], int shifterPosition[], int gear) {
|
#define CALIB_MIN(calibValue, curValue, minValue) if( calibValue < 0 || curValue < minValue ) { calibValue = minValue = curValue; }
|
||||||
Serial.print("\nSHIFTER X: ");
|
#define CALIB_MAX(calibValue, curValue, maxValue) if( calibValue < 0 || curValue > maxValue ) { calibValue = maxValue = curValue; }
|
||||||
Serial.print(shifterPosition[0]);
|
#define CALIB_RANGE(calibValue, curValue, minValue, maxValue) \
|
||||||
Serial.print(" Y: ");
|
{ if( calibValue < 0 ) { calibValue = minValue = maxValue = curValue; } \
|
||||||
Serial.print(shifterPosition[1]);
|
CALIB_MIN(calibValue, curValue, minValue); \
|
||||||
|
CALIB_MAX(calibValue, curValue, maxValue); }
|
||||||
|
|
||||||
Serial.print(" GEAR: ");
|
void calib(struct Pedal *gas, Pedal *brake, Pedal *clutch, int shifter_X, int shifter_Y, int calibButton)
|
||||||
Serial.print(gear);
|
{
|
||||||
Serial.print(" REVERSE: ");
|
static uint8_t printMode = 0;
|
||||||
Serial.print(buttons[BUTTON_REVERSE]);
|
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;
|
||||||
|
|
||||||
Serial.print(" RED BUTTONS:");
|
case SET_PEDAL_AUTO_CALIBRATE: calibration.data.pedals_auto_calib = 1; break;
|
||||||
if (buttons[BUTTON_RED_LEFT]) {
|
case RESET_PEDAL_AUTO_CALIBRATE: calibration.data.pedals_auto_calib= 0; break;
|
||||||
Serial.print(" 1");
|
|
||||||
}
|
|
||||||
if (buttons[BUTTON_RED_CENTERLEFT]) {
|
|
||||||
Serial.print(" 2");
|
|
||||||
}
|
|
||||||
if (buttons[BUTTON_RED_CENTERLEFT]) {
|
|
||||||
Serial.print(" 3");
|
|
||||||
}
|
|
||||||
if (buttons[BUTTON_RED_RIGHT]) {
|
|
||||||
Serial.print(" 4");
|
|
||||||
}
|
|
||||||
|
|
||||||
Serial.print(" BLACK BUTTONS:");
|
case SET_PEDALS_ENABLED: calibration.data.use_pedals = 1; break;
|
||||||
if (buttons[BUTTON_BLACK_LEFT]) {
|
case RESET_PEDALS_ENABLED: calibration.data.use_pedals = 0; break;
|
||||||
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:");
|
case SET_SHIFTER_ENABLED: calibration.data.use_shifter = 1; break;
|
||||||
if (buttons[BUTTON_DPAD_LEFT]) {
|
case RESET_SHIFTER_ENABLED: calibration.data.use_shifter = 0; break;
|
||||||
Serial.print(" LEFT");
|
|
||||||
|
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;
|
||||||
|
|
||||||
|
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_DPAD_TOP]) {
|
if(printMode)
|
||||||
Serial.print(" UP");
|
{
|
||||||
|
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_DPAD_BOTTOM]) {
|
if(calibButton)
|
||||||
Serial.print(" DOWN");
|
{
|
||||||
|
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_DPAD_RIGHT]) {
|
switch(currentMode)
|
||||||
Serial.print(" RIGHT");
|
{
|
||||||
|
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() {
|
void setup() {
|
||||||
Serial.begin(38400);
|
Serial.begin(460800);
|
||||||
#if !defined(DEBUG_PEDALS) && !defined(DEBUG_SHIFTER)
|
|
||||||
G27.begin(false);
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// lights
|
// lights
|
||||||
pinMode(RED_PIN, OUTPUT);
|
pinMode(RED_PIN, OUTPUT);
|
||||||
@ -380,73 +584,89 @@ void setup() {
|
|||||||
brake->pin = BRAKE_PIN;
|
brake->pin = BRAKE_PIN;
|
||||||
clutch->pin = CLUTCH_PIN;
|
clutch->pin = CLUTCH_PIN;
|
||||||
|
|
||||||
#if defined(STATIC_THRESHOLDS)
|
loadEEPROM();
|
||||||
gas->min = MIN_GAS;
|
|
||||||
gas->max = MAX_GAS;
|
|
||||||
|
|
||||||
brake->min = MIN_BRAKE;
|
if( !calibration.data.pedals_auto_calib )
|
||||||
brake->max = MAX_BRAKE;
|
{
|
||||||
|
gas->min = calibration.data.gasMin;
|
||||||
|
gas->max = calibration.data.gasMax;
|
||||||
|
|
||||||
clutch->min = MIN_CLUTCH;
|
brake->min = calibration.data.brakeMin;
|
||||||
clutch->max = MAX_CLUTCH;
|
brake->max = calibration.data.brakeMax;
|
||||||
#endif
|
|
||||||
|
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;
|
gasPedal = gas;
|
||||||
brakePedal = brake;
|
brakePedal = brake;
|
||||||
clutchPedal = clutch;
|
clutchPedal = clutch;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
SignalFilter signalFilters[5];
|
||||||
|
|
||||||
void loop() {
|
void loop() {
|
||||||
|
debug.profiling[2] = micros();
|
||||||
// pedals
|
// pedals
|
||||||
processPedal(gasPedal);
|
processPedal(gasPedal, &signalFilters[0], calibration.data.pedal_median_size);
|
||||||
processPedal(brakePedal);
|
processPedal(brakePedal, &signalFilters[1], calibration.data.pedal_median_size);
|
||||||
processPedal(clutchPedal);
|
processPedal(clutchPedal, &signalFilters[2], calibration.data.pedal_median_size);
|
||||||
|
|
||||||
#if defined(INVERT_BRAKE)
|
if(calibration.data.flags & FLAG_INVERT_GAS )
|
||||||
Pedal* brake = (Pedal*)brakePedal;
|
{
|
||||||
brake->axis = map(brake->axis, 0, MAX_AXIS, MAX_AXIS, 0);
|
Pedal* gas = (Pedal*)gasPedal;
|
||||||
#endif
|
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 defined(DEBUG_PEDALS)
|
if(calibration.data.use_pedals)
|
||||||
describePedal("GAS", "X", gasPedal);
|
{
|
||||||
describePedal("BRAKE", "Y", brakePedal);
|
setXAxis(gasPedal);
|
||||||
describePedal("CLUTCH", "Z", clutchPedal);
|
setYAxis(brakePedal);
|
||||||
#elif defined(USE_PEDALS)
|
setZAxis(clutchPedal);
|
||||||
setXAxis(gasPedal);
|
debug.out_axis[0] = gasPedal->axis;
|
||||||
setYAxis(brakePedal);
|
debug.out_axis[1] = brakePedal->axis;
|
||||||
setZAxis(clutchPedal);
|
debug.out_axis[2] = clutchPedal->axis;
|
||||||
#endif
|
} else
|
||||||
|
{
|
||||||
#if defined(PEDAL_COLORS)
|
debug.out_axis[0] = 0xffff;
|
||||||
pedalColor(gasPedal, brakePedal, clutchPedal);
|
debug.out_axis[1] = 0xffff;
|
||||||
#endif
|
debug.out_axis[2] = 0xffff;
|
||||||
|
}
|
||||||
|
|
||||||
// shifter
|
// shifter
|
||||||
int buttonStates[16];
|
int buttonStates[16];
|
||||||
getButtonStates(buttonStates);
|
getButtonStates(buttonStates);
|
||||||
|
|
||||||
int shifterPosition[2];
|
int shifterPosition[2];
|
||||||
getShifterPosition(shifterPosition);
|
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);
|
int gear = getCurrentGear(shifterPosition, buttonStates);
|
||||||
|
|
||||||
#if defined(DEBUG_SHIFTER)
|
if(calibration.data.use_shifter)
|
||||||
describeButtonStates(buttonStates, shifterPosition, gear);
|
{
|
||||||
#elif defined(USE_SHIFTER)
|
setButtonStates(buttonStates, gear);
|
||||||
setButtonStates(buttonStates, gear);
|
} else
|
||||||
#endif
|
{
|
||||||
|
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();
|
G27.sendState();
|
||||||
#endif
|
debug.profiling[1] = micros();
|
||||||
|
|
||||||
#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);
|
|
||||||
}
|
}
|
||||||
|
|||||||
100
G27_Pedals_and_Shifter/src/Filter.cpp
Normal file
100
G27_Pedals_and_Shifter/src/Filter.cpp
Normal 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];
|
||||||
|
}
|
||||||
|
|
||||||
22
G27_Pedals_and_Shifter/src/Filter.h
Normal file
22
G27_Pedals_and_Shifter/src/Filter.h
Normal 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
|
||||||
95
G27_Pedals_and_Shifter/src/test_filter.py
Normal file
95
G27_Pedals_and_Shifter/src/test_filter.py
Normal 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
|
||||||
480
g27_pedals_and_shifter_gui/calib.py
Normal file
480
g27_pedals_and_shifter_gui/calib.py
Normal 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
13
setup.py
Normal 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']},
|
||||||
|
)
|
||||||
Loading…
Reference in New Issue
Block a user