add option to disable gt safety angle slowdown

This commit is contained in:
interfisch 2023-06-17 21:43:59 +02:00
parent 7945c2d9ac
commit de1757d7af
2 changed files with 29 additions and 17 deletions

View File

@ -28,4 +28,3 @@ lib_deps =
https://github.com/adafruit/Adafruit-GFX-Library https://github.com/adafruit/Adafruit-GFX-Library
arduino-libraries/SD@^1.2.4 arduino-libraries/SD@^1.2.4
mathertel/OneButton@^2.0.3 mathertel/OneButton@^2.0.3
arduino-libraries/SD@^1.2.4

View File

@ -73,6 +73,13 @@ float steer_coefficient_nrf=0.5; // higher value == stronger
float speed_coefficient_gt=1.0; // higher value == stronger float speed_coefficient_gt=1.0; // higher value == stronger
float steer_coefficient_gt=0.5; // higher value == stronger float steer_coefficient_gt=0.5; // higher value == stronger
#define GT_SAFETY_THRESHOLD_H 120 //above which value (abs) safety slowdown should start
#define GT_SAFETY_THRESHOLD_V 120
#define SAFETYMULTIPLIER_UPDATE_INTERVAL 100
#define SAFETYMULTIPLIER_CHANGE_TIME_DOWN 2.0 //Time how long it should take to go from 100% to 0% when in safety slowdown
#define SAFETYMULTIPLIER_CHANGE_TIME_UP 1.0
bool gt_safety_enable=true;
unsigned long watchdogtimer=0; //set to current millis everytime new good control input is calculated unsigned long watchdogtimer=0; //set to current millis everytime new good control input is calculated
long last_adcupdated=0; long last_adcupdated=0;
@ -359,7 +366,6 @@ void loop() {
if (loopmillis - last_adcupdated > ADC_UPDATEPERIOD) { //update analog readings if (loopmillis - last_adcupdated > ADC_UPDATEPERIOD) { //update analog readings
int raw_length_a=analogRead(PIN_GAMETRAK_LENGTH_A); int raw_length_a=analogRead(PIN_GAMETRAK_LENGTH_A);
int raw_length_b=analogRead(PIN_GAMETRAK_LENGTH_B); int raw_length_b=analogRead(PIN_GAMETRAK_LENGTH_B);
@ -367,7 +373,6 @@ void loop() {
int raw_length=(raw_length_a+raw_length_b)/2; int raw_length=(raw_length_a+raw_length_b)/2;
uint16_t gt_length_1 = GT_LENGTH_1_OFFSET+raw_length*GT_LENGTH_1_SCALE; uint16_t gt_length_1 = GT_LENGTH_1_OFFSET+raw_length*GT_LENGTH_1_SCALE;
uint16_t gt_length_2 = GT_LENGTH_2_OFFSET+raw_length*GT_LENGTH_2_SCALE; uint16_t gt_length_2 = GT_LENGTH_2_OFFSET+raw_length*GT_LENGTH_2_SCALE;
@ -591,15 +596,12 @@ void loop() {
} }
} }
static float safetymultiplier=1.0; //value to reduce output speed if gametrack is pointing too far down or up (string might be behind vehicle) static float safetymultiplier=1.0; //value to reduce output speed if gametrack is pointing too far down or up (string might be behind vehicle)
#define GT_SAFETY_THRESHOLD_H 120 //above which value (abs) safety slowdown should start
#define GT_SAFETY_THRESHOLD_V 120
#define SAFETYMULTIPLIER_UPDATE_INTERVAL 100
#define SAFETYMULTIPLIER_CHANGE_TIME_DOWN 2.0 //Time how long it should take to go from 100% to 0% when in safety slowdown
#define SAFETYMULTIPLIER_CHANGE_TIME_UP 1.0
static unsigned long last_safetymultiplier_update=0; static unsigned long last_safetymultiplier_update=0;
if (gt_safety_enable) {
if (loopmillis-last_safetymultiplier_update > SAFETYMULTIPLIER_UPDATE_INTERVAL) { if (loopmillis-last_safetymultiplier_update > SAFETYMULTIPLIER_UPDATE_INTERVAL) {
if (abs(gt_vertical)>GT_SAFETY_THRESHOLD_V || abs(gt_horizontal)>GT_SAFETY_THRESHOLD_H) { if (abs(gt_vertical)>GT_SAFETY_THRESHOLD_V || abs(gt_horizontal)>GT_SAFETY_THRESHOLD_H) {
safetymultiplier-=1.0/(1000.0/SAFETYMULTIPLIER_UPDATE_INTERVAL)/SAFETYMULTIPLIER_CHANGE_TIME_DOWN; safetymultiplier-=1.0/(1000.0/SAFETYMULTIPLIER_UPDATE_INTERVAL)/SAFETYMULTIPLIER_CHANGE_TIME_DOWN;
@ -609,6 +611,9 @@ void loop() {
safetymultiplier=constrain(safetymultiplier,0.0,1.0); safetymultiplier=constrain(safetymultiplier,0.0,1.0);
last_safetymultiplier_update=loopmillis; last_safetymultiplier_update=loopmillis;
} }
}else{
safetymultiplier=1.0; //always 100% when disabled
}
//calculate speed l and r from speed and steer //calculate speed l and r from speed and steer
static unsigned long last_controlupdate_gt=0; static unsigned long last_controlupdate_gt=0;
@ -891,6 +896,14 @@ void updateInputs(unsigned long loopmillis) {
steer_coefficient_gt = constrain(steer_coefficient_gt, 0.05, 1.0); steer_coefficient_gt = constrain(steer_coefficient_gt, 0.05, 1.0);
} }
break; break;
case 3: // gt_safety_enable
if (button_inc_click) {
gt_safety_enable=true;
}
if (button_dec_click) {
gt_safety_enable=false;
}
break;
} }
@ -1109,7 +1122,7 @@ void display_show_menu() {
display.print(F("steer_coeff_gt=")); display.println(steer_coefficient_gt,2); display.print(F("steer_coeff_gt=")); display.println(steer_coefficient_gt,2);
break; break;
case 3: case 3:
display.print(F("gt_safety_enable=")); display.println(gt_safety_enable);
break; break;
case 4: case 4: