Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

update DH names + notes for MPC output curvatures #24701

Merged
merged 4 commits into from
Jun 7, 2022

Conversation

ClockeNessMnstr
Copy link
Contributor

"current_curvature" is not the correct description of what the MPC is outputting in it's curvature_ego state.
The MPC is integrating it's free variable, curvature_rate, such that curvature[0] is actually the desired_curvature before any delay.
inversely: the curvature_rate_desired is the desired rate of change to the setpoint and not the actual curvature rate.

If we were to set the initial curvature = measured curvature in the MPC initiation these names would be correct.
This was possibly how it was initially set up but the nomenclature here is now confusing.

"current_curvature" is not the correct description of what the MPC is outputting in it's curvature_ego state.
The MPC is integrating it's free variable, curvature_rate, such that curvature[0] is actually the desired_curvature before any delay.
inversely: the curvature_rate_desired is the desired rate of change to the setpoint and not the actual curvature rate. 

If we were to set the initial curvature = measured curvature in the MPC initiation these names would be correct. 
This was possibly how it was initially set up but the nomenclature here is now confusing.
@ClockeNessMnstr ClockeNessMnstr changed the title update names + notes for MPC outputs update DH names + notes for MPC output curvatures Jun 1, 2022
@ClockeNessMnstr ClockeNessMnstr mentioned this pull request Jun 3, 2022
@@ -79,6 +79,9 @@ def update(self, sm):
y_pts,
heading_pts)
# init state for next
# mpc.u_sol is the desired curvature rate from x0[3].
# we integrate here so that x_sol[:,3] is the desired curvature for lat_control
# if we set x0[3] = measured_curvature the output would be desired_curvature_rate
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't understand this whole comment block. Integrate or Interpolate.

Copy link
Contributor Author

@ClockeNessMnstr ClockeNessMnstr Jun 3, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Wanted to clarify it's interpolating over the integral of rate, since rate is really the MPC's "output" state.

This is better maybe:

# mpc.u_sol is the desired curvature rate given x0 curv state. 
# with x0[3] = measured_curvature, this would be the actual desired rate.
# instead, interpolate x_sol so that x0[3] is the desired curvature for lat_control.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah that makes sense!

@haraschax
Copy link
Contributor

LGTM besides that comment block

@haraschax haraschax merged commit b215d61 into commaai:master Jun 7, 2022
@ClockeNessMnstr ClockeNessMnstr deleted the patch-1 branch June 7, 2022 17:52
mmmorks pushed a commit to mmmorks/openpilot that referenced this pull request Jun 14, 2022
* update names + notes for MPC outputs

"current_curvature" is not the correct description of what the MPC is outputting in it's curvature_ego state.
The MPC is integrating it's free variable, curvature_rate, such that curvature[0] is actually the desired_curvature before any delay.
inversely: the curvature_rate_desired is the desired rate of change to the setpoint and not the actual curvature rate. 

If we were to set the initial curvature = measured curvature in the MPC initiation these names would be correct. 
This was possibly how it was initially set up but the nomenclature here is now confusing.

* more notes

* match

* Clarify commaai#1
mmmorks pushed a commit to mmmorks/openpilot that referenced this pull request Jun 15, 2022
* update names + notes for MPC outputs

"current_curvature" is not the correct description of what the MPC is outputting in it's curvature_ego state.
The MPC is integrating it's free variable, curvature_rate, such that curvature[0] is actually the desired_curvature before any delay.
inversely: the curvature_rate_desired is the desired rate of change to the setpoint and not the actual curvature rate.

If we were to set the initial curvature = measured curvature in the MPC initiation these names would be correct.
This was possibly how it was initially set up but the nomenclature here is now confusing.

* more notes

* match

* Clarify commaai#1

(cherry picked from commit b215d61)
ntegan1 pushed a commit to ntegan1/apilot that referenced this pull request Jun 16, 2022
* update names + notes for MPC outputs

"current_curvature" is not the correct description of what the MPC is outputting in it's curvature_ego state.
The MPC is integrating it's free variable, curvature_rate, such that curvature[0] is actually the desired_curvature before any delay.
inversely: the curvature_rate_desired is the desired rate of change to the setpoint and not the actual curvature rate.

If we were to set the initial curvature = measured curvature in the MPC initiation these names would be correct.
This was possibly how it was initially set up but the nomenclature here is now confusing.

* more notes

* match

* Clarify #1
dkiiv added a commit to dkiiv/openpilot that referenced this pull request Jun 16, 2022
* Laikad: cleanup fetching orbits (commaai#24759)

* Seperate prediction orbits from regular observation orbits and download them efficient

* Cleanup

* clean and update laika

* Fix test

* Fix test

* Fix checking pos fix

* space

* update DH names + notes for MPC output curvatures (commaai#24701)

* update names + notes for MPC outputs

"current_curvature" is not the correct description of what the MPC is outputting in it's curvature_ego state.
The MPC is integrating it's free variable, curvature_rate, such that curvature[0] is actually the desired_curvature before any delay.
inversely: the curvature_rate_desired is the desired rate of change to the setpoint and not the actual curvature rate. 

If we were to set the initial curvature = measured curvature in the MPC initiation these names would be correct. 
This was possibly how it was initially set up but the nomenclature here is now confusing.

* more notes

* match

* Clarify #1

* Laikad: Use process for parsing orbits (commaai#24769)

* Use Process instead of Thread to fetch orbits

* small refactor

* Cleanup

* UI: remove DM icon (commaai#24771)

* PlotJuggler: add torque control layout (commaai#24726)

* add torque control PJ layout

* less custom transformation

* Use curvature, less noisy

* remove that

* Honda: rename Bosch harness to Bosch A

* GM: interface cleanup (commaai#24774)

* Move all defaults above models

* Remove reduntant/defaults

* make minEnableSpeed common

* Update selfdrive/car/gm/interface.py

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

Co-authored-by: Shane Smiskol <shane@smiskol.com>
Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

* IsoTpParallelQuery: handle response pending (commaai#24724)

* handle response pending

* match commit

* remove total timeout, just keep track of individual response timeouts

* fix

* add back total timeout

* this isn't reliable enough

* keep track of pending responses to print warning

* tx_addr is (addr, subaddr)

* debug

only query hyundai

import time

reponse pending

no cache

all cars

no timeout to test before

* Revert "debug"

This reverts commit abe9cfc.

* always print pending

always debug

* Only debug

* Update selfdrive/car/isotp_parallel_query.py

* remove variable only for debugging

* Install readme (commaai#24776)

README: better install on device instructions

* VW MQB: Add FW for 2020 Volkswagen Tiguan (commaai#24777)

* Kia Ceed: fix eps ECU type (commaai#24767)

should be eps

* Add missing MIRAI ESP f/w (commaai#24779)

* Add missing ESP f/w 

`@JaySheikh#7759`  2021 Mirai  DongleID/route b2c2b20082938bed|2022-06-08--05-04-03  [confirmed working]

* formatting

Co-authored-by: Shane Smiskol <shane@smiskol.com>

* Laikad: kill process correctly. Fixes CI (commaai#24780)

Kill process correctly

* LaikaD: Fix offline handling (commaai#24781)

* Test handling no internet correctly.

* Clean

* Comment

* remove del

* fullframe DM model (commaai#24762)

* get log

* simplify two nonsense

* not needed

* libyuv is a joke

* clean up

* try small

* fast but not bad

* working

* clean up driverview

* simplified

* thats mirrored

* smol

* tweak

* ref is screen

* w/ ee

* update camera model

* no if TICI

* start

* update pose thresh

* less cpu more dsp

* new libyuv

* new snpe

* add files

* test

* should be fast

* update out len

* trigger test

* use master snpe

* add cereal

* update cereal

* refactor parsing

* missing ;

* get

* wrong type

* test model

* use driver data

* 10829278-72fe-4283-a118-2cef959ce174/1550

* no pf

* adapt driverview

* ;

* rhd learner

* update libyuv buildi x64

* ad4337e

* remove blink slack

* test

* no

* use toggle

* b16

* fix for nv12

* 5b02cff5 both

* update test

* update cereal

* update cereal

* update cereal

* v2 packets

* revert libyuv

* no /

* update snpemodel

* ;

* memcpy

* fix test

* use toggle in driverview

* update power

* update replay

* Revert "update replay"

This reverts commit 1d0979c.

* update model ref

* halve cpu

* fake 8bit onnx runner

* same thresh as report

* cereal master

Co-authored-by: Comma Device <device@comma.ai>

* bump opendbc (commaai#24790)

* Honda Bosch: fix openpilot longitudinal controls mismatch (commaai#24788)

* fix bosch controls mismatch

* bump

* bump panda

* VW MQB: Add FW for 2020 Volkswagen Tiguan (commaai#24795)

* build checks do not rely on each other (commaai#24783)

* Add missing CHR engine f/w (commaai#24797)

`@papifrio#5178` 2019 Toyota C-HR ICE ("nodsu" TSS-P) DongleID/route ade67da77ee74b16|2022-06-09--00-36-15

* Lexus: NX Hybrid 2020 support (commaai#24796)

* LEXUS: Missing esp and engine FW versions in values.py

DongleId: 09ae96064ed85a14  | testRoute: 09ae96064ed85a14|2022-01-10--01-57-37 |

* Make a new platform for NX Hybrid TSS2

* Update releases

Co-authored-by: Shane Smiskol <shane@smiskol.com>

* some release notes

* EyeSight standard on all 2019+ Ascent and Forester (commaai#24799)

* on all 2019+ Ascent Forester

* update docs

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

* fullframe DM: flip RHD yaw to use matching thresholds

* Toyota: Add missing esp fw version for RAV4_TSS2 (commaai#24793)

add fwVersion for RAV4_TSS2

add b'\x01F15260R302\x00\x00\x00\x00\x00\x00' to list of Ecu.esp FwVersions for RAV4_TSS2

* Revert "UI: remove DM icon (commaai#24771)"

This reverts commit 4d836c6.

* Hyundai: Tucson 2021 support (commaai#24791)

* add support for 2021 Hyundai Tucson

base configuration and fingerprints

* fix fcw again and don't use mando radar for tucson

* last fixes for working tucson

* Apply suggestions from code review

* add to cars

* increase steerRatio

* missing car info

* one platform

* rename

* add min enable speed

* update releases

Co-authored-by: bluesforte <harry3b9@gmail.com>
Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

* docs: adjust steering torque thresholds (commaai#24798)

* Prius has good steering control now

* 1.5 is the threshold for good torque

* get back down there, hondas

* half stars for 1.0-1.5

* show number of cars

* try bigger

* emphasize tiers

balanced

* Add half star

* Update ref_commit

* bypass HDA2 dashcam with file (commaai#24803)

Co-authored-by: Comma Device <device@comma.ai>

* Honda: use common imperial unit message (commaai#24786)

* CAR_SPEED should be on all cars

* bump opendbc

* clean this up too

* it's an or

* clean up

* comment

* add PyQt (commaai#24811)

* count events: add simple camera debugging

* Revert fullframe DM model (commaai#24812)

* Revert "fullframe DM: flip RHD yaw to use matching thresholds"

This reverts commit 2ac6931.

* Revert "fullframe DM model (commaai#24762)"

This reverts commit d6c07a6.

* revert cereal

* Add missing fw versions for 2019 Sonata (commaai#24814)

* put cereal on master

* Car interface: set max lateral torque from table (commaai#24789)

* json

* better naem

* Read from table

* formatting and default to nan

* Generate docs

* Read from table

* this should be the same

* Prius v is full

* test we always set the tunes correctly

add to release files

* Set for all cars

Set for all cars

* Revert tuning changes

Revert tuning changes

* remove that

* fixes

* update ref commit for new maxLateralAccels

Co-authored-by: Shane Smiskol <shane@smiskol.com>

* Fix Lexus NX Hybrid 2020 engine ecu (commaai#24817)

Wrong address in engine ecu.

* compatibility docs: auto-generate star descriptions (commaai#24809)

* Auto-generate star descriptions

* Need this for the website

* And this

* required changes to make the website generation work

* better names

* Revert "better names"

This reverts commit be7dbbb.

* simpler

* Misc torque control fixes (commaai#24801)

* Fiction compensation should be based on error

* Update refs

* Add deadzone

* update ref

* Honda carcontroller and signal cleanup (commaai#24806)

* common signals

* move stopping

* space

* clean up

* bump opendbc

* test for the strip_bz2_extension method (commaai#24826)

* CI: fix docker push for base image

* pylint: add PyQt5 support

* simulator: run simulator test in ci (commaai#24691)

* run simulator test in ci

* block navd process

* block ui

* fix jenkins

* build docker

* remove tty

* remove tty for carla

* detach carla_sim

* more retries

* only build once

* add more time for bridge

* cleanup

* use qt offscreen

* expose to docker

* block ui

* use new dockerimage

* fix

* from ubuntu20.04

* install curl

* add ssh

* add locales

* noninteractive

* syntax

* use base

* smaller image

* add git + git lfs

* kill carla

* run in parallel

* fix missing agents

* default agent?

* little cleanup

* default doesn't work

* not in ci

* fix path

* fix path

* new msg

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

* Move selfdrive/hardware/ to system/ (commaai#24725)

* move hardware to system/

* fix mypy

* jenkins: remove unnecessary workstation clean

* bump cereal

* set AGNOS from /AGNOS file

* CI: add build job for latest Ubuntu (commaai#24637)

* CI: add build job for latest Ubuntu

* source

* source env

* scons cache

* cache pyenv

* fix key

* source

* Move a bunch of stuff to system/ part 3 (commaai#24829)

* move swaglog.py

* timezoned

* logmessaged

* version.py

* fix linter

* acados build script improvements for mac (commaai#24634)

* add Darwin build w/ universal2 libs

* add rust for acados rebuilds

* just build script fixes

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

* Ford: disable radar for now (commaai#24832)

The newer Ford vehicles require a different radar parser.

* Updated CARLA to v0.9.13 (commaai#24575)

* Updated CARLA to v0.9.13

* pipenv lock

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

* misc jenkins fixups (commaai#24840)

* bump cereal

* remove that

* pull cl image

* lil docker cleanup

* Ford: remove Fusion DBC from release (commaai#24841)

* jenkins: lock simulator

* SConstruct: set AGNOS from /AGNOS

* Revert "SConstruct: set AGNOS from /AGNOS" until Jenkins devices are updated

This reverts commit 84c741e.

* Laikad: Use filter for correcting measurements (commaai#24824)

* Update laikad.

* Update log error

* ui: skip texture frame copy (commaai#24700)

* ui_blit working

* simpler and working

* more believable that it's real

* working on device

* build on pc

* use hardware pc

* reduce cpu usage

* yuv conversion to EGL

* move everything to cameraview

* some cleanup

* more cleanup

* init array

* init images with std::map

* dont destroy images

* do destroy images

Co-authored-by: Comma Device <device@comma.ai>

* Laikad: process executor to fetch orbits (commaai#24843)

* Use ProcessPoolExecutor to fetch orbits

* update laika repo

* Minor

* camerad: remove unused SubMaster (commaai#24844)

* navd: speed limits only when localizer is valid (commaai#24845)

* Toyota Camry TSS2: update torque control params (commaai#24819)

Use updated accel and friction values for TSS2 Camry

* ui.py: update for nv12

* Revert "thermald: consider pmic in component temp management (commaai#24708)"

This reverts commit c8c21ba.

* Couple more cars to torque tune (commaai#24848)

* try sonata on torque tune

* Couple known cars to torque control

* fix

* more fix

* Honda Bosch long: fix ACC fault (commaai#24851)

Fix Honda bosch long

Co-authored-by: redacid95 <jonathanc.olivier@gmail.com>

Co-authored-by: redacid95 <jonathanc.olivier@gmail.com>

* Move couple toyotas to torque table values (commaai#24849)

* Move couple toyotas to torque table values

* Dont set for all cars yet

* Dont regress docs

* update ref

* Move RAV4 2022 out of bronze

* pin protobuf and hypothesis versions (commaai#24852)

pin hypothesis and protobuf

* bump opendbc (commaai#24853)

* process replay: clean up common code (commaai#24855)

* regen and process replay clean up

* test_fuzzy actually uses fingerprint hardcoding

fix

* revert

* revert

* this can be a url or path so just print full variable

* remove weights fixup with new SNPE (commaai#24254)

* remove weights fixup with new SNPE

* Update ref

Co-authored-by: Comma Device <device@comma.ai>
Co-authored-by: Harald Schafer <harald.the.engineer@gmail.com>

* GM: add support for vehicles with manual parking brakes (commaai#24766)

Switch to general park brake signal

* little zookeeper fixes (commaai#24861)

* little zookeeper fixes

* bump that up

* bump up modeld power draw threshold

* Torque control: low speed boost (commaai#24859)

* Make very low speed more aggressive

* Less extreme low speed boost

* Update ref

* regen & process replay: support no disengage on accelerator (commaai#24850)

* ACC on if enabled != 0

* small regen clean up and add HONDA3

* fixes

* revert unneeded changes

* not used

* just alt exp

Co-authored-by: redacid95 <jonathanc.olivier@gmail.com>

* Laikad: Cache orbit and nav data (commaai#24831)

* Cache orbit and nav data

* Cleanup

* Cleanup

* Use ProcessPoolExecutor to fetch orbits

* update laika repo

* Minor

* Create json de/serializers
Save cache only 1 minute at max

* Update laika repo

* Speed up json by caching json in ephemeris class

* Update laika

* Fix test

* Use constant

* UI: new set speed design, show speed limits (commaai#24736)

* basic US design

* place based on center position

* fix typo

* eu sign without rounded box

* same as steering wheel icon

* proper rounded bottom for eu sign

* add border

* proper placement/sizes

* needs to be semi bold

* color changes

* only when engaged

* move helpers into util.h

* Fix MAX placement

* only change color when at least 5 over

* implement override state

* pixel perfect spacing around us sign

* add speed limits to release notes

* Laikad: Remove bearingDeg from message (commaai#24864)

* Remove bearingDeg from message.

* Push cereal

* Commit cereal

* ui: add black color define (commaai#24866)

* ui: advanced network settings fix button colors and sizes (commaai#24846)

* ui: button pressed color add

* match colors from other buttons

Co-authored-by: Willem Melching <willem.melching@gmail.com>

* networking.cc: remove resolved TODO

* selfdrive/ui/qt/util.cc: missing include (commaai#24867)

* laikad: calc_pos_fix numpy implementation (commaai#24865)

* Replace posfix with gauss newton method

* Cleanup

* Check if glonass is in the list

* Fix

* also return residual

* Add residuals

* Update selfdrive/locationd/laikad.py

Co-authored-by: Willem Melching <willem.melching@gmail.com>

* Cleanup

Co-authored-by: Gijs Koning <gijs-koning@live.nl>

* Update model ref

* Revert "Update model ref"

This reverts commit efc8aa0.

* Rocket league model (commaai#24869)

* dd9a502d-c8e2-4831-b365-804b0ae0739d/600 80041070-d276-4fed-bdb9-0075e5442908/420

* no elementwise op

* 9dabf0f-2e60-44bf-8d3a-d20a74aca072/600 ae746590-0bb5-4a16-80db-15f02d314f03/300 c4663a12-b499-4c9b-90dd-b169e3948cb1/60

* explicit slice

* some copies are useful

* 1456d261-d232-4654-8885-4d9fde883894/440 c06eba55-1931-4e00-9d63-acad00161be0/700 af2eb6ba-1935-4318-aaf8-868db81a4932/425

* 154f663e-d3e9-4020-ad49-0e640588ebbe/399 badb5e69-504f-4544-a99e-ba75ed204b74/800 08330327-7663-4874-af7a-dcbd2c994ba7/800

* set steer rate cost to 1.0

* smaller temporal size

* Update model reg

* update model ref again

* This did upload somehow

* Update steer rate cost

Co-authored-by: Yassine Yousfi <yyousfi1@binghamton.edu>

* HKG: simplify Kia K5 compatibility (commaai#24810)

LKAS/LFA is standard on the K5

* thermald: fix panda dropout when we miss a pandaStates (commaai#24870)

immediate fix for "panda dropout"

* VW MQB: Populate stock ACC standstill flag (commaai#24877)

* VW MQB: Populate stock ACC standstill flag

* it really do be like that sometimes

* Mazda: fix resume spam at standstill (commaai#24876)

* Fix Mazda resume spam at standstill

* one line

* Revert "one line"

This reverts commit 30c6504.

* Laikad: More logging and use last_pos_fix for correcting (commaai#24868)

Use last available pos_fix for correcting measurements.
Improve logging measurements

* ui: disable controls unresponsive alert on PC

* HCA7 + ramp speed

* bump openDBC

* bump openDBC

* HCA7 + ramp speed

* bump panda

* bump panda

* HCA7 tuning from edgy

Co-authored-by: Gijs Koning <gijs-koning@live.nl>
Co-authored-by: ClockeNessMnstr <locke.dftc@gmail.com>
Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
Co-authored-by: Shane Smiskol <shane@smiskol.com>
Co-authored-by: Jason Shuler <jshuler@gmail.com>
Co-authored-by: HaraldSchafer <harald.the.engineer@gmail.com>
Co-authored-by: Jason Young <46612682+jyoung8607@users.noreply.github.com>
Co-authored-by: Erich Moraga <33645296+ErichMoraga@users.noreply.github.com>
Co-authored-by: ZwX1616 <zwx1616@gmail.com>
Co-authored-by: Comma Device <device@comma.ai>
Co-authored-by: AlexandreSato <66435071+AlexandreSato@users.noreply.github.com>
Co-authored-by: George Hotz <72895+geohot@users.noreply.github.com>
Co-authored-by: Griffin Christenson <36981741+griff12@users.noreply.github.com>
Co-authored-by: bluesforte <harry3b9@gmail.com>
Co-authored-by: Maykon Pacheco <38124066+maykonpacheco@users.noreply.github.com>
Co-authored-by: Maxime Desroches <desroches.maxime@gmail.com>
Co-authored-by: Andrew <andrew.tec@pm.me>
Co-authored-by: Cameron Clough <cameronjclough@gmail.com>
Co-authored-by: Jeroen <33349469+jeroenlammersma@users.noreply.github.com>
Co-authored-by: Willem Melching <willem.melching@gmail.com>
Co-authored-by: Joost Wooning <jwooning@gmail.com>
Co-authored-by: redacid95 <jonathanc.olivier@gmail.com>
Co-authored-by: YassineYousfi <yyousfi1@binghamton.edu>
Co-authored-by: Lee Jong Mun <43285072+crwusiz@users.noreply.github.com>
rjsmith1999 pushed a commit to rjsmith1999/openpilot that referenced this pull request Jun 27, 2022
* update names + notes for MPC outputs

"current_curvature" is not the correct description of what the MPC is outputting in it's curvature_ego state.
The MPC is integrating it's free variable, curvature_rate, such that curvature[0] is actually the desired_curvature before any delay.
inversely: the curvature_rate_desired is the desired rate of change to the setpoint and not the actual curvature rate. 

If we were to set the initial curvature = measured curvature in the MPC initiation these names would be correct. 
This was possibly how it was initially set up but the nomenclature here is now confusing.

* more notes

* match

* Clarify commaai#1
rjsmith1999 added a commit to rjsmith1999/openpilot that referenced this pull request Jun 27, 2022
commit d066702
Author: ClockeNessMnstr <locke.dftc@gmail.com>
Date:   Tue Jun 7 12:41:03 2022 -0400

    update DH names + notes for MPC output curvatures (commaai#24701)

    * update names + notes for MPC outputs

    "current_curvature" is not the correct description of what the MPC is outputting in it's curvature_ego state.
    The MPC is integrating it's free variable, curvature_rate, such that curvature[0] is actually the desired_curvature before any delay.
    inversely: the curvature_rate_desired is the desired rate of change to the setpoint and not the actual curvature rate.

    If we were to set the initial curvature = measured curvature in the MPC initiation these names would be correct.
    This was possibly how it was initially set up but the nomenclature here is now confusing.

    * more notes

    * match

    * Clarify commaai#1

commit cb82478
Author: HaraldSchafer <harald.the.engineer@gmail.com>
Date:   Fri May 27 10:21:02 2022 -0700

    Tssp prius torque control (commaai#24669)

    * use llk

    * use steering sensor at low speed stil

    * Try more simple

    * rm prius tune

    * updated ref

commit f1f7a0b
Author: HaraldSchafer <harald.the.engineer@gmail.com>
Date:   Tue May 24 17:52:33 2022 -0700

    Rerevert torque control (commaai#24649)

    * Revert "Revert torque control (commaai#24565)"

    This reverts commit 9f8b037.

    * Move tune out of car specific stuff

    * Update ref commit

commit 041c255
Author: HaraldSchafer <harald.the.engineer@gmail.com>
Date:   Tue May 17 19:00:57 2022 -0700

    Revert torque control (commaai#24565)

    * torque reversal start

    * Fix carmodel tests

    * Update ref

    * update ref

    * Elif is better than if

commit 14f5dab
Author: HaraldSchafer <harald.the.engineer@gmail.com>
Date:   Fri May 20 13:50:47 2022 -0700

    Latcontrol torque: integrator need not be reset (commaai#24606)

    slow integrators need not be reset

commit 9221f3f
Author: ClockeNessMnstr <locke.dftc@gmail.com>
Date:   Fri May 13 19:52:20 2022 -0400

    LatControlTorque: clean up class variable (commaai#24526)

    * move to super

    * no class variable

    * there's CP

    * whitespace

    * drop CI from latcontrol super

    * Revert "drop CI from latcontrol super"

    This reverts commit 9218273.

    Co-authored-by: Shane Smiskol <shane@smiskol.com>

commit efb4dbd
Author: HaraldSchafer <harald.the.engineer@gmail.com>
Date:   Fri May 6 22:17:21 2022 -0700

    Latcontrol torque: fix integrator induced ping pong (commaai#24458)

    * Latcontrol torque: fix integrator induced ping pong

    * Reset on disengage since unwind resets anywayh

    * Might be overkill

    * rm whitespace

    * update ref

commit d105ae2
Author: HaraldSchafer <harald.the.engineer@gmail.com>
Date:   Mon May 2 14:42:18 2022 -0700

    Latcontrol torque: update tuning (commaai#24357)

    * Little more chill

    * Update ref

    * Update refs

commit 0ce2b58
Author: Shane Smiskol <shane@smiskol.com>
Date:   Thu Apr 28 00:42:52 2022 -0700

    Clean up controllers (commaai#24340)

    * clean up lat controllers

    * pass CP once

    * sort

commit 47b162c
Author: Shane Smiskol <shane@smiskol.com>
Date:   Wed Apr 27 01:18:05 2022 -0700

    LatControlTorque: fix deadzone and missing steer saturated warning (commaai#24294)

    * fix steer saturated alert and deadzone

    * fix and formatting

    * update refs

    * update refs

    * not needed

    * remove

commit 51a7ef2
Author: HaraldSchafer <harald.the.engineer@gmail.com>
Date:   Wed Apr 20 11:26:24 2022 -0700

    Latcontrol torque: max_torque rename (commaai#24265)

commit 33e00f6
Author: HaraldSchafer <harald.the.engineer@gmail.com>
Date:   Tue Apr 19 19:34:31 2022 -0700

    Lateral torque-based control with roll on TSS2 corolla and TSSP rav4 (commaai#24260)

    * Initial commit

    * Fix bugs

    * Need more torque rate

    * Cleanup cray cray control

    * Write nicely

    * Chiiil

    * Not relevant for cray cray control

    * Do some logging

    * Seems like it has more torque than I thought

    * Bit more feedforward

    * Tune change

    * Retune

    * Retune

    * Little more chill

    * Add coroll

    * Add corolla

    * Give craycray a good name

    * Update to proper logging

    * D to the PI

    * Should be in radians

    * Add d

    * Start oscillations

    * Add D term

    * Only change torque rate limits for new tune

    * Add d logging

    * Should be enough

    * Wrong sign in D

    * Downtune a little

    * Needed to prevent faults

    * Add lqr rav4 to tune

    * Try derivative again

    * Data based retune

    * Data based retune

    * add friction compensation

    * Doesnt need too much P with friction comp

    * remove lqr

    * Remove kd

    * Fix tests

    * fix tests

    * Too much error

    * Get roll induced error under 1cm/deg

    * Too much jitter

    * Do roll comp

    * Add ki

    * Final update

    * Update refs

    * Cleanup latcontrol_torque a little more
spektor56 pushed a commit to spektor56/ghostpilot that referenced this pull request Jun 30, 2022
* update names + notes for MPC outputs

"current_curvature" is not the correct description of what the MPC is outputting in it's curvature_ego state.
The MPC is integrating it's free variable, curvature_rate, such that curvature[0] is actually the desired_curvature before any delay.
inversely: the curvature_rate_desired is the desired rate of change to the setpoint and not the actual curvature rate. 

If we were to set the initial curvature = measured curvature in the MPC initiation these names would be correct. 
This was possibly how it was initially set up but the nomenclature here is now confusing.

* more notes

* match

* Clarify #1
ntegan1 pushed a commit to ntegan1/apilot that referenced this pull request Jul 12, 2022
* update names + notes for MPC outputs

"current_curvature" is not the correct description of what the MPC is outputting in it's curvature_ego state.
The MPC is integrating it's free variable, curvature_rate, such that curvature[0] is actually the desired_curvature before any delay.
inversely: the curvature_rate_desired is the desired rate of change to the setpoint and not the actual curvature rate.

If we were to set the initial curvature = measured curvature in the MPC initiation these names would be correct.
This was possibly how it was initially set up but the nomenclature here is now confusing.

* more notes

* match

* Clarify #1
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants