mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-07-19 03:42:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

add ability to disable steering ratio learner

Browse Source
This commit is contained in:
dragonpilot
2019-11-27 10:09:41 +10:00
parent 574518f163
commit 65adf79be6
5 changed files with 30 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
common/params.py
+1
View File
@@ -149,6 +149,7 @@ keys = {
"DragonToyotaSnGMod": [TxType.PERSISTENT],
"DragonIsEON": [TxType.PERSISTENT],
"DragonHWChecked": [TxType.PERSISTENT],
"DragonEnableSRLearner": [TxType.PERSISTENT],
}
selfdrive/dragonpilot/dragonconf/__init__.py
+1
View File
@@ -56,6 +56,7 @@ default_conf = {
'DragonToyotaSnGMod': '0',
'DragonIsEON': '1',
'DragonHWChecked': '0',
'DragonEnableSRLearner': '1',
}
deprecated_conf = {
selfdrive/locationd/params_learner.cc
+14 -7
View File
@@ -44,7 +44,7 @@ ParamsLearner::ParamsLearner(cereal::CarParams::Reader car_params,
alpha4 = 1.0 * learning_rate;
}
bool ParamsLearner::update(double psi, double u, double sa) {
bool ParamsLearner::update(double psi, double u, double sa, bool enable_sr_learner) {
if (u > 10.0 && fabs(sa) < (DEGREES_TO_RADIANS * 90.)) {
double ao_diff = 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 2)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 2));
double new_ao = ao - alpha1 * ao_diff;
@@ -54,6 +54,9 @@ bool ParamsLearner::update(double psi, double u, double sa) {
double new_x = x - alpha3 * (-2.0*cF0*cR0*l*m*pow(u, 3)*(slow_ao - sa)*(aF*cF0 - aR*cR0)*(1.0*cF0*cR0*l*u*x*(slow_ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 2)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 3)));
double new_sR = sR - alpha4 * (-2.0*cF0*cR0*l*u*x*(slow_ao - sa)*(1.0*cF0*cR0*l*u*x*(slow_ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 3)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 2)));
if (!enable_sr_learner) {
new_sR = sR;
}
ao = new_ao;
slow_ao = new_slow_ao;
@@ -69,12 +72,16 @@ bool ParamsLearner::update(double psi, double u, double sa) {
ao = clip(ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET);
slow_ao = clip(slow_ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET);
x = clip(x, MIN_STIFFNESS, MAX_STIFFNESS);
sR = clip(sR, min_sr, max_sr);
if (enable_sr_learner) {
sR = clip(sR, min_sr, max_sr);
bool valid = fabs(slow_ao) < MAX_ANGLE_OFFSET_TH;
valid = valid && sR > min_sr_th;
valid = valid && sR < max_sr_th;
return valid;
bool valid = fabs(slow_ao) < MAX_ANGLE_OFFSET_TH;
valid = valid && sR > min_sr_th;
valid = valid && sR < max_sr_th;
return valid;
} else {
return true;
}
}
@@ -93,7 +100,7 @@ extern "C" {
bool params_learner_update(void * params_learner, double psi, double u, double sa) {
ParamsLearner * p = (ParamsLearner*) params_learner;
return p->update(psi, u, sa);
return p->update(psi, u, sa, true);
}
double params_learner_get_ao(void * params_learner){
selfdrive/locationd/params_learner.h
+1 -1
View File
@@ -31,5 +31,5 @@ public:
double steer_ratio,
double learning_rate);
bool update(double psi, double u, double sa);
bool update(double psi, double u, double sa, bool enable_sr_learner);
};
selfdrive/locationd/paramsd.cc
+13 -1
View File
@@ -96,6 +96,18 @@ int main(int argc, char *argv[]) {
// Main loop
int save_counter = 0;
// dragonpilot
// Steer Ratio Learner
char* enable_sr_learner_val = NULL;
read_db_value(NULL, "DragonEnableSRLearner", &enable_sr_learner_val, NULL);
bool enable_sr_learner = true;
if (enable_sr_learner_val && strlen(enable_sr_learner_val) && enable_sr_learner_val[0] == '1') {
enable_sr_learner = true;
} else {
enable_sr_learner = false;
}
while (true){
for (auto s : poller->poll(-1)){
Message * msg = s->receive();
@@ -120,7 +132,7 @@ int main(int argc, char *argv[]) {
save_counter++;
double yaw_rate = -localizer.x[0];
bool valid = learner.update(yaw_rate, localizer.car_speed, localizer.steering_angle);
bool valid = learner.update(yaw_rate, localizer.car_speed, localizer.steering_angle, enable_sr_learner);
// TODO: Fix in replay
double sensor_data_age = localizer.controls_state_time - localizer.sensor_data_time;