diff --git a/cereal/car.capnp b/cereal/car.capnp index 056cbf8b0..02971fda4 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -530,14 +530,14 @@ struct CarParams { struct LateralTorqueTuning { useSteeringAngle @0 :Bool; - kp @1 :Float32; - ki @2 :Float32; - kd @8 :Float32; friction @3 :Float32; - kf @4 :Float32; steeringAngleDeadzoneDeg @5 :Float32; latAccelFactor @6 :Float32; latAccelOffset @7 :Float32; + kpDEPRECATED @1 :Float32; + kiDEPRECATED @2 :Float32; + kfDEPRECATED @4 :Float32; + kdDEPRECATED @8 : Float32; } struct LongitudinalPIDTuning { @@ -545,7 +545,7 @@ struct CarParams { kpV @1 :List(Float32); kiBP @2 :List(Float32); kiV @3 :List(Float32); - kf @6 :Float32; + kfDEPRECATED @6 :Float32; deadzoneBP @4 :List(Float32); deadzoneV @5 :List(Float32); } diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 029709a26..ad587c63a 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -106,10 +106,6 @@ struct FrogPilotCarParams @0xf35cc4560bbf6ec2 { openpilotLongitudinalControlDisabled @4 :Bool; safetyConfigs @5 :List(SafetyConfig); - lateralTuning :union { - pid @6 :Car.CarParams.LateralPIDTuning; - torque @7 :Car.CarParams.LateralTorqueTuning; - } struct SafetyConfig { safetyParam @0 :UInt16; diff --git a/cereal/gen/cpp/car.capnp.c++ b/cereal/gen/cpp/car.capnp.c++ index 0e1555736..8a48d82dc 100644 --- a/cereal/gen/cpp/car.capnp.c++ +++ b/cereal/gen/cpp/car.capnp.c++ @@ -5262,7 +5262,7 @@ const ::capnp::_::RawSchema s_9622723fcbd14c2e = { 0, 5, i_9622723fcbd14c2e, nullptr, nullptr, { &s_9622723fcbd14c2e, nullptr, nullptr, 0, 0, nullptr }, false }; #endif // !CAPNP_LITE -static const ::capnp::_::AlignedData<162> b_80366e0e804ecc1d = { +static const ::capnp::_::AlignedData<166> b_80366e0e804ecc1d = { { 0, 0, 0, 0, 5, 0, 6, 0, 29, 204, 78, 128, 14, 110, 54, 128, 20, 0, 0, 0, 1, 0, 5, 0, @@ -5289,62 +5289,62 @@ static const ::capnp::_::AlignedData<162> b_80366e0e804ecc1d = { 0, 0, 0, 0, 0, 0, 0, 0, 240, 0, 0, 0, 3, 0, 1, 0, 252, 0, 0, 0, 2, 0, 1, 0, - 1, 0, 0, 0, 1, 0, 0, 0, + 5, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 249, 0, 0, 0, 26, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 244, 0, 0, 0, 3, 0, 1, 0, - 0, 1, 0, 0, 2, 0, 1, 0, - 2, 0, 0, 0, 2, 0, 0, 0, - 0, 0, 1, 0, 2, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 253, 0, 0, 0, 26, 0, 0, 0, + 249, 0, 0, 0, 106, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 248, 0, 0, 0, 3, 0, 1, 0, 4, 1, 0, 0, 2, 0, 1, 0, - 4, 0, 0, 0, 3, 0, 0, 0, - 0, 0, 1, 0, 3, 0, 0, 0, + 6, 0, 0, 0, 2, 0, 0, 0, + 0, 0, 1, 0, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 1, 1, 0, 0, 74, 0, 0, 0, + 1, 1, 0, 0, 106, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 3, 0, 1, 0, 12, 1, 0, 0, 2, 0, 1, 0, - 5, 0, 0, 0, 4, 0, 0, 0, + 1, 0, 0, 0, 3, 0, 0, 0, + 0, 0, 1, 0, 3, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 9, 1, 0, 0, 74, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 8, 1, 0, 0, 3, 0, 1, 0, + 20, 1, 0, 0, 2, 0, 1, 0, + 7, 0, 0, 0, 4, 0, 0, 0, 0, 0, 1, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 9, 1, 0, 0, 26, 0, 0, 0, + 17, 1, 0, 0, 106, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 4, 1, 0, 0, 3, 0, 1, 0, - 16, 1, 0, 0, 2, 0, 1, 0, - 6, 0, 0, 0, 5, 0, 0, 0, + 16, 1, 0, 0, 3, 0, 1, 0, + 28, 1, 0, 0, 2, 0, 1, 0, + 2, 0, 0, 0, 5, 0, 0, 0, 0, 0, 1, 0, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 13, 1, 0, 0, 202, 0, 0, 0, + 25, 1, 0, 0, 202, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 20, 1, 0, 0, 3, 0, 1, 0, - 32, 1, 0, 0, 2, 0, 1, 0, - 7, 0, 0, 0, 6, 0, 0, 0, + 32, 1, 0, 0, 3, 0, 1, 0, + 44, 1, 0, 0, 2, 0, 1, 0, + 3, 0, 0, 0, 6, 0, 0, 0, 0, 0, 1, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 29, 1, 0, 0, 122, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 28, 1, 0, 0, 3, 0, 1, 0, - 40, 1, 0, 0, 2, 0, 1, 0, - 8, 0, 0, 0, 7, 0, 0, 0, - 0, 0, 1, 0, 7, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 37, 1, 0, 0, 122, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 36, 1, 0, 0, 3, 0, 1, 0, - 48, 1, 0, 0, 2, 0, 1, 0, - 3, 0, 0, 0, 8, 0, 0, 0, - 0, 0, 1, 0, 8, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 45, 1, 0, 0, 26, 0, 0, 0, + 41, 1, 0, 0, 122, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 40, 1, 0, 0, 3, 0, 1, 0, 52, 1, 0, 0, 2, 0, 1, 0, + 4, 0, 0, 0, 7, 0, 0, 0, + 0, 0, 1, 0, 7, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 49, 1, 0, 0, 122, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 48, 1, 0, 0, 3, 0, 1, 0, + 60, 1, 0, 0, 2, 0, 1, 0, + 8, 0, 0, 0, 8, 0, 0, 0, + 0, 0, 1, 0, 8, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 57, 1, 0, 0, 106, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 56, 1, 0, 0, 3, 0, 1, 0, + 68, 1, 0, 0, 2, 0, 1, 0, 117, 115, 101, 83, 116, 101, 101, 114, 105, 110, 103, 65, 110, 103, 108, 101, 0, 0, 0, 0, 0, 0, 0, 0, @@ -5355,7 +5355,8 @@ static const ::capnp::_::AlignedData<162> b_80366e0e804ecc1d = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 107, 112, 0, 0, 0, 0, 0, 0, + 107, 112, 68, 69, 80, 82, 69, 67, + 65, 84, 69, 68, 0, 0, 0, 0, 10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, @@ -5363,7 +5364,8 @@ static const ::capnp::_::AlignedData<162> b_80366e0e804ecc1d = { 10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 107, 105, 0, 0, 0, 0, 0, 0, + 107, 105, 68, 69, 80, 82, 69, 67, + 65, 84, 69, 68, 0, 0, 0, 0, 10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, @@ -5380,7 +5382,8 @@ static const ::capnp::_::AlignedData<162> b_80366e0e804ecc1d = { 10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 107, 102, 0, 0, 0, 0, 0, 0, + 107, 102, 68, 69, 80, 82, 69, 67, + 65, 84, 69, 68, 0, 0, 0, 0, 10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, @@ -5417,7 +5420,8 @@ static const ::capnp::_::AlignedData<162> b_80366e0e804ecc1d = { 10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 107, 100, 0, 0, 0, 0, 0, 0, + 107, 100, 68, 69, 80, 82, 69, 67, + 65, 84, 69, 68, 0, 0, 0, 0, 10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, @@ -5431,11 +5435,11 @@ static const ::capnp::_::AlignedData<162> b_80366e0e804ecc1d = { static const uint16_t m_80366e0e804ecc1d[] = {3, 8, 4, 2, 1, 6, 7, 5, 0}; static const uint16_t i_80366e0e804ecc1d[] = {0, 1, 2, 3, 4, 5, 6, 7, 8}; const ::capnp::_::RawSchema s_80366e0e804ecc1d = { - 0x80366e0e804ecc1d, b_80366e0e804ecc1d.words, 162, nullptr, m_80366e0e804ecc1d, + 0x80366e0e804ecc1d, b_80366e0e804ecc1d.words, 166, nullptr, m_80366e0e804ecc1d, 0, 9, i_80366e0e804ecc1d, nullptr, nullptr, { &s_80366e0e804ecc1d, nullptr, nullptr, 0, 0, nullptr }, false }; #endif // !CAPNP_LITE -static const ::capnp::_::AlignedData<151> b_c342cefc303e9b8e = { +static const ::capnp::_::AlignedData<152> b_c342cefc303e9b8e = { { 0, 0, 0, 0, 5, 0, 6, 0, 142, 155, 62, 48, 252, 206, 66, 195, 20, 0, 0, 0, 1, 0, 1, 0, @@ -5501,10 +5505,10 @@ static const ::capnp::_::AlignedData<151> b_c342cefc303e9b8e = { 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 53, 1, 0, 0, 26, 0, 0, 0, + 53, 1, 0, 0, 106, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 48, 1, 0, 0, 3, 0, 1, 0, - 60, 1, 0, 0, 2, 0, 1, 0, + 52, 1, 0, 0, 3, 0, 1, 0, + 64, 1, 0, 0, 2, 0, 1, 0, 107, 112, 66, 80, 0, 0, 0, 0, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, @@ -5579,7 +5583,8 @@ static const ::capnp::_::AlignedData<151> b_c342cefc303e9b8e = { 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 107, 102, 0, 0, 0, 0, 0, 0, + 107, 102, 68, 69, 80, 82, 69, 67, + 65, 84, 69, 68, 0, 0, 0, 0, 10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, @@ -5593,7 +5598,7 @@ static const ::capnp::_::AlignedData<151> b_c342cefc303e9b8e = { static const uint16_t m_c342cefc303e9b8e[] = {4, 5, 6, 2, 3, 0, 1}; static const uint16_t i_c342cefc303e9b8e[] = {0, 1, 2, 3, 4, 5, 6}; const ::capnp::_::RawSchema s_c342cefc303e9b8e = { - 0xc342cefc303e9b8e, b_c342cefc303e9b8e.words, 151, nullptr, m_c342cefc303e9b8e, + 0xc342cefc303e9b8e, b_c342cefc303e9b8e.words, 152, nullptr, m_c342cefc303e9b8e, 0, 7, i_c342cefc303e9b8e, nullptr, nullptr, { &s_c342cefc303e9b8e, nullptr, nullptr, 0, 0, nullptr }, false }; #endif // !CAPNP_LITE diff --git a/cereal/gen/cpp/car.capnp.h b/cereal/gen/cpp/car.capnp.h index dbd41180a..20a2e4aa1 100644 --- a/cereal/gen/cpp/car.capnp.h +++ b/cereal/gen/cpp/car.capnp.h @@ -3134,13 +3134,13 @@ public: inline bool getUseSteeringAngle() const; - inline float getKp() const; + inline float getKpDEPRECATED() const; - inline float getKi() const; + inline float getKiDEPRECATED() const; inline float getFriction() const; - inline float getKf() const; + inline float getKfDEPRECATED() const; inline float getSteeringAngleDeadzoneDeg() const; @@ -3148,7 +3148,7 @@ public: inline float getLatAccelOffset() const; - inline float getKd() const; + inline float getKdDEPRECATED() const; private: ::capnp::_::StructReader _reader; @@ -3181,17 +3181,17 @@ public: inline bool getUseSteeringAngle(); inline void setUseSteeringAngle(bool value); - inline float getKp(); - inline void setKp(float value); + inline float getKpDEPRECATED(); + inline void setKpDEPRECATED(float value); - inline float getKi(); - inline void setKi(float value); + inline float getKiDEPRECATED(); + inline void setKiDEPRECATED(float value); inline float getFriction(); inline void setFriction(float value); - inline float getKf(); - inline void setKf(float value); + inline float getKfDEPRECATED(); + inline void setKfDEPRECATED(float value); inline float getSteeringAngleDeadzoneDeg(); inline void setSteeringAngleDeadzoneDeg(float value); @@ -3202,8 +3202,8 @@ public: inline float getLatAccelOffset(); inline void setLatAccelOffset(float value); - inline float getKd(); - inline void setKd(float value); + inline float getKdDEPRECATED(); + inline void setKdDEPRECATED(float value); private: ::capnp::_::StructBuilder _builder; @@ -3266,7 +3266,7 @@ public: inline bool hasDeadzoneV() const; inline ::capnp::List::Reader getDeadzoneV() const; - inline float getKf() const; + inline float getKfDEPRECATED() const; private: ::capnp::_::StructReader _reader; @@ -3344,8 +3344,8 @@ public: inline void adoptDeadzoneV(::capnp::Orphan< ::capnp::List>&& value); inline ::capnp::Orphan< ::capnp::List> disownDeadzoneV(); - inline float getKf(); - inline void setKf(float value); + inline float getKfDEPRECATED(); + inline void setKfDEPRECATED(float value); private: ::capnp::_::StructBuilder _builder; @@ -7754,30 +7754,30 @@ inline void CarParams::LateralTorqueTuning::Builder::setUseSteeringAngle(bool va ::capnp::bounded<0>() * ::capnp::ELEMENTS, value); } -inline float CarParams::LateralTorqueTuning::Reader::getKp() const { +inline float CarParams::LateralTorqueTuning::Reader::getKpDEPRECATED() const { return _reader.getDataField( ::capnp::bounded<1>() * ::capnp::ELEMENTS); } -inline float CarParams::LateralTorqueTuning::Builder::getKp() { +inline float CarParams::LateralTorqueTuning::Builder::getKpDEPRECATED() { return _builder.getDataField( ::capnp::bounded<1>() * ::capnp::ELEMENTS); } -inline void CarParams::LateralTorqueTuning::Builder::setKp(float value) { +inline void CarParams::LateralTorqueTuning::Builder::setKpDEPRECATED(float value) { _builder.setDataField( ::capnp::bounded<1>() * ::capnp::ELEMENTS, value); } -inline float CarParams::LateralTorqueTuning::Reader::getKi() const { +inline float CarParams::LateralTorqueTuning::Reader::getKiDEPRECATED() const { return _reader.getDataField( ::capnp::bounded<2>() * ::capnp::ELEMENTS); } -inline float CarParams::LateralTorqueTuning::Builder::getKi() { +inline float CarParams::LateralTorqueTuning::Builder::getKiDEPRECATED() { return _builder.getDataField( ::capnp::bounded<2>() * ::capnp::ELEMENTS); } -inline void CarParams::LateralTorqueTuning::Builder::setKi(float value) { +inline void CarParams::LateralTorqueTuning::Builder::setKiDEPRECATED(float value) { _builder.setDataField( ::capnp::bounded<2>() * ::capnp::ELEMENTS, value); } @@ -7796,16 +7796,16 @@ inline void CarParams::LateralTorqueTuning::Builder::setFriction(float value) { ::capnp::bounded<3>() * ::capnp::ELEMENTS, value); } -inline float CarParams::LateralTorqueTuning::Reader::getKf() const { +inline float CarParams::LateralTorqueTuning::Reader::getKfDEPRECATED() const { return _reader.getDataField( ::capnp::bounded<4>() * ::capnp::ELEMENTS); } -inline float CarParams::LateralTorqueTuning::Builder::getKf() { +inline float CarParams::LateralTorqueTuning::Builder::getKfDEPRECATED() { return _builder.getDataField( ::capnp::bounded<4>() * ::capnp::ELEMENTS); } -inline void CarParams::LateralTorqueTuning::Builder::setKf(float value) { +inline void CarParams::LateralTorqueTuning::Builder::setKfDEPRECATED(float value) { _builder.setDataField( ::capnp::bounded<4>() * ::capnp::ELEMENTS, value); } @@ -7852,16 +7852,16 @@ inline void CarParams::LateralTorqueTuning::Builder::setLatAccelOffset(float val ::capnp::bounded<7>() * ::capnp::ELEMENTS, value); } -inline float CarParams::LateralTorqueTuning::Reader::getKd() const { +inline float CarParams::LateralTorqueTuning::Reader::getKdDEPRECATED() const { return _reader.getDataField( ::capnp::bounded<8>() * ::capnp::ELEMENTS); } -inline float CarParams::LateralTorqueTuning::Builder::getKd() { +inline float CarParams::LateralTorqueTuning::Builder::getKdDEPRECATED() { return _builder.getDataField( ::capnp::bounded<8>() * ::capnp::ELEMENTS); } -inline void CarParams::LateralTorqueTuning::Builder::setKd(float value) { +inline void CarParams::LateralTorqueTuning::Builder::setKdDEPRECATED(float value) { _builder.setDataField( ::capnp::bounded<8>() * ::capnp::ELEMENTS, value); } @@ -8094,16 +8094,16 @@ inline ::capnp::Orphan< ::capnp::List> CarPara ::capnp::bounded<5>() * ::capnp::POINTERS)); } -inline float CarParams::LongitudinalPIDTuning::Reader::getKf() const { +inline float CarParams::LongitudinalPIDTuning::Reader::getKfDEPRECATED() const { return _reader.getDataField( ::capnp::bounded<0>() * ::capnp::ELEMENTS); } -inline float CarParams::LongitudinalPIDTuning::Builder::getKf() { +inline float CarParams::LongitudinalPIDTuning::Builder::getKfDEPRECATED() { return _builder.getDataField( ::capnp::bounded<0>() * ::capnp::ELEMENTS); } -inline void CarParams::LongitudinalPIDTuning::Builder::setKf(float value) { +inline void CarParams::LongitudinalPIDTuning::Builder::setKfDEPRECATED(float value) { _builder.setDataField( ::capnp::bounded<0>() * ::capnp::ELEMENTS, value); } diff --git a/cereal/gen/cpp/custom.capnp.c++ b/cereal/gen/cpp/custom.capnp.c++ index 5d751d44b..ba0109f47 100644 --- a/cereal/gen/cpp/custom.capnp.c++ +++ b/cereal/gen/cpp/custom.capnp.c++ @@ -642,17 +642,17 @@ const ::capnp::_::RawSchema s_aedffd8f31e7b55d = { }; #endif // !CAPNP_LITE CAPNP_DEFINE_ENUM(EventName_aedffd8f31e7b55d, aedffd8f31e7b55d); -static const ::capnp::_::AlignedData<132> b_f35cc4560bbf6ec2 = { +static const ::capnp::_::AlignedData<123> b_f35cc4560bbf6ec2 = { { 0, 0, 0, 0, 5, 0, 6, 0, 194, 110, 191, 11, 86, 196, 92, 243, 13, 0, 0, 0, 1, 0, 1, 0, 89, 10, 85, 29, 102, 186, 38, 181, - 2, 0, 7, 0, 0, 0, 0, 0, + 1, 0, 7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 21, 0, 0, 0, 2, 1, 0, 0, 33, 0, 0, 0, 23, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 45, 0, 0, 0, 143, 1, 0, 0, + 45, 0, 0, 0, 87, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 99, 117, 115, 116, 111, 109, 46, 99, @@ -664,56 +664,49 @@ static const ::capnp::_::AlignedData<132> b_f35cc4560bbf6ec2 = { 1, 0, 0, 0, 106, 0, 0, 0, 83, 97, 102, 101, 116, 121, 67, 111, 110, 102, 105, 103, 0, 0, 0, 0, - 28, 0, 0, 0, 3, 0, 4, 0, + 24, 0, 0, 0, 3, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 181, 0, 0, 0, 98, 0, 0, 0, + 153, 0, 0, 0, 98, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 180, 0, 0, 0, 3, 0, 1, 0, - 192, 0, 0, 0, 2, 0, 1, 0, + 152, 0, 0, 0, 3, 0, 1, 0, + 164, 0, 0, 0, 2, 0, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 189, 0, 0, 0, 90, 0, 0, 0, + 161, 0, 0, 0, 90, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 188, 0, 0, 0, 3, 0, 1, 0, - 200, 0, 0, 0, 2, 0, 1, 0, + 160, 0, 0, 0, 3, 0, 1, 0, + 172, 0, 0, 0, 2, 0, 1, 0, 2, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 197, 0, 0, 0, 66, 0, 0, 0, + 169, 0, 0, 0, 66, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 192, 0, 0, 0, 3, 0, 1, 0, - 204, 0, 0, 0, 2, 0, 1, 0, + 164, 0, 0, 0, 3, 0, 1, 0, + 176, 0, 0, 0, 2, 0, 1, 0, 3, 0, 0, 0, 2, 0, 0, 0, 0, 0, 1, 0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 201, 0, 0, 0, 58, 0, 0, 0, + 173, 0, 0, 0, 58, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 196, 0, 0, 0, 3, 0, 1, 0, - 208, 0, 0, 0, 2, 0, 1, 0, + 168, 0, 0, 0, 3, 0, 1, 0, + 180, 0, 0, 0, 2, 0, 1, 0, 4, 0, 0, 0, 3, 0, 0, 0, 0, 0, 1, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 205, 0, 0, 0, 42, 1, 0, 0, + 177, 0, 0, 0, 42, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 216, 0, 0, 0, 3, 0, 1, 0, - 228, 0, 0, 0, 2, 0, 1, 0, + 188, 0, 0, 0, 3, 0, 1, 0, + 200, 0, 0, 0, 2, 0, 1, 0, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 225, 0, 0, 0, 114, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 224, 0, 0, 0, 3, 0, 1, 0, - 252, 0, 0, 0, 2, 0, 1, 0, - 6, 0, 0, 0, 0, 0, 0, 0, - 1, 0, 0, 0, 0, 0, 0, 0, - 73, 118, 234, 203, 210, 73, 201, 252, - 249, 0, 0, 0, 114, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, + 197, 0, 0, 0, 114, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 196, 0, 0, 0, 3, 0, 1, 0, + 224, 0, 0, 0, 2, 0, 1, 0, 99, 97, 110, 85, 115, 101, 80, 101, 100, 97, 108, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, @@ -772,21 +765,18 @@ static const ::capnp::_::AlignedData<132> b_f35cc4560bbf6ec2 = { 0, 0, 0, 0, 0, 0, 0, 0, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 108, 97, 116, 101, 114, 97, 108, 84, - 117, 110, 105, 110, 103, 0, 0, 0, } + 0, 0, 0, 0, 0, 0, 0, 0, } }; ::capnp::word const* const bp_f35cc4560bbf6ec2 = b_f35cc4560bbf6ec2.words; #if !CAPNP_LITE static const ::capnp::_::RawSchema* const d_f35cc4560bbf6ec2[] = { &s_8d65dd40bad40951, - &s_fcc949d2cbea7649, }; -static const uint16_t m_f35cc4560bbf6ec2[] = {0, 1, 2, 3, 6, 4, 5}; -static const uint16_t i_f35cc4560bbf6ec2[] = {0, 1, 2, 3, 4, 5, 6}; +static const uint16_t m_f35cc4560bbf6ec2[] = {0, 1, 2, 3, 4, 5}; +static const uint16_t i_f35cc4560bbf6ec2[] = {0, 1, 2, 3, 4, 5}; const ::capnp::_::RawSchema s_f35cc4560bbf6ec2 = { - 0xf35cc4560bbf6ec2, b_f35cc4560bbf6ec2.words, 132, d_f35cc4560bbf6ec2, m_f35cc4560bbf6ec2, - 2, 7, i_f35cc4560bbf6ec2, nullptr, nullptr, { &s_f35cc4560bbf6ec2, nullptr, nullptr, 0, 0, nullptr }, false + 0xf35cc4560bbf6ec2, b_f35cc4560bbf6ec2.words, 123, d_f35cc4560bbf6ec2, m_f35cc4560bbf6ec2, + 1, 6, i_f35cc4560bbf6ec2, nullptr, nullptr, { &s_f35cc4560bbf6ec2, nullptr, nullptr, 0, 0, nullptr }, false }; #endif // !CAPNP_LITE static const ::capnp::_::AlignedData<36> b_8d65dd40bad40951 = { @@ -836,71 +826,6 @@ const ::capnp::_::RawSchema s_8d65dd40bad40951 = { 0, 1, i_8d65dd40bad40951, nullptr, nullptr, { &s_8d65dd40bad40951, nullptr, nullptr, 0, 0, nullptr }, false }; #endif // !CAPNP_LITE -static const ::capnp::_::AlignedData<49> b_fcc949d2cbea7649 = { - { 0, 0, 0, 0, 5, 0, 6, 0, - 73, 118, 234, 203, 210, 73, 201, 252, - 32, 0, 0, 0, 1, 0, 1, 0, - 194, 110, 191, 11, 86, 196, 92, 243, - 2, 0, 7, 0, 1, 0, 2, 0, - 1, 0, 0, 0, 0, 0, 0, 0, - 21, 0, 0, 0, 114, 1, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 33, 0, 0, 0, 119, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 99, 117, 115, 116, 111, 109, 46, 99, - 97, 112, 110, 112, 58, 70, 114, 111, - 103, 80, 105, 108, 111, 116, 67, 97, - 114, 80, 97, 114, 97, 109, 115, 46, - 108, 97, 116, 101, 114, 97, 108, 84, - 117, 110, 105, 110, 103, 0, 0, 0, - 8, 0, 0, 0, 3, 0, 4, 0, - 0, 0, 255, 255, 1, 0, 0, 0, - 0, 0, 1, 0, 6, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 41, 0, 0, 0, 34, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 36, 0, 0, 0, 3, 0, 1, 0, - 48, 0, 0, 0, 2, 0, 1, 0, - 1, 0, 254, 255, 1, 0, 0, 0, - 0, 0, 1, 0, 7, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 45, 0, 0, 0, 58, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 40, 0, 0, 0, 3, 0, 1, 0, - 52, 0, 0, 0, 2, 0, 1, 0, - 112, 105, 100, 0, 0, 0, 0, 0, - 16, 0, 0, 0, 0, 0, 0, 0, - 46, 76, 209, 203, 63, 114, 34, 150, - 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 16, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 116, 111, 114, 113, 117, 101, 0, 0, - 16, 0, 0, 0, 0, 0, 0, 0, - 29, 204, 78, 128, 14, 110, 54, 128, - 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 16, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, } -}; -::capnp::word const* const bp_fcc949d2cbea7649 = b_fcc949d2cbea7649.words; -#if !CAPNP_LITE -static const ::capnp::_::RawSchema* const d_fcc949d2cbea7649[] = { - &s_80366e0e804ecc1d, - &s_9622723fcbd14c2e, - &s_f35cc4560bbf6ec2, -}; -static const uint16_t m_fcc949d2cbea7649[] = {0, 1}; -static const uint16_t i_fcc949d2cbea7649[] = {0, 1}; -const ::capnp::_::RawSchema s_fcc949d2cbea7649 = { - 0xfcc949d2cbea7649, b_fcc949d2cbea7649.words, 49, d_fcc949d2cbea7649, m_fcc949d2cbea7649, - 3, 2, i_fcc949d2cbea7649, nullptr, nullptr, { &s_fcc949d2cbea7649, nullptr, nullptr, 0, 0, nullptr }, false -}; -#endif // !CAPNP_LITE static const ::capnp::_::AlignedData<268> b_da96579883444c35 = { { 0, 0, 0, 0, 5, 0, 6, 0, 53, 76, 68, 131, 152, 87, 150, 218, @@ -2777,18 +2702,6 @@ constexpr ::capnp::_::RawSchema const* FrogPilotCarParams::SafetyConfig::_capnpP #endif // !CAPNP_NEED_REDUNDANT_CONSTEXPR_DECL #endif // !CAPNP_LITE -// FrogPilotCarParams::LateralTuning -#if CAPNP_NEED_REDUNDANT_CONSTEXPR_DECL -constexpr uint16_t FrogPilotCarParams::LateralTuning::_capnpPrivate::dataWordSize; -constexpr uint16_t FrogPilotCarParams::LateralTuning::_capnpPrivate::pointerCount; -#endif // !CAPNP_NEED_REDUNDANT_CONSTEXPR_DECL -#if !CAPNP_LITE -#if CAPNP_NEED_REDUNDANT_CONSTEXPR_DECL -constexpr ::capnp::Kind FrogPilotCarParams::LateralTuning::_capnpPrivate::kind; -constexpr ::capnp::_::RawSchema const* FrogPilotCarParams::LateralTuning::_capnpPrivate::schema; -#endif // !CAPNP_NEED_REDUNDANT_CONSTEXPR_DECL -#endif // !CAPNP_LITE - // FrogPilotCarState #if CAPNP_NEED_REDUNDANT_CONSTEXPR_DECL constexpr uint16_t FrogPilotCarState::_capnpPrivate::dataWordSize; diff --git a/cereal/gen/cpp/custom.capnp.h b/cereal/gen/cpp/custom.capnp.h index 043621b64..248fd8f9d 100644 --- a/cereal/gen/cpp/custom.capnp.h +++ b/cereal/gen/cpp/custom.capnp.h @@ -84,7 +84,6 @@ enum class EventName_aedffd8f31e7b55d: uint16_t { CAPNP_DECLARE_ENUM(EventName, aedffd8f31e7b55d); CAPNP_DECLARE_SCHEMA(f35cc4560bbf6ec2); CAPNP_DECLARE_SCHEMA(8d65dd40bad40951); -CAPNP_DECLARE_SCHEMA(fcc949d2cbea7649); CAPNP_DECLARE_SCHEMA(da96579883444c35); CAPNP_DECLARE_SCHEMA(ccb4d6b0dc102d40); CAPNP_DECLARE_SCHEMA(8033e8e60d6a0edb); @@ -185,10 +184,9 @@ struct FrogPilotCarParams { class Builder; class Pipeline; struct SafetyConfig; - struct LateralTuning; struct _capnpPrivate { - CAPNP_DECLARE_STRUCT_HEADER(f35cc4560bbf6ec2, 1, 2) + CAPNP_DECLARE_STRUCT_HEADER(f35cc4560bbf6ec2, 1, 1) #if !CAPNP_LITE static constexpr ::capnp::_::RawBrandedSchema const* brand() { return &schema->defaultBrand; } #endif // !CAPNP_LITE @@ -210,25 +208,6 @@ struct FrogPilotCarParams::SafetyConfig { }; }; -struct FrogPilotCarParams::LateralTuning { - LateralTuning() = delete; - - class Reader; - class Builder; - class Pipeline; - enum Which: uint16_t { - PID, - TORQUE, - }; - - struct _capnpPrivate { - CAPNP_DECLARE_STRUCT_HEADER(fcc949d2cbea7649, 1, 2) - #if !CAPNP_LITE - static constexpr ::capnp::_::RawBrandedSchema const* brand() { return &schema->defaultBrand; } - #endif // !CAPNP_LITE - }; -}; - struct FrogPilotCarState { FrogPilotCarState() = delete; @@ -690,8 +669,6 @@ public: inline bool hasSafetyConfigs() const; inline ::capnp::List< ::cereal::FrogPilotCarParams::SafetyConfig, ::capnp::Kind::STRUCT>::Reader getSafetyConfigs() const; - inline typename LateralTuning::Reader getLateralTuning() const; - private: ::capnp::_::StructReader _reader; template @@ -742,9 +719,6 @@ public: inline void adoptSafetyConfigs(::capnp::Orphan< ::capnp::List< ::cereal::FrogPilotCarParams::SafetyConfig, ::capnp::Kind::STRUCT>>&& value); inline ::capnp::Orphan< ::capnp::List< ::cereal::FrogPilotCarParams::SafetyConfig, ::capnp::Kind::STRUCT>> disownSafetyConfigs(); - inline typename LateralTuning::Builder getLateralTuning(); - inline typename LateralTuning::Builder initLateralTuning(); - private: ::capnp::_::StructBuilder _builder; template @@ -763,7 +737,6 @@ public: inline explicit Pipeline(::capnp::AnyPointer::Pipeline&& typeless) : _typeless(kj::mv(typeless)) {} - inline typename LateralTuning::Pipeline getLateralTuning(); private: ::capnp::AnyPointer::Pipeline _typeless; friend class ::capnp::PipelineHook; @@ -848,103 +821,6 @@ private: }; #endif // !CAPNP_LITE -class FrogPilotCarParams::LateralTuning::Reader { -public: - typedef LateralTuning Reads; - - Reader() = default; - inline explicit Reader(::capnp::_::StructReader base): _reader(base) {} - - inline ::capnp::MessageSize totalSize() const { - return _reader.totalSize().asPublic(); - } - -#if !CAPNP_LITE - inline ::kj::StringTree toString() const { - return ::capnp::_::structString(_reader, *_capnpPrivate::brand()); - } -#endif // !CAPNP_LITE - - inline Which which() const; - inline bool isPid() const; - inline bool hasPid() const; - inline ::cereal::CarParams::LateralPIDTuning::Reader getPid() const; - - inline bool isTorque() const; - inline bool hasTorque() const; - inline ::cereal::CarParams::LateralTorqueTuning::Reader getTorque() const; - -private: - ::capnp::_::StructReader _reader; - template - friend struct ::capnp::ToDynamic_; - template - friend struct ::capnp::_::PointerHelpers; - template - friend struct ::capnp::List; - friend class ::capnp::MessageBuilder; - friend class ::capnp::Orphanage; -}; - -class FrogPilotCarParams::LateralTuning::Builder { -public: - typedef LateralTuning Builds; - - Builder() = delete; // Deleted to discourage incorrect usage. - // You can explicitly initialize to nullptr instead. - inline Builder(decltype(nullptr)) {} - inline explicit Builder(::capnp::_::StructBuilder base): _builder(base) {} - inline operator Reader() const { return Reader(_builder.asReader()); } - inline Reader asReader() const { return *this; } - - inline ::capnp::MessageSize totalSize() const { return asReader().totalSize(); } -#if !CAPNP_LITE - inline ::kj::StringTree toString() const { return asReader().toString(); } -#endif // !CAPNP_LITE - - inline Which which(); - inline bool isPid(); - inline bool hasPid(); - inline ::cereal::CarParams::LateralPIDTuning::Builder getPid(); - inline void setPid( ::cereal::CarParams::LateralPIDTuning::Reader value); - inline ::cereal::CarParams::LateralPIDTuning::Builder initPid(); - inline void adoptPid(::capnp::Orphan< ::cereal::CarParams::LateralPIDTuning>&& value); - inline ::capnp::Orphan< ::cereal::CarParams::LateralPIDTuning> disownPid(); - - inline bool isTorque(); - inline bool hasTorque(); - inline ::cereal::CarParams::LateralTorqueTuning::Builder getTorque(); - inline void setTorque( ::cereal::CarParams::LateralTorqueTuning::Reader value); - inline ::cereal::CarParams::LateralTorqueTuning::Builder initTorque(); - inline void adoptTorque(::capnp::Orphan< ::cereal::CarParams::LateralTorqueTuning>&& value); - inline ::capnp::Orphan< ::cereal::CarParams::LateralTorqueTuning> disownTorque(); - -private: - ::capnp::_::StructBuilder _builder; - template - friend struct ::capnp::ToDynamic_; - friend class ::capnp::Orphanage; - template - friend struct ::capnp::_::PointerHelpers; -}; - -#if !CAPNP_LITE -class FrogPilotCarParams::LateralTuning::Pipeline { -public: - typedef LateralTuning Pipelines; - - inline Pipeline(decltype(nullptr)): _typeless(nullptr) {} - inline explicit Pipeline(::capnp::AnyPointer::Pipeline&& typeless) - : _typeless(kj::mv(typeless)) {} - -private: - ::capnp::AnyPointer::Pipeline _typeless; - friend class ::capnp::PipelineHook; - template - friend struct ::capnp::ToDynamic_; -}; -#endif // !CAPNP_LITE - class FrogPilotCarState::Reader { public: typedef FrogPilotCarState Reads; @@ -2344,22 +2220,6 @@ inline ::capnp::Orphan< ::capnp::List< ::cereal::FrogPilotCarParams::SafetyConfi ::capnp::bounded<0>() * ::capnp::POINTERS)); } -inline typename FrogPilotCarParams::LateralTuning::Reader FrogPilotCarParams::Reader::getLateralTuning() const { - return typename FrogPilotCarParams::LateralTuning::Reader(_reader); -} -inline typename FrogPilotCarParams::LateralTuning::Builder FrogPilotCarParams::Builder::getLateralTuning() { - return typename FrogPilotCarParams::LateralTuning::Builder(_builder); -} -#if !CAPNP_LITE -inline typename FrogPilotCarParams::LateralTuning::Pipeline FrogPilotCarParams::Pipeline::getLateralTuning() { - return typename FrogPilotCarParams::LateralTuning::Pipeline(_typeless.noop()); -} -#endif // !CAPNP_LITE -inline typename FrogPilotCarParams::LateralTuning::Builder FrogPilotCarParams::Builder::initLateralTuning() { - _builder.setDataField< ::uint16_t>(::capnp::bounded<1>() * ::capnp::ELEMENTS, 0); - _builder.getPointerField(::capnp::bounded<1>() * ::capnp::POINTERS).clear(); - return typename FrogPilotCarParams::LateralTuning::Builder(_builder); -} inline ::uint16_t FrogPilotCarParams::SafetyConfig::Reader::getSafetyParam() const { return _reader.getDataField< ::uint16_t>( ::capnp::bounded<0>() * ::capnp::ELEMENTS); @@ -2374,123 +2234,6 @@ inline void FrogPilotCarParams::SafetyConfig::Builder::setSafetyParam( ::uint16_ ::capnp::bounded<0>() * ::capnp::ELEMENTS, value); } -inline ::cereal::FrogPilotCarParams::LateralTuning::Which FrogPilotCarParams::LateralTuning::Reader::which() const { - return _reader.getDataField( - ::capnp::bounded<1>() * ::capnp::ELEMENTS); -} -inline ::cereal::FrogPilotCarParams::LateralTuning::Which FrogPilotCarParams::LateralTuning::Builder::which() { - return _builder.getDataField( - ::capnp::bounded<1>() * ::capnp::ELEMENTS); -} - -inline bool FrogPilotCarParams::LateralTuning::Reader::isPid() const { - return which() == FrogPilotCarParams::LateralTuning::PID; -} -inline bool FrogPilotCarParams::LateralTuning::Builder::isPid() { - return which() == FrogPilotCarParams::LateralTuning::PID; -} -inline bool FrogPilotCarParams::LateralTuning::Reader::hasPid() const { - if (which() != FrogPilotCarParams::LateralTuning::PID) return false; - return !_reader.getPointerField( - ::capnp::bounded<1>() * ::capnp::POINTERS).isNull(); -} -inline bool FrogPilotCarParams::LateralTuning::Builder::hasPid() { - if (which() != FrogPilotCarParams::LateralTuning::PID) return false; - return !_builder.getPointerField( - ::capnp::bounded<1>() * ::capnp::POINTERS).isNull(); -} -inline ::cereal::CarParams::LateralPIDTuning::Reader FrogPilotCarParams::LateralTuning::Reader::getPid() const { - KJ_IREQUIRE((which() == FrogPilotCarParams::LateralTuning::PID), - "Must check which() before get()ing a union member."); - return ::capnp::_::PointerHelpers< ::cereal::CarParams::LateralPIDTuning>::get(_reader.getPointerField( - ::capnp::bounded<1>() * ::capnp::POINTERS)); -} -inline ::cereal::CarParams::LateralPIDTuning::Builder FrogPilotCarParams::LateralTuning::Builder::getPid() { - KJ_IREQUIRE((which() == FrogPilotCarParams::LateralTuning::PID), - "Must check which() before get()ing a union member."); - return ::capnp::_::PointerHelpers< ::cereal::CarParams::LateralPIDTuning>::get(_builder.getPointerField( - ::capnp::bounded<1>() * ::capnp::POINTERS)); -} -inline void FrogPilotCarParams::LateralTuning::Builder::setPid( ::cereal::CarParams::LateralPIDTuning::Reader value) { - _builder.setDataField( - ::capnp::bounded<1>() * ::capnp::ELEMENTS, FrogPilotCarParams::LateralTuning::PID); - ::capnp::_::PointerHelpers< ::cereal::CarParams::LateralPIDTuning>::set(_builder.getPointerField( - ::capnp::bounded<1>() * ::capnp::POINTERS), value); -} -inline ::cereal::CarParams::LateralPIDTuning::Builder FrogPilotCarParams::LateralTuning::Builder::initPid() { - _builder.setDataField( - ::capnp::bounded<1>() * ::capnp::ELEMENTS, FrogPilotCarParams::LateralTuning::PID); - return ::capnp::_::PointerHelpers< ::cereal::CarParams::LateralPIDTuning>::init(_builder.getPointerField( - ::capnp::bounded<1>() * ::capnp::POINTERS)); -} -inline void FrogPilotCarParams::LateralTuning::Builder::adoptPid( - ::capnp::Orphan< ::cereal::CarParams::LateralPIDTuning>&& value) { - _builder.setDataField( - ::capnp::bounded<1>() * ::capnp::ELEMENTS, FrogPilotCarParams::LateralTuning::PID); - ::capnp::_::PointerHelpers< ::cereal::CarParams::LateralPIDTuning>::adopt(_builder.getPointerField( - ::capnp::bounded<1>() * ::capnp::POINTERS), kj::mv(value)); -} -inline ::capnp::Orphan< ::cereal::CarParams::LateralPIDTuning> FrogPilotCarParams::LateralTuning::Builder::disownPid() { - KJ_IREQUIRE((which() == FrogPilotCarParams::LateralTuning::PID), - "Must check which() before get()ing a union member."); - return ::capnp::_::PointerHelpers< ::cereal::CarParams::LateralPIDTuning>::disown(_builder.getPointerField( - ::capnp::bounded<1>() * ::capnp::POINTERS)); -} - -inline bool FrogPilotCarParams::LateralTuning::Reader::isTorque() const { - return which() == FrogPilotCarParams::LateralTuning::TORQUE; -} -inline bool FrogPilotCarParams::LateralTuning::Builder::isTorque() { - return which() == FrogPilotCarParams::LateralTuning::TORQUE; -} -inline bool FrogPilotCarParams::LateralTuning::Reader::hasTorque() const { - if (which() != FrogPilotCarParams::LateralTuning::TORQUE) return false; - return !_reader.getPointerField( - ::capnp::bounded<1>() * ::capnp::POINTERS).isNull(); -} -inline bool FrogPilotCarParams::LateralTuning::Builder::hasTorque() { - if (which() != FrogPilotCarParams::LateralTuning::TORQUE) return false; - return !_builder.getPointerField( - ::capnp::bounded<1>() * ::capnp::POINTERS).isNull(); -} -inline ::cereal::CarParams::LateralTorqueTuning::Reader FrogPilotCarParams::LateralTuning::Reader::getTorque() const { - KJ_IREQUIRE((which() == FrogPilotCarParams::LateralTuning::TORQUE), - "Must check which() before get()ing a union member."); - return ::capnp::_::PointerHelpers< ::cereal::CarParams::LateralTorqueTuning>::get(_reader.getPointerField( - ::capnp::bounded<1>() * ::capnp::POINTERS)); -} -inline ::cereal::CarParams::LateralTorqueTuning::Builder FrogPilotCarParams::LateralTuning::Builder::getTorque() { - KJ_IREQUIRE((which() == FrogPilotCarParams::LateralTuning::TORQUE), - "Must check which() before get()ing a union member."); - return ::capnp::_::PointerHelpers< ::cereal::CarParams::LateralTorqueTuning>::get(_builder.getPointerField( - ::capnp::bounded<1>() * ::capnp::POINTERS)); -} -inline void FrogPilotCarParams::LateralTuning::Builder::setTorque( ::cereal::CarParams::LateralTorqueTuning::Reader value) { - _builder.setDataField( - ::capnp::bounded<1>() * ::capnp::ELEMENTS, FrogPilotCarParams::LateralTuning::TORQUE); - ::capnp::_::PointerHelpers< ::cereal::CarParams::LateralTorqueTuning>::set(_builder.getPointerField( - ::capnp::bounded<1>() * ::capnp::POINTERS), value); -} -inline ::cereal::CarParams::LateralTorqueTuning::Builder FrogPilotCarParams::LateralTuning::Builder::initTorque() { - _builder.setDataField( - ::capnp::bounded<1>() * ::capnp::ELEMENTS, FrogPilotCarParams::LateralTuning::TORQUE); - return ::capnp::_::PointerHelpers< ::cereal::CarParams::LateralTorqueTuning>::init(_builder.getPointerField( - ::capnp::bounded<1>() * ::capnp::POINTERS)); -} -inline void FrogPilotCarParams::LateralTuning::Builder::adoptTorque( - ::capnp::Orphan< ::cereal::CarParams::LateralTorqueTuning>&& value) { - _builder.setDataField( - ::capnp::bounded<1>() * ::capnp::ELEMENTS, FrogPilotCarParams::LateralTuning::TORQUE); - ::capnp::_::PointerHelpers< ::cereal::CarParams::LateralTorqueTuning>::adopt(_builder.getPointerField( - ::capnp::bounded<1>() * ::capnp::POINTERS), kj::mv(value)); -} -inline ::capnp::Orphan< ::cereal::CarParams::LateralTorqueTuning> FrogPilotCarParams::LateralTuning::Builder::disownTorque() { - KJ_IREQUIRE((which() == FrogPilotCarParams::LateralTuning::TORQUE), - "Must check which() before get()ing a union member."); - return ::capnp::_::PointerHelpers< ::cereal::CarParams::LateralTorqueTuning>::disown(_builder.getPointerField( - ::capnp::bounded<1>() * ::capnp::POINTERS)); -} - inline bool FrogPilotCarState::Reader::getAccelPressed() const { return _reader.getDataField( ::capnp::bounded<0>() * ::capnp::ELEMENTS); diff --git a/cereal/gen/cpp/log.capnp.c++ b/cereal/gen/cpp/log.capnp.c++ index 2561497b4..487d3ae30 100644 --- a/cereal/gen/cpp/log.capnp.c++ +++ b/cereal/gen/cpp/log.capnp.c++ @@ -9336,17 +9336,17 @@ const ::capnp::_::RawSchema s_f28c5dc9e09375e3 = { 0, 10, i_f28c5dc9e09375e3, nullptr, nullptr, { &s_f28c5dc9e09375e3, nullptr, nullptr, 0, 0, nullptr }, false }; #endif // !CAPNP_LITE -static const ::capnp::_::AlignedData<191> b_e774a050cbf689a4 = { +static const ::capnp::_::AlignedData<223> b_e774a050cbf689a4 = { { 0, 0, 0, 0, 5, 0, 6, 0, 164, 137, 246, 203, 80, 160, 116, 231, - 24, 0, 0, 0, 1, 0, 5, 0, + 24, 0, 0, 0, 1, 0, 6, 0, 241, 171, 1, 54, 197, 105, 255, 151, 0, 0, 7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 21, 0, 0, 0, 90, 1, 0, 0, 41, 0, 0, 0, 7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 37, 0, 0, 0, 111, 2, 0, 0, + 37, 0, 0, 0, 223, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 108, 111, 103, 46, 99, 97, 112, 110, @@ -9356,84 +9356,98 @@ static const ::capnp::_::AlignedData<191> b_e774a050cbf689a4 = { 111, 114, 113, 117, 101, 83, 116, 97, 116, 101, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, - 44, 0, 0, 0, 3, 0, 4, 0, + 52, 0, 0, 0, 3, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 37, 1, 0, 0, 58, 0, 0, 0, + 93, 1, 0, 0, 58, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 32, 1, 0, 0, 3, 0, 1, 0, - 44, 1, 0, 0, 2, 0, 1, 0, + 88, 1, 0, 0, 3, 0, 1, 0, + 100, 1, 0, 0, 2, 0, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 41, 1, 0, 0, 50, 0, 0, 0, + 97, 1, 0, 0, 50, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 36, 1, 0, 0, 3, 0, 1, 0, - 48, 1, 0, 0, 2, 0, 1, 0, + 92, 1, 0, 0, 3, 0, 1, 0, + 104, 1, 0, 0, 2, 0, 1, 0, 3, 0, 0, 0, 2, 0, 0, 0, 0, 0, 1, 0, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 45, 1, 0, 0, 18, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 40, 1, 0, 0, 3, 0, 1, 0, - 52, 1, 0, 0, 2, 0, 1, 0, - 4, 0, 0, 0, 3, 0, 0, 0, - 0, 0, 1, 0, 3, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 49, 1, 0, 0, 18, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 44, 1, 0, 0, 3, 0, 1, 0, - 56, 1, 0, 0, 2, 0, 1, 0, - 5, 0, 0, 0, 4, 0, 0, 0, - 0, 0, 1, 0, 4, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 53, 1, 0, 0, 18, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 48, 1, 0, 0, 3, 0, 1, 0, - 60, 1, 0, 0, 2, 0, 1, 0, - 6, 0, 0, 0, 5, 0, 0, 0, - 0, 0, 1, 0, 5, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 57, 1, 0, 0, 18, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 52, 1, 0, 0, 3, 0, 1, 0, - 64, 1, 0, 0, 2, 0, 1, 0, - 7, 0, 0, 0, 6, 0, 0, 0, - 0, 0, 1, 0, 6, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 61, 1, 0, 0, 58, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 56, 1, 0, 0, 3, 0, 1, 0, - 68, 1, 0, 0, 2, 0, 1, 0, - 8, 0, 0, 0, 1, 0, 0, 0, - 0, 0, 1, 0, 7, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 65, 1, 0, 0, 82, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 64, 1, 0, 0, 3, 0, 1, 0, - 76, 1, 0, 0, 2, 0, 1, 0, - 2, 0, 0, 0, 7, 0, 0, 0, - 0, 0, 1, 0, 8, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 73, 1, 0, 0, 82, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 72, 1, 0, 0, 3, 0, 1, 0, - 84, 1, 0, 0, 2, 0, 1, 0, - 9, 0, 0, 0, 8, 0, 0, 0, - 0, 0, 1, 0, 9, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 81, 1, 0, 0, 154, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 84, 1, 0, 0, 3, 0, 1, 0, - 96, 1, 0, 0, 2, 0, 1, 0, - 10, 0, 0, 0, 9, 0, 0, 0, - 0, 0, 1, 0, 10, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 93, 1, 0, 0, 162, 0, 0, 0, + 101, 1, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 96, 1, 0, 0, 3, 0, 1, 0, 108, 1, 0, 0, 2, 0, 1, 0, + 4, 0, 0, 0, 3, 0, 0, 0, + 0, 0, 1, 0, 3, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 105, 1, 0, 0, 18, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 100, 1, 0, 0, 3, 0, 1, 0, + 112, 1, 0, 0, 2, 0, 1, 0, + 5, 0, 0, 0, 4, 0, 0, 0, + 0, 0, 1, 0, 4, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 109, 1, 0, 0, 18, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 104, 1, 0, 0, 3, 0, 1, 0, + 116, 1, 0, 0, 2, 0, 1, 0, + 6, 0, 0, 0, 5, 0, 0, 0, + 0, 0, 1, 0, 5, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 113, 1, 0, 0, 18, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 108, 1, 0, 0, 3, 0, 1, 0, + 120, 1, 0, 0, 2, 0, 1, 0, + 7, 0, 0, 0, 6, 0, 0, 0, + 0, 0, 1, 0, 6, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 117, 1, 0, 0, 58, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 112, 1, 0, 0, 3, 0, 1, 0, + 124, 1, 0, 0, 2, 0, 1, 0, + 8, 0, 0, 0, 1, 0, 0, 0, + 0, 0, 1, 0, 7, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 121, 1, 0, 0, 82, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 120, 1, 0, 0, 3, 0, 1, 0, + 132, 1, 0, 0, 2, 0, 1, 0, + 2, 0, 0, 0, 7, 0, 0, 0, + 0, 0, 1, 0, 8, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 129, 1, 0, 0, 82, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 128, 1, 0, 0, 3, 0, 1, 0, + 140, 1, 0, 0, 2, 0, 1, 0, + 9, 0, 0, 0, 8, 0, 0, 0, + 0, 0, 1, 0, 9, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 137, 1, 0, 0, 154, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 140, 1, 0, 0, 3, 0, 1, 0, + 152, 1, 0, 0, 2, 0, 1, 0, + 10, 0, 0, 0, 9, 0, 0, 0, + 0, 0, 1, 0, 10, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 149, 1, 0, 0, 162, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 152, 1, 0, 0, 3, 0, 1, 0, + 164, 1, 0, 0, 2, 0, 1, 0, + 11, 0, 0, 0, 10, 0, 0, 0, + 0, 0, 1, 0, 11, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 161, 1, 0, 0, 154, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 164, 1, 0, 0, 3, 0, 1, 0, + 176, 1, 0, 0, 2, 0, 1, 0, + 12, 0, 0, 0, 11, 0, 0, 0, + 0, 0, 1, 0, 12, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 173, 1, 0, 0, 66, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 168, 1, 0, 0, 3, 0, 1, 0, + 180, 1, 0, 0, 2, 0, 1, 0, 97, 99, 116, 105, 118, 101, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, @@ -9526,16 +9540,34 @@ static const ::capnp::_::AlignedData<191> b_e774a050cbf689a4 = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 100, 101, 115, 105, 114, 101, 100, 76, + 97, 116, 101, 114, 97, 108, 74, 101, + 114, 107, 0, 0, 0, 0, 0, 0, + 10, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 10, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 118, 101, 114, 115, 105, 111, 110, 0, + 4, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, } }; ::capnp::word const* const bp_e774a050cbf689a4 = b_e774a050cbf689a4.words; #if !CAPNP_LITE -static const uint16_t m_e774a050cbf689a4[] = {0, 9, 4, 10, 1, 8, 5, 3, 6, 2, 7}; -static const uint16_t i_e774a050cbf689a4[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10}; +static const uint16_t m_e774a050cbf689a4[] = {0, 9, 4, 10, 11, 1, 8, 5, 3, 6, 2, 7, 12}; +static const uint16_t i_e774a050cbf689a4[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}; const ::capnp::_::RawSchema s_e774a050cbf689a4 = { - 0xe774a050cbf689a4, b_e774a050cbf689a4.words, 191, nullptr, m_e774a050cbf689a4, - 0, 11, i_e774a050cbf689a4, nullptr, nullptr, { &s_e774a050cbf689a4, nullptr, nullptr, 0, 0, nullptr }, false + 0xe774a050cbf689a4, b_e774a050cbf689a4.words, 223, nullptr, m_e774a050cbf689a4, + 0, 13, i_e774a050cbf689a4, nullptr, nullptr, { &s_e774a050cbf689a4, nullptr, nullptr, 0, 0, nullptr }, false }; #endif // !CAPNP_LITE static const ::capnp::_::AlignedData<130> b_9024e2d790c82ade = { diff --git a/cereal/gen/cpp/log.capnp.h b/cereal/gen/cpp/log.capnp.h index 03e9d480b..553643b44 100644 --- a/cereal/gen/cpp/log.capnp.h +++ b/cereal/gen/cpp/log.capnp.h @@ -1076,7 +1076,7 @@ struct ControlsState::LateralTorqueState { class Pipeline; struct _capnpPrivate { - CAPNP_DECLARE_STRUCT_HEADER(e774a050cbf689a4, 5, 0) + CAPNP_DECLARE_STRUCT_HEADER(e774a050cbf689a4, 6, 0) #if !CAPNP_LITE static constexpr ::capnp::_::RawBrandedSchema const* brand() { return &schema->defaultBrand; } #endif // !CAPNP_LITE @@ -7531,6 +7531,10 @@ public: inline float getDesiredLateralAccel() const; + inline float getDesiredLateralJerk() const; + + inline ::int32_t getVersion() const; + private: ::capnp::_::StructReader _reader; template @@ -7592,6 +7596,12 @@ public: inline float getDesiredLateralAccel(); inline void setDesiredLateralAccel(float value); + inline float getDesiredLateralJerk(); + inline void setDesiredLateralJerk(float value); + + inline ::int32_t getVersion(); + inline void setVersion( ::int32_t value); + private: ::capnp::_::StructBuilder _builder; template @@ -30064,6 +30074,34 @@ inline void ControlsState::LateralTorqueState::Builder::setDesiredLateralAccel(f ::capnp::bounded<9>() * ::capnp::ELEMENTS, value); } +inline float ControlsState::LateralTorqueState::Reader::getDesiredLateralJerk() const { + return _reader.getDataField( + ::capnp::bounded<10>() * ::capnp::ELEMENTS); +} + +inline float ControlsState::LateralTorqueState::Builder::getDesiredLateralJerk() { + return _builder.getDataField( + ::capnp::bounded<10>() * ::capnp::ELEMENTS); +} +inline void ControlsState::LateralTorqueState::Builder::setDesiredLateralJerk(float value) { + _builder.setDataField( + ::capnp::bounded<10>() * ::capnp::ELEMENTS, value); +} + +inline ::int32_t ControlsState::LateralTorqueState::Reader::getVersion() const { + return _reader.getDataField< ::int32_t>( + ::capnp::bounded<11>() * ::capnp::ELEMENTS); +} + +inline ::int32_t ControlsState::LateralTorqueState::Builder::getVersion() { + return _builder.getDataField< ::int32_t>( + ::capnp::bounded<11>() * ::capnp::ELEMENTS); +} +inline void ControlsState::LateralTorqueState::Builder::setVersion( ::int32_t value) { + _builder.setDataField< ::int32_t>( + ::capnp::bounded<11>() * ::capnp::ELEMENTS, value); +} + inline bool ControlsState::LateralLQRState::Reader::getActive() const { return _reader.getDataField( ::capnp::bounded<0>() * ::capnp::ELEMENTS); diff --git a/cereal/libcereal_shared.so b/cereal/libcereal_shared.so index a3be5241e..bbe1027bd 100755 Binary files a/cereal/libcereal_shared.so and b/cereal/libcereal_shared.so differ diff --git a/cereal/log.capnp b/cereal/log.capnp index 54e877f2a..882086939 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -795,6 +795,8 @@ struct ControlsState @0x97ff69c53601abf1 { saturated @7 :Bool; actualLateralAccel @9 :Float32; desiredLateralAccel @10 :Float32; + desiredLateralJerk @11 :Float32; + version @12 :Int32; } struct LateralLQRState { diff --git a/frogpilot/assets/nnff_models/CHEVROLET_BOLT_EUV.json b/frogpilot/assets/nnff_models/CHEVROLET_BOLT_EUV.json new file mode 100644 index 000000000..f228dcfa1 --- /dev/null +++ b/frogpilot/assets/nnff_models/CHEVROLET_BOLT_EUV.json @@ -0,0 +1 @@ +{"input_std":[[9.281861],[1.8477924],[0.7977224],[0.047467366],[1.7969038],[1.8168812],[1.8351218],[1.785757],[1.7335743],[1.6658221],[1.5893887],[0.047346078],[0.0473731],[0.047383286],[0.047291175],[0.047300573],[0.04712479],[0.046799928]],"model_test_loss":0.027355343103408813,"input_size":18,"current_date_and_time":"2023-08-05_05-11-44","input_mean":[[21.655252],[-0.07694559],[-0.006081294],[-0.007598456],[-0.07309746],[-0.075890236],[-0.07804942],[-0.076106496],[-0.07184982],[-0.06528668],[-0.060404416],[-0.0077262702],[-0.0077046235],[-0.0076850485],[-0.007687444],[-0.007708868],[-0.0077959923],[-0.008017422]],"input_vars":["v_ego","lateral_accel","lateral_jerk","roll","lateral_accel_m03","lateral_accel_m02","lateral_accel_m01","lateral_accel_p03","lateral_accel_p06","lateral_accel_p10","lateral_accel_p15","roll_m03","roll_m02","roll_m01","roll_p03","roll_p06","roll_p10","roll_p15"],"output_size":1,"layers":[{"dense_1_b":[[-0.35776415],[-0.126288],[0.007988413],[0.03850029],[-0.056796823],[0.0072075897],[0.17376427]],"dense_1_W":[[0.010538558,6.7293744,-0.007391298,-1.035045,-0.47446147,1.0940393,0.08225446,-0.5795131,0.573115,1.82923,-0.16999252,0.8818023,0.03560901,-0.9470653,-1.042151,1.1532398,1.9009485,-1.2265956],[0.009010456,-1.3618345,0.039026346,0.22943486,-0.6669799,-0.768271,-0.16828564,-0.13406865,0.19760236,-0.18953702,-0.019137766,0.1468623,0.26367718,0.4732375,0.73679143,0.62450147,0.30198866,-0.93340015],[-0.044318866,2.2138615,9.620889,-0.66480356,-0.06849887,-0.038568456,-0.30580127,-0.088089585,0.31187677,0.5968372,-1.1462088,0.13234706,0.29166338,-0.69978577,-0.01790655,-0.097928315,-0.56950736,0.7560642],[1.3758256,0.8700429,-0.24295154,0.20832618,-0.7735097,1.0379355,-0.60753095,-0.5457418,-1.0892137,-0.6708325,0.24982774,0.09251328,0.08596103,-0.58034134,-0.3046114,-0.19754426,0.006461508,0.5989185],[0.06314328,-2.492993,-0.28200585,0.49902886,0.8513198,-0.5704965,1.0960953,-0.08426956,-0.76074624,0.10669376,0.02775254,-0.42194095,-0.34657434,-0.10863868,0.17019278,0.0963825,-0.10169116,0.21243806],[0.007984091,-1.9276509,-0.0013926353,0.04057924,1.4438432,1.3440262,-2.506722,1.700801,0.72410774,0.13471895,-0.18753268,0.7303607,0.018133093,-0.71643597,0.07050904,-0.30161944,0.085108586,0.061202843],[0.9413167,-0.9954305,-0.19510143,-0.4711845,-0.12398665,-0.45175523,1.0616771,0.28550953,-0.55543137,-0.21576759,-0.2364377,-0.012782618,-0.050565187,0.257694,-0.20975965,0.00022543047,-0.08760746,0.4983852]],"activation":"σ"},{"dense_2_W":[[-0.71804935,0.017023304,-0.099854074,-0.3262651,-0.402829,-0.12073083,0.13537839],[-0.44002575,-0.1071093,-0.58886194,-0.17319413,-0.21134079,0.23135148,0.0006880979],[-0.55711204,-0.8693639,-0.6443761,-0.7382846,0.18980889,-0.6082511,-0.63103676],[0.11338112,-0.5327033,0.2856197,0.32239884,-0.72682667,0.5302305,-0.45094746],[-0.35197002,-0.14440043,-0.025249843,-0.48697743,0.3340018,-0.25992322,0.0050561456],[0.34757507,0.15670523,-0.14263277,0.50704384,-0.020171141,0.6408679,-0.3074123],[0.40258837,-0.59363264,0.35948923,-0.060853776,-0.0072541097,0.89743376,-0.49789146],[-0.63476,0.29580453,0.1689314,-0.5061521,0.24901053,-0.23218995,0.57968193],[-0.66173035,0.50861925,-0.5845035,-0.6602214,0.8341883,-0.31437424,0.8046359],[0.038493533,0.15464582,-0.04848341,-0.57820857,-0.25891733,-0.47527292,0.21441916],[-0.15464531,-0.07454202,-0.8215851,-0.12614948,-0.5924209,0.00017916794,0.24154592],[0.6091424,-0.112086505,0.1144111,-0.31035227,-0.9237534,0.041003596,-0.3542808],[0.2989792,-0.23780549,0.116059326,-0.6056522,0.5499526,-0.9001413,0.5200723]],"activation":"σ","dense_2_b":[[-0.2638964],[-0.24607328],[-0.15493082],[-0.0071187904],[-0.23515384],[-0.09098133],[-0.034066215],[0.03609036],[-0.03842933],[-0.042302527],[-0.2439947],[-0.16342077],[-0.01030317]]},{"dense_3_W":[[0.45771673,-0.2084603,0.3570187,0.35835707,0.13684571,-0.582958,0.29889587,0.43543494,0.11841301,-0.26185623,-0.4988437,0.5752996,-0.28057483],[-0.19594137,0.050280698,-0.29057446,-0.2829161,-0.16987291,-0.21278952,-0.59946907,0.21295123,0.7040468,0.53549695,-0.52553934,-0.19560973,0.6233473],[-0.19091003,0.08669418,-0.5792192,0.57799137,-0.36263424,0.6037143,0.27898273,-0.30951327,-0.3572644,0.46720102,-0.5403428,0.32415462,-0.60570025]],"activation":"identity","dense_3_b":[[-0.0047377176],[0.023170695],[-0.021546118]]},{"dense_4_W":[[-0.12453941,-1.006034,0.96776205]],"dense_4_b":[[-0.02351544]],"activation":"identity"}]} \ No newline at end of file diff --git a/frogpilot/common/frogpilot_utilities.py b/frogpilot/common/frogpilot_utilities.py index 72ebb0b4c..9f722438d 100644 --- a/frogpilot/common/frogpilot_utilities.py +++ b/frogpilot/common/frogpilot_utilities.py @@ -296,12 +296,12 @@ def update_openpilot(): if params.get("UpdaterState", encoding="utf-8") != "idle": return - while params.get_bool("IsOnroad") or params_memory.get_bool("UpdateSpeedLimits") or running_threads.get("lock_doors", threading.Thread()).is_alive(): - time.sleep(60) - if not update_available(): return + while params.get_bool("IsOnroad") or params_memory.get_bool("UpdateSpeedLimits") or running_threads.get("lock_doors", threading.Thread()).is_alive(): + time.sleep(60) + while True: if not update_available(): break diff --git a/frogpilot/common/frogpilot_variables.py b/frogpilot/common/frogpilot_variables.py index 3c072c2d5..25005724f 100644 --- a/frogpilot/common/frogpilot_variables.py +++ b/frogpilot/common/frogpilot_variables.py @@ -19,6 +19,7 @@ from openpilot.selfdrive.car.mock.interface import CarInterface from openpilot.selfdrive.car.mock.values import CAR as MOCK from openpilot.selfdrive.car.toyota.values import ToyotaFlags, ToyotaFrogPilotFlags from openpilot.selfdrive.controls.lib.desire_helper import LANE_CHANGE_SPEED_MIN +from openpilot.selfdrive.controls.lib.latcontrol_torque import KP from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.system.hardware import HARDWARE from openpilot.system.hardware.power_monitoring import VBATT_PAUSE_CHARGING @@ -531,6 +532,10 @@ class FrogPilotVariables: safety_config.safetyModel = car.CarParams.SafetyModel.noOutput CP.safetyConfigs = [safety_config] + is_torque_car = CP.lateralTuning.which() == "torque" + if not is_torque_car: + CarInterfaceBase.configure_torque_tune(MOCK.MOCK, CP.lateralTuning) + fpmsg_bytes = params.get("FrogPilotCarParams" if started else "FrogPilotCarParamsPersistent", block=started) if fpmsg_bytes: with custom.FrogPilotCarParams.from_bytes(fpmsg_bytes) as fpcp_reader: @@ -539,17 +544,13 @@ class FrogPilotVariables: CarInterface, _, _ = interfaces[MOCK.MOCK] FPCP = CarInterface.get_frogpilot_params(MOCK.MOCK, gen_empty_fingerprint(), [], CP, toggle) - is_torque_car = FPCP.lateralTuning.which() == "torque" - if not is_torque_car: - CarInterfaceBase.configure_torque_tune(MOCK.MOCK, FPCP.lateralTuning) - toggle.always_on_lateral_set = bool(CP.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL) toggle.car_make = CP.carName toggle.car_model = CP.carFingerprint toggle.disable_openpilot_long = params.get_bool("DisableOpenpilotLongitudinal") if tuning_level >= level["DisableOpenpilotLongitudinal"] else default.get_bool("DisableOpenpilotLongitudinal") - friction = FPCP.lateralTuning.torque.friction - has_auto_tune = toggle.car_make in {"hyundai", "toyota"} and FPCP.lateralTuning.which() == "torque" + friction = CP.lateralTuning.torque.friction + has_auto_tune = toggle.car_make in {"hyundai", "toyota"} and CP.lateralTuning.which() == "torque" has_bsm = CP.enableBsm toggle.has_cc_long = toggle.car_make == "gm" and bool(CP.flags & GMFlags.CC_LONG.value) has_nnff = nnff_supported(toggle.car_model) @@ -559,14 +560,14 @@ class FrogPilotVariables: has_sng = CP.autoResumeSng toggle.has_zss = toggle.car_make == "toyota" and bool(FPCP.fpFlags & ToyotaFrogPilotFlags.ZSS.value) is_angle_car = CP.steerControlType == car.CarParams.SteerControlType.angle - latAccelFactor = FPCP.lateralTuning.torque.latAccelFactor + latAccelFactor = CP.lateralTuning.torque.latAccelFactor longitudinalActuatorDelay = CP.longitudinalActuatorDelay toggle.openpilot_longitudinal = CP.openpilotLongitudinalControl and not toggle.disable_openpilot_long pcm_cruise = CP.pcmCruise startAccel = CP.startAccel stopAccel = CP.stopAccel steerActuatorDelay = CP.steerActuatorDelay - steerKp = FPCP.lateralTuning.torque.kp + steerKp = CP.lateralTuning.pid.kp if CP.lateralTuning.which() == "pid" else KP steerRatio = CP.steerRatio toggle.stoppingDecelRate = CP.stoppingDecelRate taco_hacks_allowed = CP.safetyConfigs[0].safetyModel == SafetyModel.hyundaiCanfd diff --git a/frogpilot/controls/frogpilot_planner.py b/frogpilot/controls/frogpilot_planner.py index d0b854778..5c2047b21 100644 --- a/frogpilot/controls/frogpilot_planner.py +++ b/frogpilot/controls/frogpilot_planner.py @@ -32,7 +32,6 @@ class FrogPilotPlanner: self.tracking_lead_filter = FirstOrderFilter(0, 0.5, DT_MDL) - self.disable_throttle = False self.driving_in_curve = False self.lateral_check = False self.model_stopped = False @@ -66,9 +65,6 @@ class FrogPilotPlanner: self.cem.curve_detected = False self.cem.stop_sign_and_light(v_ego, sm, PLANNER_TIME - 2) - self.disable_throttle = self.tracking_lead and not self.frogpilot_following.following_lead - self.disable_throttle &= self.lead_one.vLead + COMFORT_BRAKE < v_ego > CRUISING_SPEED - self.disable_throttle &= sm["controlsState"].enabled self.driving_in_curve = abs(self.lateral_acceleration) >= MINIMUM_LATERAL_ACCELERATION @@ -146,7 +142,7 @@ class FrogPilotPlanner: frogpilotPlan.desiredFollowDistance = self.frogpilot_following.desired_follow_distance - frogpilotPlan.disableThrottle = self.disable_throttle + frogpilotPlan.disableThrottle = self.frogpilot_following.disable_throttle frogpilotPlan.experimentalMode = self.cem.experimental_mode or self.frogpilot_vcruise.slc.experimental_mode diff --git a/frogpilot/controls/lib/frogpilot_following.py b/frogpilot/controls/lib/frogpilot_following.py index 8aefa244d..072fcc7ab 100644 --- a/frogpilot/controls/lib/frogpilot_following.py +++ b/frogpilot/controls/lib/frogpilot_following.py @@ -11,6 +11,7 @@ class FrogPilotFollowing: def __init__(self, FrogPilotPlanner): self.frogpilot_planner = FrogPilotPlanner + self.disable_throttle = False self.following_lead = False self.slower_lead = False @@ -63,7 +64,12 @@ class FrogPilotFollowing: self.danger_jerk = self.base_danger_jerk self.speed_jerk = self.base_speed_jerk - self.following_lead = self.frogpilot_planner.tracking_lead and self.frogpilot_planner.lead_one.dRel < (self.t_follow + 1) * v_ego + self.following_lead = self.frogpilot_planner.tracking_lead and self.frogpilot_planner.lead_one.dRel < (self.t_follow * 2) * v_ego + + + self.disable_throttle = self.frogpilot_planner.tracking_lead and not self.following_lead + self.disable_throttle &= self.frogpilot_planner.lead_one.dRel + 6.0 < (self.t_follow * 2 * 2) * v_ego + self.disable_throttle &= self.frogpilot_planner.lead_one.vLead < v_ego * 0.75 if sm["controlsState"].enabled and self.frogpilot_planner.tracking_lead: self.update_follow_values(self.frogpilot_planner.lead_one.dRel, v_ego, self.frogpilot_planner.lead_one.vLead, frogpilot_toggles) diff --git a/frogpilot/controls/lib/neural_network_feedforward.py b/frogpilot/controls/lib/neural_network_feedforward.py index d2dcf99ed..a849d3cdb 100644 --- a/frogpilot/controls/lib/neural_network_feedforward.py +++ b/frogpilot/controls/lib/neural_network_feedforward.py @@ -1,16 +1,21 @@ #!/usr/bin/env python3 -# Twilsonco's Lateral Neural Network Feedforward -from collections import deque -from difflib import SequenceMatcher - +# Twilsonco's Lateral Neural Network Feedforward Controller import json import math import numpy as np import os +from collections import deque +from difflib import SequenceMatcher +from cereal import log from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.numpy_fast import interp +from openpilot.common.params import Params from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N +from openpilot.selfdrive.controls.lib.latcontrol import LatControl +from openpilot.selfdrive.controls.lib.latcontrol_torque import KD, KI, KP +from openpilot.selfdrive.controls.lib.pid import PIDController +from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.frogpilot.common.frogpilot_variables import NNFF_MODELS_PATH, get_nnff_model_files, params @@ -29,8 +34,8 @@ from openpilot.frogpilot.common.frogpilot_variables import NNFF_MODELS_PATH, get # dict used to rename activation functions whose names aren't valid python identifiers ACTIVATION_FUNCTION_NAMES = {'σ': 'sigmoid'} -LOW_SPEED_Y_NN = [12, 3, 1, 0] - +LOW_SPEED_X = [0, 10, 20, 30] +LOW_SPEED_Y = [12, 3, 1, 0] LAT_PLAN_MIN_IDX = 5 @@ -40,6 +45,7 @@ class FluxModel: params = json.load(f) self.input_size = params["input_size"] + self.output_size = params["output_size"] self.input_mean = np.array(params["input_mean"], dtype=np.float32).T self.input_std = np.array(params["input_std"], dtype=np.float32).T @@ -123,7 +129,7 @@ def get_nn_model_path(car, eps_firmware) -> str | None: def find_valid_model(*queries): for query in queries: path, score = best_model_path(query) - if path and car in path and score >= 0.9: + if path and score >= 0.9: return path return None @@ -149,16 +155,18 @@ def sign(x): def similarity(s1: str, s2: str) -> float: return SequenceMatcher(None, s1, s2).ratio() - -class NeuralNetworkFeedforward: - def __init__(self, CP, LatControlTorque): - self.lat_control_torque = LatControlTorque - +class LatControlNNFF(LatControl): + def __init__(self, CP, CI, dt): + super().__init__(CP, CI, dt) self.lat_torque_nn_model = get_nn_model(CP.carFingerprint, str(next((fw.fwVersion for fw in CP.carFw if fw.ecu == "eps"), "")).replace("\\", "")) + self.nnff_loaded = self.lat_torque_nn_model is not None - self.torque_from_lateral_accel = self.lat_control_torque.torque_from_lateral_accel - - self.use_steering_angle = self.lat_control_torque.torque_params.useSteeringAngle + self.torque_params = CP.lateralTuning.torque + self.pid = PIDController(KP, KI, k_d=KD, + pos_limit=self.steer_max, neg_limit=-self.steer_max) + self.torque_from_lateral_accel = CI.torque_from_lateral_accel() + self.use_steering_angle = self.torque_params.useSteeringAngle + self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg # Instantaneous lateral jerk changes very rapidly, making it not useful on its own, # however, we can "look ahead" to the future planned lateral jerk in order to gauge @@ -176,7 +184,6 @@ class NeuralNetworkFeedforward: self.lat_jerk_friction_factor = 0.4 # precompute time differences between ModelConstants.T_IDXS - self.lateral_delay = CP.steerActuatorDelay self.t_diffs = np.diff(ModelConstants.T_IDXS) self.pitch = FirstOrderFilter(0.0, 0.5, 0.01) @@ -188,7 +195,7 @@ class NeuralNetworkFeedforward: # setup future time offsets self.future_times = [0.3, 0.6, 1.0, 1.5] # seconds in the future - self.nn_future_times = [time + self.lateral_delay for time in self.future_times] + self.nn_future_times = [time + CP.steerActuatorDelay for time in self.future_times] # setup past time offsets self.past_times = [-0.3, -0.2, -0.1] @@ -199,95 +206,150 @@ class NeuralNetworkFeedforward: self.roll_deque = deque(maxlen=history_check_frames[0]) def update_live_delay(self, lateral_delay): - self.lateral_delay = lateral_delay - - self.nn_future_times = [time + self.lateral_delay for time in self.future_times] + self.nn_future_times = [time + lateral_delay for time in self.future_times] self.past_future_len = len(self.past_times) + len(self.nn_future_times) - def compute_nnff(self, CS, VM, actual_lateral_accel, future_desired_lateral_accel, error, gravity_adjusted_future_lateral_accel, llk, measurement, model_data, params, pid_log, setpoint, frogpilot_toggles): - if self.use_steering_angle: - actual_curvature_rate = -VM.calc_curvature(math.radians(CS.steeringRateDeg), CS.vEgo, 0.0) - actual_lateral_jerk = actual_curvature_rate * CS.vEgo ** 2 + def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): + self.torque_params.latAccelFactor = latAccelFactor + self.torque_params.latAccelOffset = latAccelOffset + self.torque_params.friction = friction + + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay, llk, model_data, frogpilot_toggles): + pid_log = log.ControlsState.LateralTorqueState.new_message() + if not active: + output_torque = 0.0 + pid_log.active = False else: - actual_lateral_jerk = 0.0 + actual_curvature_vm = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) + roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY + if self.use_steering_angle: + actual_curvature = actual_curvature_vm + curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) + else: + actual_curvature_llk = llk.angularVelocityCalibrated.value[2] / CS.vEgo + actual_curvature = interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_llk]) + curvature_deadzone = 0.0 + desired_lateral_accel = desired_curvature * CS.vEgo ** 2 - model_good = model_data is not None and len(model_data.orientation.x) >= CONTROL_N + # desired rate is the desired rate of change in the setpoint, not the absolute desired curvature + # desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2 + actual_lateral_accel = actual_curvature * CS.vEgo ** 2 + lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 - if model_good: - # prepare "look-ahead" desired lateral jerk - lookahead = interp(CS.vEgo, self.friction_look_ahead_bp, self.friction_look_ahead_v) - friction_upper_idx = next((idxs for idxs, value in enumerate(ModelConstants.T_IDXS) if value > lookahead), 16) + low_speed_factor = interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2 + setpoint = desired_lateral_accel + low_speed_factor * desired_curvature + measurement = actual_lateral_accel + low_speed_factor * actual_curvature + gravity_adjusted_lateral_accel = desired_lateral_accel - roll_compensation + if self.nnff_loaded and frogpilot_toggles.nnff or frogpilot_toggles.nnff_lite: + if self.use_steering_angle: + actual_curvature_rate = -VM.calc_curvature(math.radians(CS.steeringRateDeg), CS.vEgo, 0.0) + actual_lateral_jerk = actual_curvature_rate * CS.vEgo ** 2 + else: + actual_lateral_jerk = 0.0 - predicted_lateral_jerk = get_predicted_lateral_jerk(model_data.acceleration.y, self.t_diffs) - desired_lateral_jerk = (interp(self.lateral_delay, ModelConstants.T_IDXS, model_data.acceleration.y) - future_desired_lateral_accel) / self.lateral_delay + model_good = model_data is not None and len(model_data.orientation.x) >= CONTROL_N - lookahead_lateral_jerk = get_lookahead_value(predicted_lateral_jerk[LAT_PLAN_MIN_IDX:friction_upper_idx], desired_lateral_jerk) + if model_good: + # prepare "look-ahead" desired lateral jerk + lookahead = interp(CS.vEgo, self.friction_look_ahead_bp, self.friction_look_ahead_v) + friction_upper_idx = next((idxs for idxs, value in enumerate(ModelConstants.T_IDXS) if value > lookahead), 16) - if not self.use_steering_angle or lookahead_lateral_jerk == 0.0: - lookahead_lateral_jerk = 0.0 - actual_lateral_jerk = 0.0 - self.lat_accel_friction_factor = 1.0 + predicted_lateral_jerk = get_predicted_lateral_jerk(model_data.acceleration.y, self.t_diffs) + desired_lateral_jerk = (interp(lat_delay, ModelConstants.T_IDXS, model_data.acceleration.y) - desired_lateral_accel) / lat_delay - lateral_jerk_setpoint = self.lat_jerk_friction_factor * lookahead_lateral_jerk - lateral_jerk_measurement = self.lat_jerk_friction_factor * actual_lateral_jerk - else: - lateral_jerk_setpoint = 0 - lateral_jerk_measurement = 0 - lookahead_lateral_jerk = 0 + lookahead_lateral_jerk = get_lookahead_value(predicted_lateral_jerk[LAT_PLAN_MIN_IDX:friction_upper_idx], desired_lateral_jerk) - if self.lat_control_torque.nnff_loaded and model_good and frogpilot_toggles.nnff: - # update past data - pitch = 0 - roll = params.roll - if len(llk.calibratedOrientationNED.value) > 1: - pitch = self.pitch.update(llk.calibratedOrientationNED.value[1]) - roll = roll_pitch_adjust(roll, pitch) + if not self.use_steering_angle or lookahead_lateral_jerk == 0.0: + lookahead_lateral_jerk = 0.0 + actual_lateral_jerk = 0.0 + self.lat_accel_friction_factor = 1.0 - self.roll_deque.append(roll) - self.lateral_accel_desired_deque.append(future_desired_lateral_accel) + lateral_jerk_setpoint = self.lat_jerk_friction_factor * lookahead_lateral_jerk + lateral_jerk_measurement = self.lat_jerk_friction_factor * actual_lateral_jerk + else: + lateral_jerk_setpoint = 0 + lateral_jerk_measurement = 0 + lookahead_lateral_jerk = 0 - # prepare past and future values - # adjust future times to account for longitudinal acceleration - adjusted_future_times = [time + 0.5 * CS.aEgo * (time / max(CS.vEgo, 1.0)) for time in self.nn_future_times] - past_rolls = [self.roll_deque[min(len(self.roll_deque)-1, offset)] for offset in self.history_frame_offsets] - future_rolls = [roll_pitch_adjust(interp(time, ModelConstants.T_IDXS, model_data.orientation.x) + roll, interp(time, ModelConstants.T_IDXS, model_data.orientation.y) + pitch) for time in adjusted_future_times] + if self.nnff_loaded and model_good and frogpilot_toggles.nnff: + # update past data + pitch = 0 + roll = params.roll + if len(llk.calibratedOrientationNED.value) > 1: + pitch = self.pitch.update(llk.calibratedOrientationNED.value[1]) + roll = roll_pitch_adjust(roll, pitch) - past_lateral_accels_desired = [self.lateral_accel_desired_deque[min(len(self.lateral_accel_desired_deque)-1, offset)] for offset in self.history_frame_offsets] - future_lateral_accels = [interp(time, ModelConstants.T_IDXS[:CONTROL_N], model_data.acceleration.y) for time in adjusted_future_times] + self.roll_deque.append(roll) + self.lateral_accel_desired_deque.append(desired_lateral_accel) - base_input = [CS.vEgo, roll] - nnff_common = past_rolls + future_rolls + # prepare past and future values + # adjust future times to account for longitudinal acceleration + adjusted_future_times = [time + 0.5 * CS.aEgo * (time / max(CS.vEgo, 1.0)) for time in self.nn_future_times] + past_rolls = [self.roll_deque[min(len(self.roll_deque)-1, offset)] for offset in self.history_frame_offsets] + future_rolls = [roll_pitch_adjust(interp(time, ModelConstants.T_IDXS, model_data.orientation.x) + roll, interp(time, ModelConstants.T_IDXS, model_data.orientation.y) + pitch) for time in adjusted_future_times] - # compute NNFF error response - nnff_setpoint_input = base_input[:1] + [setpoint, lateral_jerk_setpoint] + base_input[1:] + [setpoint] * self.past_future_len + nnff_common - nnff_measurement_input = base_input[:1] + [measurement, lateral_jerk_measurement] + base_input[1:] + [measurement] * self.past_future_len + nnff_common + past_lateral_accels_desired = [self.lateral_accel_desired_deque[min(len(self.lateral_accel_desired_deque)-1, offset)] for offset in self.history_frame_offsets] + future_lateral_accels = [interp(time, ModelConstants.T_IDXS[:CONTROL_N], model_data.acceleration.y) for time in adjusted_future_times] - torque_from_setpoint = self.lat_torque_nn_model.evaluate(nnff_setpoint_input) - torque_from_measurement = self.lat_torque_nn_model.evaluate(nnff_measurement_input) + base_input = [CS.vEgo, roll] + nnff_common = past_rolls + future_rolls - pid_log.error = torque_from_setpoint - torque_from_measurement + # compute NNFF error response + nnff_setpoint_input = base_input[:1] + [setpoint, lateral_jerk_setpoint] + base_input[1:] + [setpoint] * self.past_future_len + nnff_common + nnff_measurement_input = base_input[:1] + [measurement, lateral_jerk_measurement] + base_input[1:] + [measurement] * self.past_future_len + nnff_common - error_blend = interp(abs(future_desired_lateral_accel), [1.0, 2.0], [0.0, 1.0]) - if error_blend > 0.0: # blend in stronger error response when in high lat accel - torque_from_error = self.lat_torque_nn_model.evaluate([CS.vEgo, setpoint - measurement, lateral_jerk_setpoint - lateral_jerk_measurement, 0.0]) - if sign(pid_log.error) == sign(torque_from_error) and abs(pid_log.error) < abs(torque_from_error): - pid_log.error = pid_log.error * (1.0 - error_blend) + torque_from_error * error_blend + torque_from_setpoint = self.lat_torque_nn_model.evaluate(nnff_setpoint_input) + torque_from_measurement = self.lat_torque_nn_model.evaluate(nnff_measurement_input) - # compute feedforward (same as nn setpoint output) - friction_input = self.lat_accel_friction_factor * error + self.lat_jerk_friction_factor * lookahead_lateral_jerk - nn_input = [CS.vEgo, future_desired_lateral_accel, friction_input, roll] + past_lateral_accels_desired + future_lateral_accels + nnff_common - ff = self.lat_torque_nn_model.evaluate(nn_input) + pid_log.error = torque_from_setpoint - torque_from_measurement - # apply friction override for cars with low NN friction response - if self.nn_friction_override: - pid_log.error += self.torque_from_lateral_accel(0.0, self.lat_control_torque.torque_params) - else: - torque_from_measurement = self.torque_from_lateral_accel(measurement, self.lat_control_torque.torque_params) - torque_from_setpoint = self.torque_from_lateral_accel(setpoint, self.lat_control_torque.torque_params) + error_blend = interp(abs(desired_lateral_accel), [1.0, 2.0], [0.0, 1.0]) + if error_blend > 0.0: # blend in stronger error response when in high lat accel + torque_from_error = self.lat_torque_nn_model.evaluate([CS.vEgo, setpoint - measurement, lateral_jerk_setpoint - lateral_jerk_measurement, 0.0]) + if sign(pid_log.error) == sign(torque_from_error) and abs(pid_log.error) < abs(torque_from_error): + pid_log.error = pid_log.error * (1.0 - error_blend) + torque_from_error * error_blend - pid_log.error = float(torque_from_setpoint - torque_from_measurement) + # compute feedforward (same as nn setpoint output) + error = setpoint - measurement + friction_input = self.lat_accel_friction_factor * error + self.lat_jerk_friction_factor * lookahead_lateral_jerk + nn_input = [CS.vEgo, desired_lateral_accel, friction_input, roll] + past_lateral_accels_desired + future_lateral_accels + nnff_common + ff = self.lat_torque_nn_model.evaluate(nn_input) - friction_input = self.lat_accel_friction_factor * error + self.lat_jerk_friction_factor * lookahead_lateral_jerk - ff = self.torque_from_lateral_accel(gravity_adjusted_future_lateral_accel, self.lat_control_torque.torque_params) + # apply friction override for cars with low NN friction response + if self.nn_friction_override: + pid_log.error += self.torque_from_lateral_accel(0.0, self.torque_params) + else: + torque_from_measurement = self.torque_from_lateral_accel(measurement, self.torque_params) + torque_from_setpoint = self.torque_from_lateral_accel(setpoint, self.torque_params) - return pid_log, ff + pid_log.error = float(torque_from_setpoint - torque_from_measurement) + + error = desired_lateral_accel - actual_lateral_accel + friction_input = self.lat_accel_friction_factor * error + self.lat_jerk_friction_factor * lookahead_lateral_jerk + ff = self.torque_from_lateral_accel(gravity_adjusted_lateral_accel, self.torque_params) + else: + torque_from_measurement = self.torque_from_lateral_accel(measurement, self.torque_params) + torque_from_setpoint = self.torque_from_lateral_accel(setpoint, self.torque_params) + + pid_log.error = float(torque_from_setpoint - torque_from_measurement) + + ff = self.torque_from_lateral_accel(gravity_adjusted_lateral_accel, self.torque_params) + + freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 + output_torque = self.pid.update(pid_log.error, + feedforward=ff, + speed=CS.vEgo, + freeze_integrator=freeze_integrator) + + pid_log.active = True + pid_log.p = float(self.pid.p) + pid_log.i = float(self.pid.i) + pid_log.d = float(self.pid.d) + pid_log.f = float(self.pid.f) + pid_log.output = float(-output_torque) + pid_log.actualLateralAccel = float(actual_lateral_accel) + pid_log.desiredLateralAccel = float(desired_lateral_accel) + pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited)) + + # TODO left is positive in this convention + return -output_torque, 0.0, pid_log diff --git a/frogpilot/system/environment_variables b/frogpilot/system/environment_variables old mode 100755 new mode 100644 index 6bf4f6a8f..0e968181e Binary files a/frogpilot/system/environment_variables and b/frogpilot/system/environment_variables differ diff --git a/frogpilot/ui/qt/offroad/frogpilot_settings.cc b/frogpilot/ui/qt/offroad/frogpilot_settings.cc index 64dc5cfb1..2ce9a4e1d 100644 --- a/frogpilot/ui/qt/offroad/frogpilot_settings.cc +++ b/frogpilot/ui/qt/offroad/frogpilot_settings.cc @@ -277,17 +277,21 @@ void FrogPilotSettingsWindow::updateVariables() { isHKG = carMake == "hyundai"; isHKGCanFd = isHKG && safetyModel == cereal::CarParams::SafetyModel::HYUNDAI_CANFD; isSubaru = carMake == "subaru"; + isTorqueCar = CP.getLateralTuning().which() == cereal::CarParams::LateralTuning::TORQUE; isToyota = carMake == "toyota"; isTSK = CP.getSecOcRequired(); isVolt = carFingerprint == "CHEVROLET_VOLT"; longitudinalActuatorDelay = CP.getLongitudinalActuatorDelay(); startAccel = CP.getStartAccel(); steerActuatorDelay = CP.getSteerActuatorDelay(); + steerKp = CP.getLateralTuning().which() == cereal::CarParams::LateralTuning::PID ? CP.getLateralTuning().getPid().getKpV()[0] : 0.6; steerRatio = CP.getSteerRatio(); stopAccel = CP.getStopAccel(); stoppingDecelRate = CP.getStoppingDecelRate(); vEgoStarting = CP.getVEgoStarting(); vEgoStopping = CP.getVEgoStopping(); + friction = CP.getLateralTuning().getTorque().getFriction(); + latAccelFactor = CP.getLateralTuning().getTorque().getLatAccelFactor(); float currentDelayStock = params.getFloat("SteerDelayStock"); float currentFrictionStock = params.getFloat("SteerFrictionStock"); @@ -387,12 +391,17 @@ void FrogPilotSettingsWindow::updateVariables() { canUsePedal = FPCP.getCanUsePedal(); canUseSDSU = FPCP.getCanUseSDSU(); - friction = FPCP.getLateralTuning().getTorque().getFriction(); - hasAutoTune = (carMake == "hyundai" || carMake == "toyota") && FPCP.getLateralTuning().which() == cereal::FrogPilotCarParams::LateralTuning::TORQUE; - isTorqueCar = FPCP.getLateralTuning().which() == cereal::FrogPilotCarParams::LateralTuning::TORQUE; - latAccelFactor = FPCP.getLateralTuning().getTorque().getLatAccelFactor(); openpilotLongitudinalControlDisabled = FPCP.getOpenpilotLongitudinalControlDisabled(); - steerKp = FPCP.getLateralTuning().getTorque().getKp(); + } + + std::string liveTorqueParameters = params.get("LiveTorqueParameters"); + if (!liveTorqueParameters.empty()) { + AlignedBuffer aligned_buf; + capnp::FlatArrayMessageReader reader(aligned_buf.align(liveTorqueParameters.data(), liveTorqueParameters.size())); + cereal::Event::Reader event = reader.getRoot(); + cereal::LiveTorqueParametersData::Reader LTP = event.getLiveTorqueParameters(); + + hasAutoTune = LTP.getUseParams(); } isC3 = util::read_file("/sys/firmware/devicetree/base/model").find("tici") != std::string::npos; diff --git a/frogpilot/ui/qt/offroad/lateral_settings.cc b/frogpilot/ui/qt/offroad/lateral_settings.cc index 3247356f7..d99244d54 100644 --- a/frogpilot/ui/qt/offroad/lateral_settings.cc +++ b/frogpilot/ui/qt/offroad/lateral_settings.cc @@ -39,11 +39,11 @@ FrogPilotLateralPanel::FrogPilotLateralPanel(FrogPilotSettingsWindow *parent) : const std::vector> lateralToggles { {"AdvancedLateralTune", tr("Advanced Lateral Tuning"), tr("Advanced steering control changes to fine-tune how openpilot drives."), "../../frogpilot/assets/toggle_icons/icon_advanced_lateral_tune.png"}, - {"SteerDelay", steerActuatorDelay != 0 ? QString(tr("Actuator Delay (Default: %1)")).arg(QString::number(steerActuatorDelay, 'f', 2)) : tr("Actuator Delay"), tr("The time between openpilot's steering command and the vehicle's response. Increase if the vehicle reacts late; decrease if it feels jumpy. Auto-learned by default."), ""}, - {"SteerFriction", friction != 0 ? QString(tr("Friction (Default: %1)")).arg(QString::number(friction, 'f', 2)) : tr("Friction"), tr("Compensates for steering friction. Increase if the wheel sticks near center; decrease if it jitters. Auto-learned by default."), ""}, - {"SteerKP", steerKp != 0 ? QString(tr("Kp Factor (Default: %1)")).arg(QString::number(steerKp, 'f', 2)) : tr("Kp Factor"), tr("How strongly openpilot corrects lane position. Higher is tighter but twitchier; lower is smoother but slower. Auto-learned by default."), ""}, - {"SteerLatAccel", latAccelFactor != 0 ? QString(tr("Lateral Acceleration (Default: %1)")).arg(QString::number(latAccelFactor, 'f', 2)) : tr("Lateral Acceleration"), tr("Maps steering torque to turning response. Increase for sharper turns; decrease for gentler steering. Auto-learned by default."), ""}, - {"SteerRatio", steerRatio != 0 ? QString(tr("Steer Ratio (Default: %1)")).arg(QString::number(steerRatio, 'f', 2)) : tr("Steer Ratio"), tr("The relationship between steering wheel rotation and road wheel angle. Increase if steering feels too quick or twitchy; decrease if it feels too slow or weak. Auto-learned by default."), ""}, + {"SteerDelay", parent->steerActuatorDelay != 0 ? QString(tr("Actuator Delay (Default: %1)")).arg(QString::number(parent->steerActuatorDelay, 'f', 2)) : tr("Actuator Delay"), tr("The time between openpilot's steering command and the vehicle's response. Increase if the vehicle reacts late; decrease if it feels jumpy. Auto-learned by default."), ""}, + {"SteerFriction", parent->friction != 0 ? QString(tr("Friction (Default: %1)")).arg(QString::number(parent->friction, 'f', 2)) : tr("Friction"), tr("Compensates for steering friction. Increase if the wheel sticks near center; decrease if it jitters. Auto-learned by default."), ""}, + {"SteerKP", parent->steerKp != 0 ? QString(tr("Kp Factor (Default: %1)")).arg(QString::number(parent->steerKp, 'f', 2)) : tr("Kp Factor"), tr("How strongly openpilot corrects lane position. Higher is tighter but twitchier; lower is smoother but slower. Auto-learned by default."), ""}, + {"SteerLatAccel", parent->latAccelFactor != 0 ? QString(tr("Lateral Acceleration (Default: %1)")).arg(QString::number(parent->latAccelFactor, 'f', 2)) : tr("Lateral Acceleration"), tr("Maps steering torque to turning response. Increase for sharper turns; decrease for gentler steering. Auto-learned by default."), ""}, + {"SteerRatio", parent->steerRatio != 0 ? QString(tr("Steer Ratio (Default: %1)")).arg(QString::number(parent->steerRatio, 'f', 2)) : tr("Steer Ratio"), tr("The relationship between steering wheel rotation and road wheel angle. Increase if steering feels too quick or twitchy; decrease if it feels too slow or weak. Auto-learned by default."), ""}, {"ForceAutoTune", tr("Force Auto-Tune On"), tr("Force-enable openpilot's live auto-tuning for \"Friction\" and \"Lateral Acceleration\"."), ""}, {"ForceAutoTuneOff", tr("Force Auto-Tune Off"), tr("Force-disable openpilot's live auto-tuning for \"Friction\" and \"Lateral Acceleration\" and use the set value instead."), ""}, {"ForceTorqueController", tr("Force Torque Controller"), tr("Use torque-based steering control instead of angle-based control for smoother lane keeping, especially in curves."), ""}, @@ -89,13 +89,13 @@ FrogPilotLateralPanel::FrogPilotLateralPanel(FrogPilotSettingsWindow *parent) : lateralToggle = new FrogPilotParamValueButtonControl(param, title, desc, icon, 0, 0.5, QString(), std::map(), 0.01, false, {}, steerFrictionButton, false, false); } else if (param == "SteerKP") { std::vector steerKPButton{"Reset"}; - lateralToggle = new FrogPilotParamValueButtonControl(param, title, desc, icon, steerKp * 0.5, steerKp * 1.5, QString(), std::map(), 0.01, false, {}, steerKPButton, false, false); + lateralToggle = new FrogPilotParamValueButtonControl(param, title, desc, icon, parent->steerKp * 0.5, parent->steerKp * 1.5, QString(), std::map(), 0.01, false, {}, steerKPButton, false, false); } else if (param == "SteerLatAccel") { std::vector steerLatAccelButton{"Reset"}; - lateralToggle = new FrogPilotParamValueButtonControl(param, title, desc, icon, latAccelFactor * 0.75, latAccelFactor * 1.25, QString(), std::map(), 0.01, false, {}, steerLatAccelButton, false, false); + lateralToggle = new FrogPilotParamValueButtonControl(param, title, desc, icon, parent->latAccelFactor * 0.75, parent->latAccelFactor * 1.25, QString(), std::map(), 0.01, false, {}, steerLatAccelButton, false, false); } else if (param == "SteerRatio") { std::vector steerRatioButton{"Reset"}; - lateralToggle = new FrogPilotParamValueButtonControl(param, title, desc, icon, steerRatio * 0.5, steerRatio * 1.5, QString(), std::map(), 0.01, false, {}, steerRatioButton, false, false); + lateralToggle = new FrogPilotParamValueButtonControl(param, title, desc, icon, parent->steerRatio * 0.5, parent->steerRatio * 1.5, QString(), std::map(), 0.01, false, {}, steerRatioButton, false, false); } else if (param == "AlwaysOnLateral") { FrogPilotManageControl *aolToggle = new FrogPilotManageControl(param, title, desc, icon); @@ -191,12 +191,6 @@ FrogPilotLateralPanel::FrogPilotLateralPanel(FrogPilotSettingsWindow *parent) : if (FrogPilotConfirmationDialog::toggleReboot(this)) { Hardware::reboot(); } - } else if (key == "NNFF" || key == "NNFFLite") { - if (!isTorqueCar) { - if (FrogPilotConfirmationDialog::toggleReboot(this)) { - Hardware::reboot(); - } - } } else if (key != "AlwaysOnLateral") { if (FrogPilotConfirmationDialog::toggleReboot(this)) { Hardware::reboot(); @@ -207,41 +201,41 @@ FrogPilotLateralPanel::FrogPilotLateralPanel(FrogPilotSettingsWindow *parent) : } steerDelayToggle = static_cast(toggles["SteerDelay"]); - QObject::connect(steerDelayToggle, &FrogPilotParamValueButtonControl::buttonClicked, [this]() { + QObject::connect(steerDelayToggle, &FrogPilotParamValueButtonControl::buttonClicked, [parent, this]() { if (FrogPilotConfirmationDialog::yesorno(tr("Reset Actuator Delay to its default value?"), this)) { - params.putFloat("SteerDelay", steerActuatorDelay); + params.putFloat("SteerDelay", parent->steerActuatorDelay); steerDelayToggle->refresh(); } }); steerFrictionToggle = static_cast(toggles["SteerFriction"]); - QObject::connect(steerFrictionToggle, &FrogPilotParamValueButtonControl::buttonClicked, [this]() { + QObject::connect(steerFrictionToggle, &FrogPilotParamValueButtonControl::buttonClicked, [parent, this]() { if (FrogPilotConfirmationDialog::yesorno(tr("Reset Friction to its default value?"), this)) { - params.putFloat("SteerFriction", friction); + params.putFloat("SteerFriction", parent->friction); steerFrictionToggle->refresh(); } }); steerKPToggle = static_cast(toggles["SteerKP"]); - QObject::connect(steerKPToggle, &FrogPilotParamValueButtonControl::buttonClicked, [this]() { + QObject::connect(steerKPToggle, &FrogPilotParamValueButtonControl::buttonClicked, [parent, this]() { if (FrogPilotConfirmationDialog::yesorno(tr("Reset Kp Factor to its default value?"), this)) { - params.putFloat("SteerKP", steerKp); + params.putFloat("SteerKP", parent->steerKp); steerKPToggle->refresh(); } }); steerLatAccelToggle = static_cast(toggles["SteerLatAccel"]); - QObject::connect(steerLatAccelToggle, &FrogPilotParamValueButtonControl::buttonClicked, [this]() { + QObject::connect(steerLatAccelToggle, &FrogPilotParamValueButtonControl::buttonClicked, [parent, this]() { if (FrogPilotConfirmationDialog::yesorno(tr("Reset Lateral Accel to its default value?"), this)) { - params.putFloat("SteerLatAccel", latAccelFactor); + params.putFloat("SteerLatAccel", parent->latAccelFactor); steerLatAccelToggle->refresh(); } }); steerRatioToggle = static_cast(toggles["SteerRatio"]); - QObject::connect(steerRatioToggle, &FrogPilotParamValueButtonControl::buttonClicked, [this]() { + QObject::connect(steerRatioToggle, &FrogPilotParamValueButtonControl::buttonClicked, [parent, this]() { if (FrogPilotConfirmationDialog::yesorno(tr("Reset Steer Ratio to its default value?"), this)) { - params.putFloat("SteerRatio", steerRatio); + params.putFloat("SteerRatio", parent->steerRatio); steerRatioToggle->refresh(); } }); @@ -258,27 +252,15 @@ FrogPilotLateralPanel::FrogPilotLateralPanel(FrogPilotSettingsWindow *parent) : void FrogPilotLateralPanel::showEvent(QShowEvent *event) { frogpilotToggleLevels = parent->frogpilotToggleLevels; - friction = parent->friction; - hasAutoTune = parent->hasAutoTune; - hasNNFFLog = parent->hasNNFFLog; - hasOpenpilotLongitudinal = parent->hasOpenpilotLongitudinal; - isAngleCar = parent->isAngleCar; - isHKGCanFd = parent->isHKGCanFd; - isTorqueCar = parent->isTorqueCar; - latAccelFactor = parent->latAccelFactor; - steerActuatorDelay = parent->steerActuatorDelay; - steerKp = parent->steerKp; - steerRatio = parent->steerRatio; - tuningLevel = parent->tuningLevel; - steerDelayToggle->setTitle(QString(tr("Actuator Delay (Default: %1)")).arg(QString::number(steerActuatorDelay, 'f', 2))); - steerFrictionToggle->setTitle(QString(tr("Friction (Default: %1)")).arg(QString::number(friction, 'f', 2))); - steerKPToggle->setTitle(QString(tr("Kp Factor (Default: %1)")).arg(QString::number(steerKp, 'f', 2))); - steerKPToggle->updateControl(steerKp * 0.5, steerKp * 1.5); - steerLatAccelToggle->setTitle(QString(tr("Lateral Accel (Default: %1)")).arg(QString::number(latAccelFactor, 'f', 2))); - steerLatAccelToggle->updateControl(latAccelFactor * 0.75, latAccelFactor * 1.25); - steerRatioToggle->setTitle(QString(tr("Steer Ratio (Default: %1)")).arg(QString::number(steerRatio, 'f', 2))); - steerRatioToggle->updateControl(steerRatio * 0.5, steerRatio * 1.5); + steerDelayToggle->setTitle(QString(tr("Actuator Delay (Default: %1)")).arg(QString::number(parent->steerActuatorDelay, 'f', 2))); + steerFrictionToggle->setTitle(QString(tr("Friction (Default: %1)")).arg(QString::number(parent->friction, 'f', 2))); + steerKPToggle->setTitle(QString(tr("Kp Factor (Default: %1)")).arg(QString::number(parent->steerKp, 'f', 2))); + steerKPToggle->updateControl(parent->steerKp * 0.5, parent->steerKp * 1.5); + steerLatAccelToggle->setTitle(QString(tr("Lateral Accel (Default: %1)")).arg(QString::number(parent->latAccelFactor, 'f', 2))); + steerLatAccelToggle->updateControl(parent->latAccelFactor * 0.75, parent->latAccelFactor * 1.25); + steerRatioToggle->setTitle(QString(tr("Steer Ratio (Default: %1)")).arg(QString::number(parent->steerRatio, 'f', 2))); + steerRatioToggle->updateControl(parent->steerRatio * 0.5, parent->steerRatio * 1.5); updateToggles(); } @@ -358,41 +340,41 @@ void FrogPilotLateralPanel::updateToggles() { } } - bool forcingAutoTune = !hasAutoTune && params.getBool("ForceAutoTune"); - bool forcingAutoTuneOff = hasAutoTune && params.getBool("ForceAutoTuneOff"); - bool forcingTorqueController = !isAngleCar && params.getBool("ForceTorqueController"); - bool usingNNFF = hasNNFFLog && params.getBool("LateralTune") && params.getBool("NNFF"); + bool forcingAutoTune = !parent->hasAutoTune && params.getBool("ForceAutoTune"); + bool forcingAutoTuneOff = parent->hasAutoTune && params.getBool("ForceAutoTuneOff"); + bool forcingTorqueController = !parent->isAngleCar && params.getBool("ForceTorqueController"); + bool usingNNFF = parent->hasNNFFLog && params.getBool("LateralTune") && params.getBool("NNFF"); for (auto &[key, toggle] : toggles) { if (parentKeys.contains(key)) { continue; } - bool setVisible = tuningLevel >= frogpilotToggleLevels[key].toDouble(); + bool setVisible = parent->tuningLevel >= frogpilotToggleLevels[key].toDouble(); if (key == "AlwaysOnLateralLKAS") { - setVisible &= isHKGCanFd; - setVisible &= !hasOpenpilotLongitudinal; + setVisible &= parent->isHKGCanFd; + setVisible &= !parent->hasOpenpilotLongitudinal; } else if (key == "AlwaysOnLateralMain") { - setVisible &= !isHKGCanFd; - setVisible |= hasOpenpilotLongitudinal; + setVisible &= !parent->isHKGCanFd; + setVisible |= parent->hasOpenpilotLongitudinal; } else if (key == "ForceAutoTune") { - setVisible &= !hasAutoTune; - setVisible &= !isAngleCar; - setVisible &= isTorqueCar || forcingTorqueController; + setVisible &= !parent->hasAutoTune; + setVisible &= !parent->isAngleCar; + setVisible &= parent->isTorqueCar || forcingTorqueController; } else if (key == "ForceAutoTuneOff") { - setVisible &= hasAutoTune; + setVisible &= parent->hasAutoTune; } else if (key == "ForceTorqueController") { - setVisible &= !isAngleCar; - setVisible &= !isTorqueCar; + setVisible &= !parent->isAngleCar; + setVisible &= !parent->isTorqueCar; } else if (key == "LaneChangeTime") { @@ -404,42 +386,41 @@ void FrogPilotLateralPanel::updateToggles() { } else if (key == "NNFF") { - setVisible &= hasNNFFLog; - setVisible &= !isAngleCar; + setVisible &= parent->hasNNFFLog; + setVisible &= !parent->isAngleCar; } else if (key == "NNFFLite") { setVisible &= !usingNNFF; - setVisible &= !isAngleCar; + setVisible &= !parent->isAngleCar; } else if (key == "SteerDelay") { - setVisible &= steerActuatorDelay != 0; + setVisible &= parent->steerActuatorDelay != 0; } else if (key == "SteerFriction") { - setVisible &= friction != 0; - setVisible &= hasAutoTune ? forcingAutoTuneOff : !forcingAutoTune; - setVisible &= isTorqueCar || forcingTorqueController; + setVisible &= parent->friction != 0; + setVisible &= parent->hasAutoTune ? forcingAutoTuneOff : !forcingAutoTune; + setVisible &= parent->isTorqueCar || forcingTorqueController; setVisible &= !usingNNFF; } else if (key == "SteerKP") { - setVisible &= steerKp != 0; - setVisible &= hasAutoTune ? forcingAutoTuneOff : !forcingAutoTune; - setVisible &= isTorqueCar || forcingTorqueController; + setVisible &= parent->steerKp != 0; + setVisible &= !parent->isAngleCar; } else if (key == "SteerLatAccel") { - setVisible &= latAccelFactor != 0; - setVisible &= hasAutoTune ? forcingAutoTuneOff : !forcingAutoTune; - setVisible &= isTorqueCar || forcingTorqueController; + setVisible &= parent->latAccelFactor != 0; + setVisible &= parent->hasAutoTune ? forcingAutoTuneOff : !forcingAutoTune; + setVisible &= parent->isTorqueCar || forcingTorqueController; setVisible &= !usingNNFF; } else if (key == "SteerRatio") { - setVisible &= steerRatio != 0; - setVisible &= hasAutoTune ? forcingAutoTuneOff : !forcingAutoTune; + setVisible &= parent->steerRatio != 0; + setVisible &= parent->hasAutoTune ? forcingAutoTuneOff : !forcingAutoTune; } toggle->setVisible(setVisible); diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 170fad5bd..d82866924 100644 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -207,7 +207,6 @@ class CarInterface(CarInterfaceBase): elif candidate in (CAR.CHEVROLET_BOLT_EUV, CAR.CHEVROLET_BOLT_CC): ret.steerActuatorDelay = 0.2 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) - ret.lateralTuning.torque.kp = 0.5 if ret.enableGasInterceptor: # ACC Bolts use pedal for full longitudinal control, not just sng @@ -279,7 +278,7 @@ class CarInterface(CarInterfaceBase): # Note: Low speed, stop and go not tested. Should be fairly smooth on highway ret.longitudinalTuning.kiBP = [0., 3., 6., 35.] ret.longitudinalTuning.kiV = [0.125, 0.175, 0.225, 0.33] - ret.longitudinalTuning.kf = 0.25 + ret.longitudinalTuning.kfDEPRECATED = 0.25 ret.stoppingDecelRate = 0.8 else: # Pedal used for SNG, ACC for longitudinal control otherwise ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM_LONG diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 8a65c28a9..2928f4354 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -1,3 +1,4 @@ +import math from collections import namedtuple from cereal import car @@ -5,10 +6,12 @@ from openpilot.common.numpy_fast import clip, interp from openpilot.common.realtime import DT_CTRL from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import create_gas_interceptor_command +from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY from openpilot.selfdrive.car.honda import hondacan from openpilot.selfdrive.car.honda.values import CruiseButtons, VISUAL_HUD, HONDA_BOSCH, HONDA_BOSCH_RADARLESS, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.controls.lib.drive_helpers import rate_limit +from openpilot.selfdrive.controls.lib.pid import PIDController VisualAlert = car.CarControl.HUDControl.VisualAlert LongCtrlState = car.CarControl.Actuators.LongControlState @@ -125,6 +128,10 @@ class CarController(CarControllerBase): self.gas = 0.0 self.brake = 0.0 self.last_steer = 0.0 + self.pitch = 0.0 + self.gasonly_pid = PIDController(k_p=([0,], [0,]), + k_i=([0., 5., 35.], [1.2, 0.8, 0.5]), + rate=1 / DT_CTRL / 2) def update(self, CC, CS, now_nanos, frogpilot_toggles): actuators = CC.actuators @@ -133,6 +140,9 @@ class CarController(CarControllerBase): hud_v_cruise = hud_control.setSpeed / conversion if hud_control.speedVisible else 255 pcm_cancel_cmd = CC.cruiseControl.cancel + if len(CC.orientationNED) == 3: + self.pitch = CC.orientationNED[1] + if CC.longActive: accel = actuators.accel gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, self.CP.carFingerprint) @@ -173,8 +183,11 @@ class CarController(CarControllerBase): CS.CP.openpilotLongitudinalControl)) # wind brake from air resistance decel at high speed - wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15]) + wind_brake_ms2 = interp(CS.out.vEgo, [0.0, 13.4, 22.4, 31.3, 40.2], [0.000, 0.049, 0.136, 0.267, 0.441]) # in m/s2 units + hill_brake = math.sin(self.pitch) * ACCELERATION_DUE_TO_GRAVITY + # all of this is only relevant for HONDA NIDEC + wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15]) # not in m/s2 units max_accel = interp(CS.out.vEgo, self.params.NIDEC_MAX_ACCEL_BP, self.params.NIDEC_MAX_ACCEL_V) # TODO this 1.44 is just to maintain previous behavior pcm_speed_BP = [-wind_brake, @@ -217,12 +230,25 @@ class CarController(CarControllerBase): if self.CP.carFingerprint in HONDA_BOSCH: self.accel = clip(accel, self.params.BOSCH_ACCEL_MIN, self.params.BOSCH_ACCEL_MAX) - self.gas = interp(accel, self.params.BOSCH_GAS_LOOKUP_BP, self.params.BOSCH_GAS_LOOKUP_V) + + if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS: + gas_pedal_force = self.accel # radarless does not need a pid + elif (actuators.longControlState == LongCtrlState.pid) and not CS.out.gasPressed: # perform a gas-only pid + gas_error = self.accel - CS.out.aEgo + self.gasonly_pid.neg_limit = self.params.BOSCH_ACCEL_MIN + self.gasonly_pid.pos_limit = self.params.BOSCH_ACCEL_MAX + gas_pedal_force = self.gasonly_pid.update(gas_error, speed=CS.out.vEgo, feedforward=self.accel) + gas_pedal_force += wind_brake_ms2 + hill_brake + else: + gas_pedal_force = self.accel + self.gasonly_pid.reset() + gas_pedal_force += wind_brake_ms2 + hill_brake + self.gas = interp(gas_pedal_force, self.params.BOSCH_GAS_LOOKUP_BP, self.params.BOSCH_GAS_LOOKUP_V) stopping = actuators.longControlState == LongCtrlState.stopping self.stopping_counter = self.stopping_counter + 1 if stopping else 0 can_sends.extend(hondacan.create_acc_commands(self.packer, self.CAN, CC.enabled, CC.longActive, self.accel, self.gas, - self.stopping_counter, self.CP.carFingerprint)) + self.stopping_counter, self.CP.carFingerprint, accel + wind_brake_ms2 + hill_brake)) else: apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0) apply_brake = int(clip(apply_brake * self.params.NIDEC_BRAKE_MAX, 0, self.params.NIDEC_BRAKE_MAX - 1)) diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index e43b0ba94..f652104e1 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -155,6 +155,11 @@ class CarInterfaceBase(ABC): ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, ret.tireStiffnessFactor) + # FrogPilot variables + toggles_to_check = ("force_torque_controller", "nnff", "nnff_lite") + if ret.steerControlType != car.CarParams.SteerControlType.angle and any(getattr(frogpilot_toggles, toggle, False) for toggle in toggles_to_check): + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + return ret @classmethod @@ -217,14 +222,6 @@ class CarInterfaceBase(ABC): fp_ret.canUsePedal = not CP.autoResumeSng fp_ret.canUseSDSU = not CP.enableDsu and candidate not in UNSUPPORTED_DSU_CAR and candidate not in TSS2_CAR - if CP.steerControlType != car.CarParams.SteerControlType.angle: - if CP.lateralTuning.which() == "pid" and (frogpilot_toggles.force_torque_controller or frogpilot_toggles.nnff or frogpilot_toggles.nnff_lite): - CarInterfaceBase.configure_torque_tune(candidate, fp_ret.lateralTuning) - elif CP.lateralTuning.which() == "torque": - CarInterfaceBase.configure_torque_tune(candidate, fp_ret.lateralTuning) - else: - fp_ret.lateralTuning.init("pid") - fp_ret.openpilotLongitudinalControlDisabled = frogpilot_toggles.disable_openpilot_long return fp_ret @@ -286,7 +283,6 @@ class CarInterfaceBase(ABC): ret.vEgoStopping = 0.5 ret.vEgoStarting = 0.5 ret.stoppingControl = True - ret.longitudinalTuning.kf = 1. ret.longitudinalTuning.kpBP = [0.] ret.longitudinalTuning.kpV = [0.] ret.longitudinalTuning.kiBP = [0.] @@ -302,10 +298,6 @@ class CarInterfaceBase(ABC): tune.init('torque') tune.torque.useSteeringAngle = use_steering_angle - tune.torque.kf = 0.95 - tune.torque.kp = 0.5 - tune.torque.ki = 0.3 - tune.torque.kd = 0.3 tune.torque.friction = params['FRICTION'] tune.torque.latAccelFactor = params['LAT_ACCEL_FACTOR'] tune.torque.latAccelOffset = 0.0 diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index e30f870a2..d6520d065 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -53,7 +53,7 @@ def get_long_tune(CP, params): kiBP = [2., 5.] kiV = [0.5, 0.25] - return PIDController(0.0, (kiBP, kiV), k_f=1.0, + return PIDController(0.0, (kiBP, kiV), pos_limit=params.ACCEL_MAX, neg_limit=params.ACCEL_MIN, rate=1 / (DT_CTRL * 3)) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index dba18a9f0..90e3f23e9 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -34,6 +34,7 @@ from openpilot.frogpilot.tinygrad_modeld.tinygrad_modeld import LAT_SMOOTH_SECON from openpilot.system.hardware import HARDWARE from openpilot.frogpilot.common.frogpilot_variables import get_frogpilot_toggles, params_memory +from openpilot.frogpilot.controls.lib.neural_network_feedforward import LatControlNNFF SOFT_DISABLE_TIME = 3 # seconds LDW_MIN_SPEED = 31 * CV.MPH_TO_MS @@ -137,10 +138,10 @@ class Controls: self.LaC: LatControl if self.CP.steerControlType == car.CarParams.SteerControlType.angle: self.LaC = LatControlAngle(self.CP, self.CI, DT_CTRL) - elif self.FPCP.lateralTuning.which() == 'pid': + elif self.CP.lateralTuning.which() == 'pid': self.LaC = LatControlPID(self.CP, self.CI, DT_CTRL) - elif self.FPCP.lateralTuning.which() == 'torque': - self.LaC = LatControlTorque(self.CP, self.FPCP, self.CI, DT_CTRL) + elif self.CP.lateralTuning.which() == 'torque': + self.LaC = LatControlTorque(self.CP, self.CI, DT_CTRL) self.initialized = False self.state = State.disabled @@ -205,6 +206,10 @@ class Controls: self.frogpilot_toggles = get_frogpilot_toggles() + if self.CP.lateralTuning.which() == "torque" and (self.frogpilot_toggles.nnff or self.frogpilot_toggles.nnff_lite): + self.LaC = LatControlNNFF(self.CP, self.CI, DT_CTRL) + + self.frogpilot_toggles.is_metric = self.is_metric def set_initial_state(self): @@ -616,14 +621,14 @@ class Controls: self.curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll) # Update Torque Params - if self.FPCP.lateralTuning.which() == 'torque': + if self.CP.lateralTuning.which() == 'torque': torque_params = self.sm['liveTorqueParameters'] if self.sm.all_checks(['liveTorqueParameters']) and (torque_params.useParams or self.frogpilot_toggles.force_auto_tune): self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered, torque_params.frictionCoefficientFiltered) - if self.sm.updated['liveDelay'] and (self.frogpilot_toggles.nnff or self.frogpilot_toggles.nnff_lite): - self.LaC.nnff.update_live_delay(self.sm['liveDelay'].lateralDelay) + if self.sm.updated['liveDelay'] and hasattr(self.LaC, "update_live_delay"): + self.LaC.update_live_delay(self.sm['liveDelay'].lateralDelay) long_plan = self.sm['longitudinalPlan'] model_v2 = self.sm['modelV2'] @@ -892,7 +897,7 @@ class Controls: controlsState.experimentalMode = self.experimental_mode controlsState.personality = self.personality - lat_tuning = self.FPCP.lateralTuning.which() + lat_tuning = self.CP.lateralTuning.which() if self.joystick_mode: controlsState.lateralControlState.debugState = lac_log elif self.CP.steerControlType == car.CarParams.SteerControlType.angle: diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 620abd51e..1227ae43d 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -189,7 +189,7 @@ def smooth_value(val, prev_val, tau, dt=DT_MDL): alpha = 1 - np.exp(-dt/tau) if tau > 0 else 1 return alpha * val + (1 - alpha) * prev_val -def clip_curvature(v_ego, prev_curvature, new_curvature, roll): +def clip_curvature(v_ego, prev_curvature, new_curvature, roll) -> tuple[float, bool]: # This function respects ISO lateral jerk and acceleration limits + a max curvature v_ego = max(v_ego, MIN_SPEED) max_curvature_rate = MAX_LATERAL_JERK / (v_ego ** 2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755 diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 73ed3b17b..1944a5a9c 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -10,7 +10,8 @@ class LatControlPID(LatControl): super().__init__(CP, CI, dt) self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), - k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) + pos_limit=self.steer_max, neg_limit=-self.steer_max) + self.ff_factor = CP.lateralTuning.pid.kf self.get_steer_feedforward = CI.get_steer_feedforward_function() def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay, llk, model_data, frogpilot_toggles): @@ -30,7 +31,7 @@ class LatControlPID(LatControl): else: # offset does not contribute to resistive torque - ff = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo) + ff = self.ff_factor * self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo) freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 output_torque = self.pid.update(error, diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index db647ae50..fa6bf76b3 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -4,50 +4,46 @@ from collections import deque from cereal import log from openpilot.common.filter_simple import FirstOrderFilter -from openpilot.selfdrive.controls.lib.drive_helpers import MIN_SPEED, get_friction from openpilot.selfdrive.car.interfaces import FRICTION_THRESHOLD +from openpilot.selfdrive.controls.lib.drive_helpers import MIN_SPEED, get_friction from openpilot.selfdrive.controls.lib.latcontrol import LatControl from openpilot.selfdrive.controls.lib.pid import PIDController from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY -from openpilot.frogpilot.controls.lib.neural_network_feedforward import LOW_SPEED_Y_NN, NeuralNetworkFeedforward - # At higher speeds (25+mph) we can assume: # Lateral acceleration achieved by a specific car correlates to # torque applied to the steering rack. It does not correlate to # wheel slip, or to speed. # This controller applies torque to achieve desired lateral -# accelerations. To compensate for the low speed effects we -# use a LOW_SPEED_FACTOR in the error. Additionally, there is -# friction in the steering wheel that needs to be overcome to -# move it at all, this is compensated for too. +# accelerations. To compensate for the low speed effects the +# proportional gain is increased at low speeds by the PID controller. +# Additionally, there is friction in the steering wheel that needs +# to be overcome to move it at all, this is compensated for too. -MAX_LAT_JERK_UP = 2.5 # m/s^3 - -LOW_SPEED_X = [0, 10, 20, 30] -LOW_SPEED_Y = [15, 13, 10, 5] +KP = 0.6 +KI = 0.3 +KD = 0.0 +INTERP_SPEEDS = [1, 1.5, 2.0, 3.0, 5, 7.5, 10, 15, 30] +KP_INTERP = [250, 120, 65, 30, 11.5, 5.5, 3.5, 2.0, KP] +LP_FILTER_CUTOFF_HZ = 1.2 +LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0 +VERSION = 0 class LatControlTorque(LatControl): - def __init__(self, CP, FPCP, CI, dt): + def __init__(self, CP, CI, dt): super().__init__(CP, CI, dt) - self.torque_params = FPCP.lateralTuning.torque + self.torque_params = CP.lateralTuning.torque self.torque_from_lateral_accel = CI.torque_from_lateral_accel() self.lateral_accel_from_torque = CI.lateral_accel_from_torque() - self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, - k_f=self.torque_params.kf, rate=1/self.dt) + self.pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI, KD, rate=1/self.dt) self.update_limits() self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg - self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES = int(1 / self.dt) - self.requested_lateral_accel_buffer = deque([0.] * self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES , maxlen=self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES) + self.lat_accel_request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt) + self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len , maxlen=self.lat_accel_request_buffer_len) self.previous_measurement = 0.0 - self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * (MAX_LAT_JERK_UP - 0.5)), self.dt) - - # FrogPilot variables - self.nnff = NeuralNetworkFeedforward(CP, self) - - self.nnff_loaded = self.nnff.lat_torque_nn_model != None + self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt) def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): self.torque_params.latAccelFactor = latAccelFactor @@ -61,6 +57,7 @@ class LatControlTorque(LatControl): def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay, llk, model_data, frogpilot_toggles): pid_log = log.ControlsState.LateralTorqueState.new_message() + pid_log.version = VERSION if not active: output_torque = 0.0 pid_log.active = False @@ -70,11 +67,11 @@ class LatControlTorque(LatControl): curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 - delay_frames = int(np.clip(lat_delay / self.dt, 1, self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES)) - expected_lateral_accel = self.requested_lateral_accel_buffer[-delay_frames] + delay_frames = int(np.clip(lat_delay / self.dt, 1, self.lat_accel_request_buffer_len)) + expected_lateral_accel = self.lat_accel_request_buffer[-delay_frames] # TODO factor out lateral jerk from error to later replace it with delay independent alternative future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2 - self.requested_lateral_accel_buffer.append(future_desired_lateral_accel) + self.lat_accel_request_buffer.append(future_desired_lateral_accel) gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation desired_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / lat_delay @@ -82,47 +79,34 @@ class LatControlTorque(LatControl): measurement_rate = self.measurement_rate_filter.update((measurement - self.previous_measurement) / self.dt) self.previous_measurement = measurement - low_speed_factor = (np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y_NN if frogpilot_toggles.nnff else LOW_SPEED_Y) / max(CS.vEgo, MIN_SPEED)) ** 2 setpoint = lat_delay * desired_lateral_jerk + expected_lateral_accel error = setpoint - measurement - error_lsf = error + low_speed_factor / self.torque_params.kp * error - if self.nnff_loaded and frogpilot_toggles.nnff or frogpilot_toggles.nnff_lite: - pid_log, ff = self.nnff.compute_nnff( - CS, VM, measurement, error, future_desired_lateral_accel, gravity_adjusted_future_lateral_accel, - llk, measurement, model_data, params, pid_log, setpoint, frogpilot_toggles - ) + # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly + pid_log.error = float(error) + ff = gravity_adjusted_future_lateral_accel + # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll + ff -= self.torque_params.latAccelOffset + # TODO jerk is weighted by lat_delay for legacy reasons, but should be made independent of it + ff += get_friction(error, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) - freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 - output_torque = self.pid.update(pid_log.error, + freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 + output_lataccel = self.pid.update(pid_log.error, + -measurement_rate, feedforward=ff, speed=CS.vEgo, freeze_integrator=freeze_integrator) - else: - # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly - pid_log.error = float(error_lsf) - ff = gravity_adjusted_future_lateral_accel - # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll - ff -= self.torque_params.latAccelOffset - # TODO jerk is weighted by lat_delay for legacy reasons, but should be made independent of it - ff += get_friction(error, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) - - freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 - output_lataccel = self.pid.update(pid_log.error, - -measurement_rate, - feedforward=ff, - speed=CS.vEgo, - freeze_integrator=freeze_integrator) - output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params) + output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params) pid_log.active = True pid_log.p = float(self.pid.p) pid_log.i = float(self.pid.i) pid_log.d = float(self.pid.d) pid_log.f = float(self.pid.f) - pid_log.output = float(-output_torque) # TODO: log lat accel? + pid_log.output = float(-output_torque) # TODO: log lat accel? pid_log.actualLateralAccel = float(measurement) pid_log.desiredLateralAccel = float(setpoint) + pid_log.desiredLateralJerk = float(desired_lateral_jerk) pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited)) # TODO left is positive in this convention diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 6563b142d..afb0a018f 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -95,8 +95,10 @@ class LongControl: pos_p_limit = 0.0 # if params("NoPositivePResponse") else None # put parameter-based control here self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), - k_f=CP.longitudinalTuning.kf, rate=1 / DT_CTRL, - pos_p_limit=pos_p_limit) + rate=1 / DT_CTRL, pos_p_limit=pos_p_limit) + # Preserve legacy behaviour when no feedforward gain is provided (default of 0.0) + kf = getattr(CP.longitudinalTuning, 'kfDEPRECATED', 0.0) + self.feedforward_gain = kf if kf != 0.0 else 1.0 self.v_pid = 0.0 self._mode_setup() self.last_output_accel = 0.0 @@ -159,7 +161,8 @@ class LongControl: else: # LongCtrlState.pid error = a_target - CS.aEgo self.update_mpc_mode(self.experimental_mode) - raw_output_accel = self.pid.update(error, speed=CS.vEgo, feedforward=a_target) + feedforward = a_target * self.feedforward_gain + raw_output_accel = self.pid.update(error, speed=CS.vEgo, feedforward=feedforward) if self.transitioning and self.prev_mode == 'acc' and self.current_mode == 'blended': @@ -236,8 +239,9 @@ class LongControl: error = self.v_pid - CS.vEgo error_deadzone = apply_deadzone(error, deadzone) + feedforward = a_target * self.feedforward_gain output_accel = self.pid.update(error_deadzone, speed=CS.vEgo, - feedforward=a_target, + feedforward=feedforward, freeze_integrator=freeze_integrator) self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1]) diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index a076ea2e0..fad8ce581 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -1,17 +1,11 @@ import numpy as np from numbers import Number -from openpilot.common.numpy_fast import clip, interp - - class PIDController: - def __init__(self, k_p, k_i, k_f=0., k_d=0., - pos_limit=1e308, neg_limit=-1e308, rate=100, - pos_p_limit=None, neg_p_limit=None): + def __init__(self, k_p, k_i, k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100, pos_p_limit=None, neg_p_limit=None): self._k_p = k_p self._k_i = k_i self._k_d = k_d - self.k_f = k_f # feedforward gain if isinstance(self._k_p, Number): self._k_p = [[0], [self._k_p]] if isinstance(self._k_i, Number): @@ -19,13 +13,8 @@ class PIDController: if isinstance(self._k_d, Number): self._k_d = [[0], [self._k_d]] - self.pos_limit = pos_limit - self.neg_limit = neg_limit + self.set_limits(pos_limit, neg_limit) - self.pos_p_limit = pos_p_limit - self.neg_p_limit = neg_p_limit - - self.i_unwind_rate = 0.3 / rate self.i_dt = 1.0 / rate self.speed = 0.0 @@ -33,23 +22,15 @@ class PIDController: @property def k_p(self): - return interp(self.speed, self._k_p[0], self._k_p[1]) + return np.interp(self.speed, self._k_p[0], self._k_p[1]) @property def k_i(self): - return interp(self.speed, self._k_i[0], self._k_i[1]) + return np.interp(self.speed, self._k_i[0], self._k_i[1]) @property def k_d(self): - return interp(self.speed, self._k_d[0], self._k_d[1]) - - @property - def error_integral(self): - return self.i/self.k_i - - def set_limits(self, pos_limit, neg_limit): - self.pos_limit = pos_limit - self.neg_limit = neg_limit + return np.interp(self.speed, self._k_d[0], self._k_d[1]) def reset(self): self.p = 0.0 @@ -58,29 +39,25 @@ class PIDController: self.f = 0.0 self.control = 0 - def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0., freeze_integrator=False): + def set_limits(self, pos_limit, neg_limit): + self.pos_limit = pos_limit + self.neg_limit = neg_limit + + def update(self, error, error_rate=0.0, speed=0.0, feedforward=0., freeze_integrator=False): self.speed = speed - self.p = self.k_p * float(error) - if self.pos_p_limit is not None and self.p > self.pos_p_limit: - self.p = self.pos_p_limit - elif self.neg_p_limit is not None and self.p < self.neg_p_limit: - self.p = self.neg_p_limit self.d = self.k_d * error_rate - self.f = self.k_f * feedforward + self.f = feedforward - if override: - self.i -= self.i_unwind_rate * float(np.sign(self.i)) - else: - if not freeze_integrator: - self.i = self.i + self.k_i * self.i_dt * error + if not freeze_integrator: + i = self.i + self.k_i * self.i_dt * error - # Clip i to prevent exceeding control limits - control_no_i = self.p + self.d + self.f - control_no_i = clip(control_no_i, self.neg_limit, self.pos_limit) - self.i = clip(self.i, self.neg_limit - control_no_i, self.pos_limit - control_no_i) + # Don't allow windup if already clipping + test_control = self.p + i + self.d + self.f + i_upperbound = self.i if test_control > self.pos_limit else self.pos_limit + i_lowerbound = self.i if test_control < self.neg_limit else self.neg_limit + self.i = np.clip(i, i_lowerbound, i_upperbound) control = self.p + self.i + self.d + self.f - - self.control = clip(control, self.neg_limit, self.pos_limit) + self.control = np.clip(control, self.neg_limit, self.pos_limit) return self.control diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 5ef0ebcae..ac1f9abfe 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -187,8 +187,6 @@ def main(): # FrogPilot variables frogpilot_toggles = get_frogpilot_toggles() - with custom.FrogPilotCarParams.from_bytes(params_reader.get("FrogPilotCarParams", block=True)) as msg: - FPCP = msg while True: sm.update() @@ -240,7 +238,7 @@ def main(): 0.2 <= liveParameters.stiffnessFactor <= 5.0, min_sr <= liveParameters.steerRatio <= max_sr, )) - if CP.carFingerprint == "RAM_HD" or CP.carName == "subaru" and FPCP.lateralTuning.which() == "torque": + if CP.carFingerprint == "RAM_HD" or CP.carName == "subaru" and CP.lateralTuning.which() == "torque": liveParameters.valid = True liveParameters.steerRatioStd = float(P[States.STEER_RATIO].item()) liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item()) diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 051f4a583..160c9f72f 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -52,7 +52,7 @@ class TorqueBuckets(PointBuckets): class TorqueEstimator(ParameterEstimator): - def __init__(self, CP, FPCP, decimated=False): + def __init__(self, CP, decimated=False): self.hist_len = int(HISTORY / DT_MDL) self.lag = 0.0 if decimated: @@ -72,11 +72,11 @@ class TorqueEstimator(ParameterEstimator): self.offline_friction = 0.0 self.offline_latAccelFactor = 0.0 self.resets = 0.0 - self.use_params = CP.carName in ALLOWED_CARS and FPCP.lateralTuning.which() == 'torque' + self.use_params = CP.carName in ALLOWED_CARS and CP.lateralTuning.which() == 'torque' - if FPCP.lateralTuning.which() == 'torque': - self.offline_friction = FPCP.lateralTuning.torque.friction - self.offline_latAccelFactor = FPCP.lateralTuning.torque.latAccelFactor + if CP.lateralTuning.which() == 'torque': + self.offline_friction = CP.lateralTuning.torque.friction + self.offline_latAccelFactor = CP.lateralTuning.torque.latAccelFactor self.reset() @@ -102,7 +102,7 @@ class TorqueEstimator(ParameterEstimator): cache_ltp = log_evt.liveTorqueParameters with car.CarParams.from_bytes(params_cache) as msg: cache_CP = msg - if self.get_restore_key(cache_CP, FPCP, cache_ltp.version) == self.get_restore_key(CP, FPCP, VERSION): + if self.get_restore_key(cache_CP, cache_ltp.version) == self.get_restore_key(CP, VERSION): if cache_ltp.liveValid: initial_params = { 'latAccelFactor': cache_ltp.latAccelFactorFiltered, @@ -121,12 +121,12 @@ class TorqueEstimator(ParameterEstimator): for param in initial_params: self.filtered_params[param] = FirstOrderFilter(initial_params[param], self.decay, DT_MDL) - def get_restore_key(self, CP, FPCP, version): + def get_restore_key(self, CP, version): a, b = None, None - if FPCP.lateralTuning.which() == 'torque': - a = FPCP.lateralTuning.torque.friction - b = FPCP.lateralTuning.torque.latAccelFactor - return (CP.carFingerprint, FPCP.lateralTuning.which(), a, b, version) + if CP.lateralTuning.which() == 'torque': + a = CP.lateralTuning.torque.friction + b = CP.lateralTuning.torque.latAccelFactor + return (CP.carFingerprint, CP.lateralTuning.which(), a, b, version) def reset(self): self.resets += 1.0 @@ -227,14 +227,14 @@ def main(demo=False): sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveLocationKalman', 'liveDelay', 'frogpilotPlan'], poll='liveLocationKalman') params = Params() - with car.CarParams.from_bytes(params.get("CarParams", block=True)) as CP, custom.FrogPilotCarParams.from_bytes(params.get("FrogPilotCarParams", block=True)) as FPCP: - estimator = TorqueEstimator(CP, FPCP) + with car.CarParams.from_bytes(params.get("CarParams", block=True)) as CP: + estimator = TorqueEstimator(CP) # FrogPilot variables frogpilot_toggles = get_frogpilot_toggles() if not frogpilot_toggles.liveValid: - estimator = TorqueEstimator(CP, FPCP, decimated=True) + estimator = TorqueEstimator(CP, decimated=True) while True: sm.update() diff --git a/selfdrive/ui/_spinner b/selfdrive/ui/_spinner index 9e9c29f3e..a93577b46 100755 Binary files a/selfdrive/ui/_spinner and b/selfdrive/ui/_spinner differ diff --git a/selfdrive/ui/_text b/selfdrive/ui/_text index 64914bf67..a91446876 100755 Binary files a/selfdrive/ui/_text and b/selfdrive/ui/_text differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index ec117ae04..7cc0d72dc 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/hardware/fan_controller.py b/system/hardware/fan_controller.py index f32133f6b..cd06e3981 100755 --- a/system/hardware/fan_controller.py +++ b/system/hardware/fan_controller.py @@ -18,7 +18,7 @@ class TiciFanController(BaseFanController): cloudlog.info("Setting up TICI fan handler") self.last_ignition = False - self.controller = PIDController(k_p=0, k_i=4e-3, k_f=1, rate=(1 / DT_HW)) + self.controller = PIDController(k_p=0, k_i=4e-3, rate=(1 / DT_HW)) def update(self, cur_temp: float, ignition: bool) -> int: self.controller.neg_limit = -(100 if ignition else 30) @@ -35,4 +35,3 @@ class TiciFanController(BaseFanController): self.last_ignition = ignition return fan_pwr_out -