mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-22 00:33:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

openpilot v0.6.1 release

Browse Source 
old-commit-hash: 94053536b4
This commit is contained in:
Vehicle Researcher
2019-07-22 19:17:47 +00:00
parent a352e8a065
commit 07aa8b1bf3
61 changed files with 1160 additions and 752 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/params_learner.cc
+51 -8
View File
@@ -2,6 +2,8 @@
#include <cmath>
#include <iostream>
#include <capnp/message.h>
#include <capnp/serialize-packed.h>
#include "cereal/gen/cpp/log.capnp.h"
#include "cereal/gen/cpp/car.capnp.h"
#include "params_learner.h"
@@ -14,14 +16,15 @@ T clip(const T& n, const T& lower, const T& upper) {
}
ParamsLearner::ParamsLearner(cereal::CarParams::Reader car_params,
double angle_offset,
double stiffness_factor,
double steer_ratio,
double learning_rate) :
ao(angle_offset * DEGREES_TO_RADIANS),
slow_ao(angle_offset * DEGREES_TO_RADIANS),
x(stiffness_factor),
sR(steer_ratio) {
double angle_offset,
double stiffness_factor,
double steer_ratio,
double learning_rate) :
ao(angle_offset * DEGREES_TO_RADIANS),
slow_ao(angle_offset * DEGREES_TO_RADIANS),
x(stiffness_factor),
sR(steer_ratio) {
cF0 = car_params.getTireStiffnessFront();
cR0 = car_params.getTireStiffnessRear();
@@ -73,3 +76,43 @@ bool ParamsLearner::update(double psi, double u, double sa) {
valid = valid && sR < max_sr_th;
return valid;
}
extern "C" {
void *params_learner_init(size_t len, char * params, double angle_offset, double stiffness_factor, double steer_ratio, double learning_rate) {
auto amsg = kj::heapArray<capnp::word>((len / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), params, len);
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
ParamsLearner * p = new ParamsLearner(car_params, angle_offset, stiffness_factor, steer_ratio, learning_rate);
return (void*)p;
}
bool params_learner_update(void * params_learner, double psi, double u, double sa) {
ParamsLearner * p = (ParamsLearner*) params_learner;
return p->update(psi, u, sa);
}
double params_learner_get_ao(void * params_learner){
ParamsLearner * p = (ParamsLearner*) params_learner;
return p->ao;
}
double params_learner_get_x(void * params_learner){
ParamsLearner * p = (ParamsLearner*) params_learner;
return p->x;
}
double params_learner_get_slow_ao(void * params_learner){
ParamsLearner * p = (ParamsLearner*) params_learner;
return p->slow_ao;
}
double params_learner_get_sR(void * params_learner){
ParamsLearner * p = (ParamsLearner*) params_learner;
return p->sR;
}
}