mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-06-21 22:12:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

dragonpilot 2023-04-13T02:41:11 for EON/C2

Browse Source 
version: dragonpilot v0.9.2 beta for EON/C2
date: 2023-04-13T02:41:11
dp-dev(priv2) master commit: 4526e2dbd703c5d3e958825e64d5912ae452ab30
This commit is contained in:
Dragonpilot Team
2023-04-13 02:27:39 +00:00
parent ca1186a7bf
commit cf18a8fb09
51 changed files with 5596 additions and 2507 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CHANGELOGS_c2.md
+4 -1
View File
@@ -18,7 +18,10 @@ dragonpilot [Latest] - EON/C2 Release
* TWEAK: DM reverted to 0.8.13 model and algorithm.
* TWEAK: E2E Initial speed matches ACC initial speed. (105 -> 40)
* FIXED: Fixed Reverse message not showing issue.
* FIXED: Honda ALKA not working. @loveloveses
* FIXED: Honda - ALKA not working. @loveloveses
* FIXED: Display Mode.
* FIXED: Toyota - Speed Override.
* FIXED: Re-include CJK system font for EON/C2.
dragonpilot 2023.02.08 - EON/C2 Release
========================
cereal/visionipc/visionipc_pyx.so
BIN
View File
Binary file not shown.
common/params_pyx.so
BIN
View File
Binary file not shown.
common/version.h
+1 -1
View File
@@ -1 +1 @@
#define COMMA_VERSION "2023.04.01"
#define COMMA_VERSION "2023.04.13"
launch_chffrplus.sh
+14
View File
@@ -9,6 +9,20 @@ source "$BASEDIR/launch_env.sh"
DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
function two_init {
if [ -f /EON ]; then
if [ ! -f /system/fonts/NotoSansCJKtc-Regular.otf ]; then
mount -o remount,rw /system
rm -fr /system/fonts/NotoSansTC*.otf
rm -fr /system/fonts/NotoSansSC*.otf
rm -fr /system/fonts/NotoSansKR*.otf
rm -fr /system/fonts/NotoSansJP*.otf
cp -rf /data/openpilot/selfdrive/assets/fonts/NotoSansCJKtc-* /system/fonts/
cp -rf /data/openpilot/selfdrive/assets/fonts/fonts.xml /system/etc/fonts.xml
chmod 644 /system/etc/fonts.xml
chmod 644 /system/fonts/NotoSansCJKtc-*
mount -o remount,r /system
fi
fi
dt=$(date +%s)
opendbc/can/libdbc.so
BIN
View File
Binary file not shown.
panda/board/obj/bootstub.panda.bin
BIN
View File
Binary file not shown.
panda/board/obj/panda.bin.signed
BIN
View File
Binary file not shown.
rednose/helpers/ekf_sym_pyx.so
BIN
View File
Binary file not shown.
selfdrive/assets/fonts/fonts.xml
+372
View File
@@ -0,0 +1,372 @@
<?xml version="1.0" encoding="utf-8"?>
<!--
NOTE: this is the newer (L) version of the system font configuration,
supporting richer weight selection. Some apps will expect the older
version, so please keep system_fonts.xml and fallback_fonts.xml in sync
with any changes, even though framework will only read this file.
All fonts withohut names are added to the default list. Fonts are chosen
based on a match: full BCP-47 language tag including script, then just
language, and finally order (the first font containing the glyph).
Order of appearance is also the tiebreaker for weight matching. This is
the reason why the 900 weights of Roboto precede the 700 weights - we
prefer the former when an 800 weight is requested. Since bold spans
effectively add 300 to the weight, this ensures that 900 is the bold
paired with the 500 weight, ensuring adequate contrast.
-->
<familyset version="22">
<!-- first font is default -->
<family name="sans-serif">
<font weight="100" style="normal">Roboto-Thin.ttf</font>
<font weight="100" style="italic">Roboto-ThinItalic.ttf</font>
<font weight="300" style="normal">Roboto-Light.ttf</font>
<font weight="300" style="italic">Roboto-LightItalic.ttf</font>
<font weight="400" style="normal">Roboto-Regular.ttf</font>
<font weight="400" style="italic">Roboto-Italic.ttf</font>
<font weight="500" style="normal">Roboto-Medium.ttf</font>
<font weight="500" style="italic">Roboto-MediumItalic.ttf</font>
<font weight="900" style="normal">Roboto-Black.ttf</font>
<font weight="900" style="italic">Roboto-BlackItalic.ttf</font>
<font weight="700" style="normal">Roboto-Bold.ttf</font>
<font weight="700" style="italic">Roboto-BoldItalic.ttf</font>
</family>
<!-- Note that aliases must come after the fonts they reference. -->
<alias name="sans-serif-thin" to="sans-serif" weight="100" />
<alias name="sans-serif-light" to="sans-serif" weight="300" />
<alias name="sans-serif-medium" to="sans-serif" weight="500" />
<alias name="sans-serif-black" to="sans-serif" weight="900" />
<alias name="arial" to="sans-serif" />
<alias name="helvetica" to="sans-serif" />
<alias name="tahoma" to="sans-serif" />
<alias name="verdana" to="sans-serif" />
<family name="sans-serif-condensed">
<font weight="300" style="normal">RobotoCondensed-Light.ttf</font>
<font weight="300" style="italic">RobotoCondensed-LightItalic.ttf</font>
<font weight="400" style="normal">RobotoCondensed-Regular.ttf</font>
<font weight="400" style="italic">RobotoCondensed-Italic.ttf</font>
<font weight="700" style="normal">RobotoCondensed-Bold.ttf</font>
<font weight="700" style="italic">RobotoCondensed-BoldItalic.ttf</font>
</family>
<alias name="sans-serif-condensed-light" to="sans-serif-condensed" weight="300" />
<family name="serif">
<font weight="400" style="normal">NotoSerif-Regular.ttf</font>
<font weight="700" style="normal">NotoSerif-Bold.ttf</font>
<font weight="400" style="italic">NotoSerif-Italic.ttf</font>
<font weight="700" style="italic">NotoSerif-BoldItalic.ttf</font>
</family>
<alias name="times" to="serif" />
<alias name="times new roman" to="serif" />
<alias name="palatino" to="serif" />
<alias name="georgia" to="serif" />
<alias name="baskerville" to="serif" />
<alias name="goudy" to="serif" />
<alias name="fantasy" to="serif" />
<alias name="ITC Stone Serif" to="serif" />
<family name="monospace">
<font weight="400" style="normal">DroidSansMono.ttf</font>
</family>
<alias name="sans-serif-monospace" to="monospace" />
<alias name="monaco" to="monospace" />
<family name="serif-monospace">
<font weight="400" style="normal">CutiveMono.ttf</font>
</family>
<alias name="courier" to="serif-monospace" />
<alias name="courier new" to="serif-monospace" />
<family name="casual">
<font weight="400" style="normal">ComingSoon.ttf</font>
</family>
<family name="cursive">
<font weight="400" style="normal">DancingScript-Regular.ttf</font>
<font weight="700" style="normal">DancingScript-Bold.ttf</font>
</family>
<family name="sans-serif-smallcaps">
<font weight="400" style="normal">CarroisGothicSC-Regular.ttf</font>
</family>
<!-- fallback fonts -->
<family variant="elegant">
<font weight="400" style="normal">NotoNaskhArabic-Regular.ttf</font>
<font weight="700" style="normal">NotoNaskhArabic-Bold.ttf</font>
</family>
<family variant="compact">
<font weight="400" style="normal">NotoNaskhArabicUI-Regular.ttf</font>
<font weight="700" style="normal">NotoNaskhArabicUI-Bold.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansEthiopic-Regular.ttf</font>
<font weight="700" style="normal">NotoSansEthiopic-Bold.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansHebrew-Regular.ttf</font>
<font weight="700" style="normal">NotoSansHebrew-Bold.ttf</font>
</family>
<family variant="elegant">
<font weight="400" style="normal">NotoSansThai-Regular.ttf</font>
<font weight="700" style="normal">NotoSansThai-Bold.ttf</font>
</family>
<family variant="compact">
<font weight="400" style="normal">NotoSansThaiUI-Regular.ttf</font>
<font weight="700" style="normal">NotoSansThaiUI-Bold.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansArmenian-Regular.ttf</font>
<font weight="700" style="normal">NotoSansArmenian-Bold.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansGeorgian-Regular.ttf</font>
<font weight="700" style="normal">NotoSansGeorgian-Bold.ttf</font>
</family>
<family variant="elegant">
<font weight="400" style="normal">NotoSansDevanagari-Regular.ttf</font>
<font weight="700" style="normal">NotoSansDevanagari-Bold.ttf</font>
</family>
<family variant="compact">
<font weight="400" style="normal">NotoSansDevanagariUI-Regular.ttf</font>
<font weight="700" style="normal">NotoSansDevanagariUI-Bold.ttf</font>
</family>
<!-- Gujarati should come after Devanagari -->
<family variant="elegant">
<font weight="400" style="normal">NotoSansGujarati-Regular.ttf</font>
<font weight="700" style="normal">NotoSansGujarati-Bold.ttf</font>
</family>
<family variant="compact">
<font weight="400" style="normal">NotoSansGujaratiUI-Regular.ttf</font>
<font weight="700" style="normal">NotoSansGujaratiUI-Bold.ttf</font>
</family>
<!-- Gurmukhi should come after Devanagari -->
<family variant="elegant">
<font weight="400" style="normal">NotoSansGurmukhi-Regular.ttf</font>
<font weight="700" style="normal">NotoSansGurmukhi-Bold.ttf</font>
</family>
<family variant="compact">
<font weight="400" style="normal">NotoSansGurmukhiUI-Regular.ttf</font>
<font weight="700" style="normal">NotoSansGurmukhiUI-Bold.ttf</font>
</family>
<family variant="elegant">
<font weight="400" style="normal">NotoSansTamil-Regular.ttf</font>
<font weight="700" style="normal">NotoSansTamil-Bold.ttf</font>
</family>
<family variant="compact">
<font weight="400" style="normal">NotoSansTamilUI-Regular.ttf</font>
<font weight="700" style="normal">NotoSansTamilUI-Bold.ttf</font>
</family>
<family variant="elegant">
<font weight="400" style="normal">NotoSansMalayalam-Regular.ttf</font>
<font weight="700" style="normal">NotoSansMalayalam-Bold.ttf</font>
</family>
<family variant="compact">
<font weight="400" style="normal">NotoSansMalayalamUI-Regular.ttf</font>
<font weight="700" style="normal">NotoSansMalayalamUI-Bold.ttf</font>
</family>
<family variant="elegant">
<font weight="400" style="normal">NotoSansBengali-Regular.ttf</font>
<font weight="700" style="normal">NotoSansBengali-Bold.ttf</font>
</family>
<family variant="compact">
<font weight="400" style="normal">NotoSansBengaliUI-Regular.ttf</font>
<font weight="700" style="normal">NotoSansBengaliUI-Bold.ttf</font>
</family>
<family variant="elegant">
<font weight="400" style="normal">NotoSansTelugu-Regular.ttf</font>
<font weight="700" style="normal">NotoSansTelugu-Bold.ttf</font>
</family>
<family variant="compact">
<font weight="400" style="normal">NotoSansTeluguUI-Regular.ttf</font>
<font weight="700" style="normal">NotoSansTeluguUI-Bold.ttf</font>
</family>
<family variant="elegant">
<font weight="400" style="normal">NotoSansKannada-Regular.ttf</font>
<font weight="700" style="normal">NotoSansKannada-Bold.ttf</font>
</family>
<family variant="compact">
<font weight="400" style="normal">NotoSansKannadaUI-Regular.ttf</font>
<font weight="700" style="normal">NotoSansKannadaUI-Bold.ttf</font>
</family>
<family variant="elegant">
<font weight="400" style="normal">NotoSansOriya-Regular.ttf</font>
<font weight="700" style="normal">NotoSansOriya-Bold.ttf</font>
</family>
<family variant="compact">
<font weight="400" style="normal">NotoSansOriyaUI-Regular.ttf</font>
<font weight="700" style="normal">NotoSansOriyaUI-Bold.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansSinhala-Regular.ttf</font>
<font weight="700" style="normal">NotoSansSinhala-Bold.ttf</font>
</family>
<family variant="elegant">
<font weight="400" style="normal">NotoSansKhmer-Regular.ttf</font>
<font weight="700" style="normal">NotoSansKhmer-Bold.ttf</font>
</family>
<family variant="compact">
<font weight="400" style="normal">NotoSansKhmerUI-Regular.ttf</font>
<font weight="700" style="normal">NotoSansKhmerUI-Bold.ttf</font>
</family>
<family variant="elegant">
<font weight="400" style="normal">NotoSansLao-Regular.ttf</font>
<font weight="700" style="normal">NotoSansLao-Bold.ttf</font>
</family>
<family variant="compact">
<font weight="400" style="normal">NotoSansLaoUI-Regular.ttf</font>
<font weight="700" style="normal">NotoSansLaoUI-Bold.ttf</font>
</family>
<family variant="elegant">
<font weight="400" style="normal">NotoSansMyanmar-Regular.ttf</font>
<font weight="700" style="normal">NotoSansMyanmar-Bold.ttf</font>
</family>
<family variant="compact">
<font weight="400" style="normal">NotoSansMyanmarUI-Regular.ttf</font>
<font weight="700" style="normal">NotoSansMyanmarUI-Bold.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansThaana-Regular.ttf</font>
<font weight="700" style="normal">NotoSansThaana-Bold.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansCham-Regular.ttf</font>
<font weight="700" style="normal">NotoSansCham-Bold.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansBalinese-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansBamum-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansBatak-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansBuginese-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansBuhid-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansCanadianAboriginal-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansCherokee-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansCoptic-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansGlagolitic-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansHanunoo-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansJavanese-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansKayahLi-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansLepcha-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansLimbu-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansLisu-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansMandaic-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansMeeteiMayek-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansNewTaiLue-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansNKo-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansOlChiki-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansRejang-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansSaurashtra-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansSundanese-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansSylotiNagri-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansSyriacEstrangela-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansTagbanwa-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansTaiTham-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansTaiViet-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansTibetan-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansTifinagh-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansVai-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansYi-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansSymbols-Regular-Subsetted.ttf</font>
</family>
<family lang="ja">
<font weight="400" style="normal">NotoSansJP-Regular.otf</font>
</family>
<family lang="ko">
<font weight="400" style="normal">NotoSansKR-Regular.otf</font>
</family>
<family>
<font weight="400" style="normal">NanumGothic.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoColorEmoji.ttf</font>
</family>
<family lang="zh-Hans">
<font weight="400" style="normal">NotoSansCJKtc-Regular.otf</font>
<font weight="500" style="normal">NotoSansCJKtc-Medium.otf</font>
<font weight="700" style="normal">NotoSansCJKtc-Bold.otf</font>
</family>
<family lang="zh-Hant">
<font weight="400" style="normal">NotoSansCJKtc-Regular.otf</font>
<font weight="500" style="normal">NotoSansCJKtc-Medium.otf</font>
<font weight="700" style="normal">NotoSansCJKtc-Bold.otf</font>
</family>
<family lang="ja">
<font weight="400" style="normal">MTLmr3m.ttf</font>
</family>
<!--
Tai Le and Mongolian are intentionally kept last, to make sure they don't override
the East Asian punctuation for Chinese.
-->
<family>
<font weight="400" style="normal">NotoSansTaiLe-Regular.ttf</font>
</family>
<family>
<font weight="400" style="normal">NotoSansMongolian-Regular.ttf</font>
</family>
</familyset>
selfdrive/boardd/boardd
BIN
View File
Binary file not shown.
selfdrive/car/toyota/interface.py
+1 -23
View File
@@ -3,24 +3,15 @@ from cereal import car
from common.conversions import Conversions as CV
from panda import Panda
from selfdrive.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \
MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR
MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR
from selfdrive.car import STD_CARGO_KG, scale_tire_stiffness, get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase
from common.params import Params
EventName = car.CarEvent.EventName
CRUISE_OVERRIDE_SPEED_MIN = 5 * CV.KPH_TO_MS
class CarInterface(CarInterfaceBase):
def __init__(self, CP, CarController, CarState):
super().__init__(CP, CarController, CarState)
self.dp_cruise_speed = 0. # km/h
self.dp_override_speed_last = 0. # km/h
self.dp_override_speed = 0. # m/s
@staticmethod
def get_pid_accel_limits(CP, current_speed, cruise_speed):
return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX
@@ -278,19 +269,6 @@ class CarInterface(CarInterfaceBase):
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
# low speed re-write
if self.dragonconf.dpToyotaCruiseOverride:
if self.dragonconf.dpToyotaCruiseOverrideSpeed != self.dp_override_speed_last:
self.dp_override_speed = self.dragonconf.dpToyotaCruiseOverrideSpeed * CV.KPH_TO_MS
self.dp_override_speed_last = self.dragonconf.dpToyotaCruiseOverrideSpeed
if self.CP.openpilotLongitudinalControl and ret.cruiseState.speed <= self.dp_override_speed:
if self.dp_cruise_speed == 0.:
self.dp_cruise_speed = self.dp_cruise_speed = max(CRUISE_OVERRIDE_SPEED_MIN, ret.vEgo)
else:
ret.cruiseState.speed = self.dp_cruise_speed
else:
self.dp_cruise_speed = 0.
# events
events = self.create_common_events(ret)
selfdrive/controls/controlsd.py
+4 -1
View File
@@ -563,7 +563,10 @@ class Controls:
def state_transition(self, CS):
"""Compute conditional state transitions and execute actions on state transitions"""
self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric)
# dp - toyota speed override here
# dp - @todo may apply to other makes in the future?
dp_override_speed = self.sm['dragonConf'].dpToyotaCruiseOverrideSpeed if self.sm['dragonConf'].dpToyotaCruiseOverride else False
self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric, dp_override_speed)
# decrement the soft disable timer at every step, as it's reset on
# entrance in SOFT_DISABLING state
selfdrive/controls/lib/drive_helpers.py
+25 -7
View File
@@ -62,12 +62,14 @@ class VCruiseHelper:
self.v_cruise_kph_last = 0
self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0}
self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers}
self.dp_override_v_cruise_kph = V_CRUISE_UNSET
self.dp_override_cruise_speed_last = V_CRUISE_UNSET
@property
def v_cruise_initialized(self):
return self.v_cruise_kph != V_CRUISE_UNSET
def update_v_cruise(self, CS, enabled, is_metric):
def update_v_cruise(self, CS, enabled, is_metric, dp_override_speed):
self.v_cruise_kph_last = self.v_cruise_kph
if CS.cruiseState.available:
@@ -77,11 +79,28 @@ class VCruiseHelper:
self.v_cruise_cluster_kph = self.v_cruise_kph
self.update_button_timers(CS, enabled)
else:
self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH
else:
self.v_cruise_kph = V_CRUISE_UNSET
self.v_cruise_cluster_kph = V_CRUISE_UNSET
if dp_override_speed:
# when set speed changed, reset override_speed to unset
if CS.cruiseState.speed != self.dp_override_cruise_speed_last:
self.dp_override_v_cruise_kph = V_CRUISE_UNSET
# when override_speed is unset, use current speed as set speed
if CS.cruiseState.speed * CV.MS_TO_KPH < dp_override_speed:
self.dp_override_v_cruise_kph = clip(CS.vEgo * CV.MS_TO_KPH, V_CRUISE_MIN, V_CRUISE_MAX)
# when we have an override_speed, use it
if self.dp_override_v_cruise_kph != V_CRUISE_UNSET:
self.v_cruise_kph = self.dp_override_v_cruise_kph
self.v_cruise_cluster_kph = self.dp_override_v_cruise_kph
else:
self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH
self.dp_override_cruise_speed_last = CS.cruiseState.speed
else:
self.dp_override_v_cruise_kph = V_CRUISE_UNSET
self.v_cruise_kph = V_CRUISE_UNSET
self.v_cruise_cluster_kph = V_CRUISE_UNSET
def _update_v_cruise_non_pcm(self, CS, enabled, is_metric):
# handle button presses. TODO: this should be in state_control, but a decelCruise press
@@ -158,7 +177,6 @@ class VCruiseHelper:
self.v_cruise_cluster_kph = self.v_cruise_kph
def apply_deadzone(error, deadzone):
if error > deadzone:
error -= deadzone
selfdrive/controls/lib/lateral_planner.py
+3 -4
View File
@@ -35,7 +35,6 @@ class LateralPlanner:
self.LP = LanePlanner(Params().get_bool('WideCameraOnly'))
self.dp_lanelines_enable = False
self.dp_lanelines_active = False
self.dp_lanelines_active_tmp = False
self.dp_camera_offset = 4 if TICI else -6
self.dp_path_offset = 4 if TICI else 0
@@ -84,6 +83,7 @@ class LateralPlanner:
# dp - when laneline mode enabled, we use old logic (including lane changing)
d_path_xyz = self.lanelines_mode(md, sm['carState'], sm['carControl'].latActive, sm['dragonConf'])
else:
self.dp_lanelines_active = False
# dp -- tab spacing begin (stock logic) --
# Lane change logic
desire_state = md.meta.desireState
@@ -152,7 +152,7 @@ class LateralPlanner:
lateralPlan.solverExecutionTime = self.lat_mpc.solve_time
lateralPlan.desire = self.DH.desire
lateralPlan.useLaneLines = self.dp_lanelines_enable and self.dp_lanelines_active
lateralPlan.useLaneLines = self.dp_lanelines_active
lateralPlan.laneChangeState = self.DH.lane_change_state
lateralPlan.laneChangeDirection = self.DH.lane_change_direction
@@ -177,8 +177,7 @@ class LateralPlanner:
self.LP.rll_prob *= self.DH.lane_change_ll_prob
# dynamic laneline/laneless logic
self.dp_lanelines_active_tmp = get_lane_laneless_mode(self.LP.lll_prob, self.LP.rll_prob, self.dp_lanelines_active_tmp)
self.dp_lanelines_active = self.dp_lanelines_active_tmp
self.dp_lanelines_active = get_lane_laneless_mode(self.LP.lll_prob, self.LP.rll_prob, self.dp_lanelines_active)
# Calculate final driving path and set MPC costs
if self.dp_lanelines_active:
selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/acados_ocp_lat.json
+450
View File
@@ -0,0 +1,450 @@
{
"acados_include_path": "/data/openpilot/third_party/acados/include/acados/include",
"acados_lib_path": "/data/openpilot/third_party/acados/include/acados/lib",
"code_export_directory": "/data/openpilot/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code",
"constraints": {
"C": [],
"C_e": [],
"D": [],
"constr_type": "BGH",
"constr_type_e": "BGH",
"idxbu": [],
"idxbx": [
2,
3
],
"idxbx_0": [
0,
1,
2,
3
],
"idxbx_e": [],
"idxbxe_0": [
0,
1,
2,
3
],
"idxsbu": [],
"idxsbx": [],
"idxsbx_e": [],
"idxsg": [],
"idxsg_e": [],
"idxsh": [],
"idxsh_e": [],
"idxsphi": [],
"idxsphi_e": [],
"lbu": [],
"lbx": [
-1.5707963267948966,
-0.8726646259971648
],
"lbx_0": [
0.0,
0.0,
0.0,
0.0
],
"lbx_e": [],
"lg": [],
"lg_e": [],
"lh": [],
"lh_e": [],
"lphi": [],
"lphi_e": [],
"lsbu": [],
"lsbx": [],
"lsbx_e": [],
"lsg": [],
"lsg_e": [],
"lsh": [],
"lsh_e": [],
"lsphi": [],
"lsphi_e": [],
"ubu": [],
"ubx": [
1.5707963267948966,
0.8726646259971648
],
"ubx_0": [
0.0,
0.0,
0.0,
0.0
],
"ubx_e": [],
"ug": [],
"ug_e": [],
"uh": [],
"uh_e": [],
"uphi": [],
"uphi_e": [],
"usbu": [],
"usbx": [],
"usbx_e": [],
"usg": [],
"usg_e": [],
"ush": [],
"ush_e": [],
"usphi": [],
"usphi_e": []
},
"cost": {
"Vu": [
[
0.0
],
[
0.0
],
[
0.0
]
],
"Vu_0": [
[
0.0
],
[
0.0
],
[
0.0
]
],
"Vx": [
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
]
],
"Vx_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
]
],
"Vx_e": [
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
]
],
"Vz": [],
"Vz_0": [],
"W": [
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
]
],
"W_0": [
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
]
],
"W_e": [
[
0.0,
0.0
],
[
0.0,
0.0
]
],
"Zl": [],
"Zl_e": [],
"Zu": [],
"Zu_e": [],
"cost_ext_fun_type": "casadi",
"cost_ext_fun_type_0": "casadi",
"cost_ext_fun_type_e": "casadi",
"cost_type": "NONLINEAR_LS",
"cost_type_0": "NONLINEAR_LS",
"cost_type_e": "NONLINEAR_LS",
"yref": [
0.0,
0.0,
0.0
],
"yref_0": [
0.0,
0.0,
0.0
],
"yref_e": [
0.0,
0.0
],
"zl": [],
"zl_e": [],
"zu": [],
"zu_e": []
},
"cython_include_dirs": "/data/data/com.termux/files/usr/lib/python3.8/site-packages/numpy/core/include",
"dims": {
"N": 16,
"nbu": 0,
"nbx": 2,
"nbx_0": 4,
"nbx_e": 0,
"nbxe_0": 4,
"ng": 0,
"ng_e": 0,
"nh": 0,
"nh_e": 0,
"np": 2,
"nphi": 0,
"nphi_e": 0,
"nr": 0,
"nr_e": 0,
"ns": 0,
"ns_e": 0,
"nsbu": 0,
"nsbx": 0,
"nsbx_e": 0,
"nsg": 0,
"nsg_e": 0,
"nsh": 0,
"nsh_e": 0,
"nsphi": 0,
"nsphi_e": 0,
"nu": 1,
"nx": 4,
"ny": 3,
"ny_0": 3,
"ny_e": 2,
"nz": 0
},
"model": {
"dyn_disc_fun": null,
"dyn_disc_fun_jac": null,
"dyn_disc_fun_jac_hess": null,
"dyn_ext_fun_type": "casadi",
"dyn_source_discrete": null,
"gnsf": {
"nontrivial_f_LO": 1,
"purely_linear": 0
},
"name": "lat"
},
"parameter_values": [
0.0,
0.0
],
"problem_class": "OCP",
"simulink_opts": {
"inputs": {
"cost_W": 0,
"cost_W_0": 0,
"cost_W_e": 0,
"lbu": 1,
"lbx": 1,
"lbx_0": 1,
"lbx_e": 1,
"lg": 1,
"lh": 1,
"parameter_traj": 1,
"reset_solver": 0,
"u_init": 0,
"ubu": 1,
"ubx": 1,
"ubx_0": 1,
"ubx_e": 1,
"ug": 1,
"uh": 1,
"x_init": 0,
"y_ref": 1,
"y_ref_0": 1,
"y_ref_e": 1
},
"outputs": {
"CPU_time": 1,
"CPU_time_lin": 0,
"CPU_time_qp": 0,
"CPU_time_sim": 0,
"KKT_residual": 1,
"solver_status": 1,
"sqp_iter": 1,
"u0": 1,
"utraj": 0,
"x1": 1,
"xtraj": 0
},
"samplingtime": "t0"
},
"solver_options": {
"Tsim": 0.009765625,
"alpha_min": 0.05,
"alpha_reduction": 0.7,
"collocation_type": "GAUSS_LEGENDRE",
"eps_sufficient_descent": 0.0001,
"exact_hess_constr": 1,
"exact_hess_cost": 1,
"exact_hess_dyn": 1,
"ext_cost_num_hess": 0,
"full_step_dual": 0,
"globalization": "FIXED_STEP",
"globalization_use_SOC": 0,
"hessian_approx": "GAUSS_NEWTON",
"hpipm_mode": "BALANCE",
"initialize_t_slacks": 0,
"integrator_type": "ERK",
"levenberg_marquardt": 0.0,
"line_search_use_sufficient_descent": 0,
"model_external_shared_lib_dir": null,
"model_external_shared_lib_name": null,
"nlp_solver_max_iter": 100,
"nlp_solver_step_length": 1.0,
"nlp_solver_tol_comp": 1e-06,
"nlp_solver_tol_eq": 1e-06,
"nlp_solver_tol_ineq": 1e-06,
"nlp_solver_tol_stat": 1e-06,
"nlp_solver_type": "SQP_RTI",
"print_level": 0,
"qp_solver": "PARTIAL_CONDENSING_HPIPM",
"qp_solver_cond_N": 1,
"qp_solver_iter_max": 1,
"qp_solver_tol_comp": null,
"qp_solver_tol_eq": null,
"qp_solver_tol_ineq": null,
"qp_solver_tol_stat": null,
"qp_solver_warm_start": 0,
"regularize_method": null,
"sim_method_jac_reuse": [
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0
],
"sim_method_newton_iter": 3,
"sim_method_num_stages": [
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4
],
"sim_method_num_steps": [
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1
],
"tf": 2.5,
"time_steps": [
0.009765625,
0.029296875,
0.048828125,
0.068359375,
0.087890625,
0.107421875,
0.126953125,
0.146484375,
0.166015625,
0.185546875,
0.205078125,
0.224609375,
0.244140625,
0.263671875,
0.283203125,
0.302734375
]
}
}
selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c
+278
View File
@@ -0,0 +1,278 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
// standard
#include <stdio.h>
#include <stdlib.h>
// acados
#include "acados_c/external_function_interface.h"
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
#include "acados/sim/sim_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/print.h"
// example specific
#include "lat_model/lat_model.h"
#include "acados_sim_solver_lat.h"
// ** solver data **
sim_solver_capsule * lat_acados_sim_solver_create_capsule()
{
void* capsule_mem = malloc(sizeof(sim_solver_capsule));
sim_solver_capsule *capsule = (sim_solver_capsule *) capsule_mem;
return capsule;
}
int lat_acados_sim_solver_free_capsule(sim_solver_capsule * capsule)
{
free(capsule);
return 0;
}
int lat_acados_sim_create(sim_solver_capsule * capsule)
{
// initialize
const int nx = LAT_NX;
const int nu = LAT_NU;
const int nz = LAT_NZ;
const int np = LAT_NP;
bool tmp_bool;
double Tsim = 0.009765625;
// explicit ode
capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
capsule->sim_forw_vde_casadi->casadi_fun = &lat_expl_vde_forw;
capsule->sim_forw_vde_casadi->casadi_n_in = &lat_expl_vde_forw_n_in;
capsule->sim_forw_vde_casadi->casadi_n_out = &lat_expl_vde_forw_n_out;
capsule->sim_forw_vde_casadi->casadi_sparsity_in = &lat_expl_vde_forw_sparsity_in;
capsule->sim_forw_vde_casadi->casadi_sparsity_out = &lat_expl_vde_forw_sparsity_out;
capsule->sim_forw_vde_casadi->casadi_work = &lat_expl_vde_forw_work;
external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np);
capsule->sim_expl_ode_fun_casadi->casadi_fun = &lat_expl_ode_fun;
capsule->sim_expl_ode_fun_casadi->casadi_n_in = &lat_expl_ode_fun_n_in;
capsule->sim_expl_ode_fun_casadi->casadi_n_out = &lat_expl_ode_fun_n_out;
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &lat_expl_ode_fun_sparsity_in;
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &lat_expl_ode_fun_sparsity_out;
capsule->sim_expl_ode_fun_casadi->casadi_work = &lat_expl_ode_fun_work;
external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np);
// sim plan & config
sim_solver_plan_t plan;
plan.sim_solver = ERK;
// create correct config based on plan
sim_config * lat_sim_config = sim_config_create(plan);
capsule->acados_sim_config = lat_sim_config;
// sim dims
void *lat_sim_dims = sim_dims_create(lat_sim_config);
capsule->acados_sim_dims = lat_sim_dims;
sim_dims_set(lat_sim_config, lat_sim_dims, "nx", &nx);
sim_dims_set(lat_sim_config, lat_sim_dims, "nu", &nu);
sim_dims_set(lat_sim_config, lat_sim_dims, "nz", &nz);
// sim opts
sim_opts *lat_sim_opts = sim_opts_create(lat_sim_config, lat_sim_dims);
capsule->acados_sim_opts = lat_sim_opts;
int tmp_int = 3;
sim_opts_set(lat_sim_config, lat_sim_opts, "newton_iter", &tmp_int);
sim_collocation_type collocation_type = GAUSS_LEGENDRE;
sim_opts_set(lat_sim_config, lat_sim_opts, "collocation_type", &collocation_type);
tmp_int = 4;
sim_opts_set(lat_sim_config, lat_sim_opts, "num_stages", &tmp_int);
tmp_int = 1;
sim_opts_set(lat_sim_config, lat_sim_opts, "num_steps", &tmp_int);
tmp_bool = 0;
sim_opts_set(lat_sim_config, lat_sim_opts, "jac_reuse", &tmp_bool);
// sim in / out
sim_in *lat_sim_in = sim_in_create(lat_sim_config, lat_sim_dims);
capsule->acados_sim_in = lat_sim_in;
sim_out *lat_sim_out = sim_out_create(lat_sim_config, lat_sim_dims);
capsule->acados_sim_out = lat_sim_out;
sim_in_set(lat_sim_config, lat_sim_dims,
lat_sim_in, "T", &Tsim);
// model functions
lat_sim_config->model_set(lat_sim_in->model,
"expl_vde_for", capsule->sim_forw_vde_casadi);
lat_sim_config->model_set(lat_sim_in->model,
"expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
// sim solver
sim_solver *lat_sim_solver = sim_solver_create(lat_sim_config,
lat_sim_dims, lat_sim_opts);
capsule->acados_sim_solver = lat_sim_solver;
/* initialize parameter values */
double* p = calloc(np, sizeof(double));
lat_acados_sim_update_params(capsule, p, np);
free(p);
/* initialize input */
// x
double x0[4];
for (int ii = 0; ii < 4; ii++)
x0[ii] = 0.0;
sim_in_set(lat_sim_config, lat_sim_dims,
lat_sim_in, "x", x0);
// u
double u0[1];
for (int ii = 0; ii < 1; ii++)
u0[ii] = 0.0;
sim_in_set(lat_sim_config, lat_sim_dims,
lat_sim_in, "u", u0);
// S_forw
double S_forw[20];
for (int ii = 0; ii < 20; ii++)
S_forw[ii] = 0.0;
for (int ii = 0; ii < 4; ii++)
S_forw[ii + ii * 4 ] = 1.0;
sim_in_set(lat_sim_config, lat_sim_dims,
lat_sim_in, "S_forw", S_forw);
int status = sim_precompute(lat_sim_solver, lat_sim_in, lat_sim_out);
return status;
}
int lat_acados_sim_solve(sim_solver_capsule *capsule)
{
// integrate dynamics using acados sim_solver
int status = sim_solve(capsule->acados_sim_solver,
capsule->acados_sim_in, capsule->acados_sim_out);
if (status != 0)
printf("error in lat_acados_sim_solve()! Exiting.\n");
return status;
}
int lat_acados_sim_free(sim_solver_capsule *capsule)
{
// free memory
sim_solver_destroy(capsule->acados_sim_solver);
sim_in_destroy(capsule->acados_sim_in);
sim_out_destroy(capsule->acados_sim_out);
sim_opts_destroy(capsule->acados_sim_opts);
sim_dims_destroy(capsule->acados_sim_dims);
sim_config_destroy(capsule->acados_sim_config);
// free external function
external_function_param_casadi_free(capsule->sim_forw_vde_casadi);
external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi);
return 0;
}
int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np)
{
int status = 0;
int casadi_np = LAT_NP;
if (casadi_np != np) {
printf("lat_acados_sim_update_params: trying to set %i parameters for external functions."
" External function has %i parameters. Exiting.\n", np, casadi_np);
exit(1);
}
capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
return status;
}
/* getters pointers to C objects*/
sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule)
{
return capsule->acados_sim_config;
};
sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule)
{
return capsule->acados_sim_in;
};
sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule)
{
return capsule->acados_sim_out;
};
void * lat_acados_get_sim_dims(sim_solver_capsule *capsule)
{
return capsule->acados_sim_dims;
};
sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule)
{
return capsule->acados_sim_opts;
};
sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule)
{
return capsule->acados_sim_solver;
};
selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h
+103
View File
@@ -0,0 +1,103 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_SIM_lat_H_
#define ACADOS_SIM_lat_H_
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
#define LAT_NX 4
#define LAT_NZ 0
#define LAT_NU 1
#define LAT_NP 2
#ifdef __cplusplus
extern "C" {
#endif
// ** capsule for solver data **
typedef struct sim_solver_capsule
{
// acados objects
sim_in *acados_sim_in;
sim_out *acados_sim_out;
sim_solver *acados_sim_solver;
sim_opts *acados_sim_opts;
sim_config *acados_sim_config;
void *acados_sim_dims;
/* external functions */
// ERK
external_function_param_casadi * sim_forw_vde_casadi;
external_function_param_casadi * sim_expl_ode_fun_casadi;
external_function_param_casadi * sim_expl_ode_hess;
// IRK
external_function_param_casadi * sim_impl_dae_fun;
external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z;
external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z;
external_function_param_casadi * sim_impl_dae_hess;
// GNSF
external_function_param_casadi * sim_gnsf_phi_fun;
external_function_param_casadi * sim_gnsf_phi_fun_jac_y;
external_function_param_casadi * sim_gnsf_phi_jac_y_uhat;
external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z;
external_function_param_casadi * sim_gnsf_get_matrices_fun;
} sim_solver_capsule;
ACADOS_SYMBOL_EXPORT int lat_acados_sim_create(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_sim_solve(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_sim_free(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np);
ACADOS_SYMBOL_EXPORT sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT void * lat_acados_get_sim_dims(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_solver_capsule * lat_acados_sim_solver_create_capsule(void);
ACADOS_SYMBOL_EXPORT int lat_acados_sim_solver_free_capsule(sim_solver_capsule *capsule);
#ifdef __cplusplus
}
#endif
#endif // ACADOS_SIM_lat_H_
selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c
+264
View File
@@ -0,0 +1,264 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#define S_FUNCTION_NAME acados_solver_sfunction_lat
#define S_FUNCTION_LEVEL 2
#define MDL_START
// acados
// #include "acados/utils/print.h"
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
// example specific
#include "lat_model/lat_model.h"
#include "acados_solver_lat.h"
#include "simstruc.h"
#define SAMPLINGTIME 0.009765625
static void mdlInitializeSizes (SimStruct *S)
{
// specify the number of continuous and discrete states
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);// specify the number of input ports
if ( !ssSetNumInputPorts(S, 8) )
return;
// specify the number of output ports
if ( !ssSetNumOutputPorts(S, 6) )
return;
// specify dimension information for the input ports
// lbx_0
ssSetInputPortVectorDimension(S, 0, 4);
// ubx_0
ssSetInputPortVectorDimension(S, 1, 4);
// parameters
ssSetInputPortVectorDimension(S, 2, (16+1) * 2);
// y_ref_0
ssSetInputPortVectorDimension(S, 3, 3);
// y_ref
ssSetInputPortVectorDimension(S, 4, 45);
// y_ref_e
ssSetInputPortVectorDimension(S, 5, 2);
// lbx
ssSetInputPortVectorDimension(S, 6, 30);
// ubx
ssSetInputPortVectorDimension(S, 7, 30);/* specify dimension information for the OUTPUT ports */
ssSetOutputPortVectorDimension(S, 0, 1 );
ssSetOutputPortVectorDimension(S, 1, 1 );
ssSetOutputPortVectorDimension(S, 2, 1 );
ssSetOutputPortVectorDimension(S, 3, 4 ); // state at shooting node 1
ssSetOutputPortVectorDimension(S, 4, 1);
ssSetOutputPortVectorDimension(S, 5, 1 );
// specify the direct feedthrough status
// should be set to 1 for all inputs used in mdlOutputs
ssSetInputPortDirectFeedThrough(S, 0, 1);
ssSetInputPortDirectFeedThrough(S, 1, 1);
ssSetInputPortDirectFeedThrough(S, 2, 1);
ssSetInputPortDirectFeedThrough(S, 3, 1);
ssSetInputPortDirectFeedThrough(S, 4, 1);
ssSetInputPortDirectFeedThrough(S, 5, 1);
ssSetInputPortDirectFeedThrough(S, 6, 1);
ssSetInputPortDirectFeedThrough(S, 7, 1);
// one sample time
ssSetNumSampleTimes(S, 1);
}
#if defined(MATLAB_MEX_FILE)
#define MDL_SET_INPUT_PORT_DIMENSION_INFO
#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
{
if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
return;
}
static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
{
if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
return;
}
#endif /* MATLAB_MEX_FILE */
static void mdlInitializeSampleTimes(SimStruct *S)
{
ssSetSampleTime(S, 0, SAMPLINGTIME);
ssSetOffsetTime(S, 0, 0.0);
}
static void mdlStart(SimStruct *S)
{
lat_solver_capsule *capsule = lat_acados_create_capsule();
lat_acados_create(capsule);
ssSetUserData(S, (void*)capsule);
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
lat_solver_capsule *capsule = ssGetUserData(S);
ocp_nlp_config *nlp_config = lat_acados_get_nlp_config(capsule);
ocp_nlp_dims *nlp_dims = lat_acados_get_nlp_dims(capsule);
ocp_nlp_in *nlp_in = lat_acados_get_nlp_in(capsule);
ocp_nlp_out *nlp_out = lat_acados_get_nlp_out(capsule);
InputRealPtrsType in_sign;
// local buffer
real_t buffer[4];
/* go through inputs */
// lbx_0
in_sign = ssGetInputPortRealSignalPtrs(S, 0);
for (int i = 0; i < 4; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer);
// ubx_0
in_sign = ssGetInputPortRealSignalPtrs(S, 1);
for (int i = 0; i < 4; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer);
// parameters - stage-variant !!!
in_sign = ssGetInputPortRealSignalPtrs(S, 2);
// update value of parameters
for (int ii = 0; ii <= 16; ii++)
{
for (int jj = 0; jj < 2; jj++)
buffer[jj] = (double)(*in_sign[ii*2+jj]);
lat_acados_update_params(capsule, ii, buffer, 2);
}
// y_ref_0
in_sign = ssGetInputPortRealSignalPtrs(S, 3);
for (int i = 0; i < 3; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", (void *) buffer);
// y_ref - for stages 1 to N-1
in_sign = ssGetInputPortRealSignalPtrs(S, 4);
for (int ii = 1; ii < 16; ii++)
{
for (int jj = 0; jj < 3; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*3+jj]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer);
}
// y_ref_e
in_sign = ssGetInputPortRealSignalPtrs(S, 5);
for (int i = 0; i < 2; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 16, "yref", (void *) buffer);
// lbx
in_sign = ssGetInputPortRealSignalPtrs(S, 6);
for (int ii = 1; ii < 16; ii++)
{
for (int jj = 0; jj < 2; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbx", (void *) buffer);
}
// ubx
in_sign = ssGetInputPortRealSignalPtrs(S, 7);
for (int ii = 1; ii < 16; ii++)
{
for (int jj = 0; jj < 2; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubx", (void *) buffer);
}
/* call solver */
int rti_phase = 0;
ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase);
int acados_status = lat_acados_solve(capsule);
/* set outputs */
// assign pointers to output signals
real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin;
int tmp_int;
out_u0 = ssGetOutputPortRealSignal(S, 0);
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0);
out_status = ssGetOutputPortRealSignal(S, 1);
*out_status = (real_t) acados_status;
out_KKT_res = ssGetOutputPortRealSignal(S, 2);
*out_KKT_res = (real_t) nlp_out->inf_norm_res;
out_x1 = ssGetOutputPortRealSignal(S, 3);
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1);
out_cpu_time = ssGetOutputPortRealSignal(S, 4);
// get solution time
ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_tot", (void *) out_cpu_time);
out_sqp_iter = ssGetOutputPortRealSignal(S, 5);
// get sqp iter
ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int);
*out_sqp_iter = (real_t) tmp_int;
}
static void mdlTerminate(SimStruct *S)
{
lat_solver_capsule *capsule = ssGetUserData(S);
lat_acados_free(capsule);
lat_acados_free_capsule(capsule);
}
#ifdef MATLAB_MEX_FILE
#include "simulink.c"
#else
#include "cg_sfun.h"
#endif
selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c
+128
View File
@@ -0,0 +1,128 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
// standard
#include <stdio.h>
#include <stdlib.h>
// acados
#include "acados/utils/print.h"
#include "acados/utils/math.h"
#include "acados_c/sim_interface.h"
#include "acados_sim_solver_lat.h"
#define NX LAT_NX
#define NZ LAT_NZ
#define NU LAT_NU
#define NP LAT_NP
int main()
{
int status = 0;
sim_solver_capsule *capsule = lat_acados_sim_solver_create_capsule();
status = lat_acados_sim_create(capsule);
if (status)
{
printf("acados_create() returned status %d. Exiting.\n", status);
exit(1);
}
sim_config *acados_sim_config = lat_acados_get_sim_config(capsule);
sim_in *acados_sim_in = lat_acados_get_sim_in(capsule);
sim_out *acados_sim_out = lat_acados_get_sim_out(capsule);
void *acados_sim_dims = lat_acados_get_sim_dims(capsule);
// initial condition
double x_current[NX];
x_current[0] = 0.0;
x_current[1] = 0.0;
x_current[2] = 0.0;
x_current[3] = 0.0;
x_current[0] = 0;
x_current[1] = 0;
x_current[2] = 0;
x_current[3] = 0;
// initial value for control input
double u0[NU];
u0[0] = 0.0;
// set parameters
double p[NP];
p[0] = 0;
p[1] = 0;
lat_acados_sim_update_params(capsule, p, NP);
int n_sim_steps = 3;
// solve ocp in loop
for (int ii = 0; ii < n_sim_steps; ii++)
{
sim_in_set(acados_sim_config, acados_sim_dims,
acados_sim_in, "x", x_current);
status = lat_acados_sim_solve(capsule);
if (status != ACADOS_SUCCESS)
{
printf("acados_solve() failed with status %d.\n", status);
}
sim_out_get(acados_sim_config, acados_sim_dims,
acados_sim_out, "x", x_current);
printf("\nx_current, %d\n", ii);
for (int jj = 0; jj < NX; jj++)
{
printf("%e\n", x_current[jj]);
}
}
printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps);
// free solver
status = lat_acados_sim_free(capsule);
if (status) {
printf("lat_acados_sim_free() returned status %d. \n", status);
}
lat_acados_sim_solver_free_capsule(capsule);
return status;
}
selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m
+125
View File
@@ -0,0 +1,125 @@
%
% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
%
% This file is part of acados.
%
% The 2-Clause BSD License
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGE.;
%
SOURCES = { ...
'lat_model/lat_expl_ode_fun.c', ...
'lat_model/lat_expl_vde_forw.c',...
'lat_cost/lat_cost_y_0_fun.c',...
'lat_cost/lat_cost_y_0_fun_jac_ut_xt.c',...
'lat_cost/lat_cost_y_0_hess.c',...
'lat_cost/lat_cost_y_fun.c',...
'lat_cost/lat_cost_y_fun_jac_ut_xt.c',...
'lat_cost/lat_cost_y_hess.c',...
'lat_cost/lat_cost_y_e_fun.c',...
'lat_cost/lat_cost_y_e_fun_jac_ut_xt.c',...
'lat_cost/lat_cost_y_e_hess.c',...
'acados_solver_sfunction_lat.c', ...
'acados_solver_lat.c'
};
INC_PATH = '/data/openpilot/third_party/acados/include/acados/include';
INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ...
['-I', fullfile(INC_PATH, 'hpipm', 'include')], ...
['-I', fullfile(INC_PATH, 'acados')], ...
['-I', fullfile(INC_PATH)]};
CFLAGS = 'CFLAGS=$CFLAGS';
LDFLAGS = 'LDFLAGS=$LDFLAGS';
COMPFLAGS = 'COMPFLAGS=$COMPFLAGS';
COMPDEFINES = 'COMPDEFINES=$COMPDEFINES';
LIB_PATH = ['-L', fullfile('/data/openpilot/third_party/acados/include/acados/lib')];
LIBS = {'-lacados', '-lhpipm', '-lblasfeo'};
% acados linking libraries and flags
mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
LIB_PATH, LIBS{:}, SOURCES{:}, ...
'-output', 'acados_solver_sfunction_lat' );
fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_lat', '.', ...
eval('mexext')] );
%% print note on usage of s-function
fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
input_note = 'Inputs are:\n';
i_in = 1;
input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',...
' size [4]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
' size [4]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N+1,',...
' size [34]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [3]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',...
' size [45]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [2]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') lbx for shooting nodes 1 to N-1, size [30]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') ubx for shooting nodes 1 to N-1, size [30]\n ');
i_in = i_in + 1;
fprintf(input_note)
disp(' ')
output_note = 'Outputs are:\n';
i_out = 0;
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [1]\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') KKT residual\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') CPU time\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n ');
fprintf(output_note)
selfdrive/dragonpilot/controls_0813/lib/lateral_planner.py
+6 -3
View File
@@ -80,10 +80,13 @@ class LateralPlanner:
# dynamic laneline/laneless logic
# decide if we want to use lanelines or laneless
self.dp_lanelines_active = get_lane_laneless_mode(self.LP.lll_prob, self.LP.rll_prob, self.dp_lanelines_active)
if self.dp_lanelines_enable:
self.dp_lanelines_active = get_lane_laneless_mode(self.LP.lll_prob, self.LP.rll_prob, self.dp_lanelines_active)
else:
self.dp_lanelines_active = False
# Calculate final driving path and set MPC costs
if self.dp_lanelines_enable and self.dp_lanelines_active:
if self.dp_lanelines_active:
self.d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, self.steer_rate_cost)
else:
@@ -139,7 +142,7 @@ class LateralPlanner:
lateralPlan.solverExecutionTime = self.lat_mpc.solve_time
lateralPlan.desire = self.DH.desire
lateralPlan.useLaneLines = self.dp_lanelines_enable and self.dp_lanelines_active
lateralPlan.useLaneLines = self.dp_lanelines_active
lateralPlan.laneChangeState = self.DH.lane_change_state
lateralPlan.laneChangeDirection = self.DH.lane_change_direction
selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/acados_ocp_lat.json
+450
View File
@@ -0,0 +1,450 @@
{
"acados_include_path": "/data/openpilot/third_party/acados/include/acados/include",
"acados_lib_path": "/data/openpilot/third_party/acados/include/acados/lib",
"code_export_directory": "/data/openpilot/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code",
"constraints": {
"C": [],
"C_e": [],
"D": [],
"constr_type": "BGH",
"constr_type_e": "BGH",
"idxbu": [],
"idxbx": [
2,
3
],
"idxbx_0": [
0,
1,
2,
3
],
"idxbx_e": [],
"idxbxe_0": [
0,
1,
2,
3
],
"idxsbu": [],
"idxsbx": [],
"idxsbx_e": [],
"idxsg": [],
"idxsg_e": [],
"idxsh": [],
"idxsh_e": [],
"idxsphi": [],
"idxsphi_e": [],
"lbu": [],
"lbx": [
-1.5707963267948966,
-0.8726646259971648
],
"lbx_0": [
0.0,
0.0,
0.0,
0.0
],
"lbx_e": [],
"lg": [],
"lg_e": [],
"lh": [],
"lh_e": [],
"lphi": [],
"lphi_e": [],
"lsbu": [],
"lsbx": [],
"lsbx_e": [],
"lsg": [],
"lsg_e": [],
"lsh": [],
"lsh_e": [],
"lsphi": [],
"lsphi_e": [],
"ubu": [],
"ubx": [
1.5707963267948966,
0.8726646259971648
],
"ubx_0": [
0.0,
0.0,
0.0,
0.0
],
"ubx_e": [],
"ug": [],
"ug_e": [],
"uh": [],
"uh_e": [],
"uphi": [],
"uphi_e": [],
"usbu": [],
"usbx": [],
"usbx_e": [],
"usg": [],
"usg_e": [],
"ush": [],
"ush_e": [],
"usphi": [],
"usphi_e": []
},
"cost": {
"Vu": [
[
0.0
],
[
0.0
],
[
0.0
]
],
"Vu_0": [
[
0.0
],
[
0.0
],
[
0.0
]
],
"Vx": [
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
]
],
"Vx_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
]
],
"Vx_e": [
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
]
],
"Vz": [],
"Vz_0": [],
"W": [
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
]
],
"W_0": [
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
]
],
"W_e": [
[
0.0,
0.0
],
[
0.0,
0.0
]
],
"Zl": [],
"Zl_e": [],
"Zu": [],
"Zu_e": [],
"cost_ext_fun_type": "casadi",
"cost_ext_fun_type_0": "casadi",
"cost_ext_fun_type_e": "casadi",
"cost_type": "NONLINEAR_LS",
"cost_type_0": "NONLINEAR_LS",
"cost_type_e": "NONLINEAR_LS",
"yref": [
0.0,
0.0,
0.0
],
"yref_0": [
0.0,
0.0,
0.0
],
"yref_e": [
0.0,
0.0
],
"zl": [],
"zl_e": [],
"zu": [],
"zu_e": []
},
"cython_include_dirs": "/data/data/com.termux/files/usr/lib/python3.8/site-packages/numpy/core/include",
"dims": {
"N": 16,
"nbu": 0,
"nbx": 2,
"nbx_0": 4,
"nbx_e": 0,
"nbxe_0": 4,
"ng": 0,
"ng_e": 0,
"nh": 0,
"nh_e": 0,
"np": 2,
"nphi": 0,
"nphi_e": 0,
"nr": 0,
"nr_e": 0,
"ns": 0,
"ns_e": 0,
"nsbu": 0,
"nsbx": 0,
"nsbx_e": 0,
"nsg": 0,
"nsg_e": 0,
"nsh": 0,
"nsh_e": 0,
"nsphi": 0,
"nsphi_e": 0,
"nu": 1,
"nx": 4,
"ny": 3,
"ny_0": 3,
"ny_e": 2,
"nz": 0
},
"model": {
"dyn_disc_fun": null,
"dyn_disc_fun_jac": null,
"dyn_disc_fun_jac_hess": null,
"dyn_ext_fun_type": "casadi",
"dyn_source_discrete": null,
"gnsf": {
"nontrivial_f_LO": 1,
"purely_linear": 0
},
"name": "lat"
},
"parameter_values": [
0.0,
0.0
],
"problem_class": "OCP",
"simulink_opts": {
"inputs": {
"cost_W": 0,
"cost_W_0": 0,
"cost_W_e": 0,
"lbu": 1,
"lbx": 1,
"lbx_0": 1,
"lbx_e": 1,
"lg": 1,
"lh": 1,
"parameter_traj": 1,
"reset_solver": 0,
"u_init": 0,
"ubu": 1,
"ubx": 1,
"ubx_0": 1,
"ubx_e": 1,
"ug": 1,
"uh": 1,
"x_init": 0,
"y_ref": 1,
"y_ref_0": 1,
"y_ref_e": 1
},
"outputs": {
"CPU_time": 1,
"CPU_time_lin": 0,
"CPU_time_qp": 0,
"CPU_time_sim": 0,
"KKT_residual": 1,
"solver_status": 1,
"sqp_iter": 1,
"u0": 1,
"utraj": 0,
"x1": 1,
"xtraj": 0
},
"samplingtime": "t0"
},
"solver_options": {
"Tsim": 0.009765625,
"alpha_min": 0.05,
"alpha_reduction": 0.7,
"collocation_type": "GAUSS_LEGENDRE",
"eps_sufficient_descent": 0.0001,
"exact_hess_constr": 1,
"exact_hess_cost": 1,
"exact_hess_dyn": 1,
"ext_cost_num_hess": 0,
"full_step_dual": 0,
"globalization": "FIXED_STEP",
"globalization_use_SOC": 0,
"hessian_approx": "GAUSS_NEWTON",
"hpipm_mode": "BALANCE",
"initialize_t_slacks": 0,
"integrator_type": "ERK",
"levenberg_marquardt": 0.0,
"line_search_use_sufficient_descent": 0,
"model_external_shared_lib_dir": null,
"model_external_shared_lib_name": null,
"nlp_solver_max_iter": 100,
"nlp_solver_step_length": 1.0,
"nlp_solver_tol_comp": 1e-06,
"nlp_solver_tol_eq": 1e-06,
"nlp_solver_tol_ineq": 1e-06,
"nlp_solver_tol_stat": 1e-06,
"nlp_solver_type": "SQP_RTI",
"print_level": 0,
"qp_solver": "PARTIAL_CONDENSING_HPIPM",
"qp_solver_cond_N": 1,
"qp_solver_iter_max": 1,
"qp_solver_tol_comp": null,
"qp_solver_tol_eq": null,
"qp_solver_tol_ineq": null,
"qp_solver_tol_stat": null,
"qp_solver_warm_start": 0,
"regularize_method": null,
"sim_method_jac_reuse": [
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0
],
"sim_method_newton_iter": 3,
"sim_method_num_stages": [
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4
],
"sim_method_num_steps": [
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1
],
"tf": 2.5,
"time_steps": [
0.009765625,
0.029296875,
0.048828125,
0.068359375,
0.087890625,
0.107421875,
0.126953125,
0.146484375,
0.166015625,
0.185546875,
0.205078125,
0.224609375,
0.244140625,
0.263671875,
0.283203125,
0.302734375
]
}
}
selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c
+278
View File
@@ -0,0 +1,278 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
// standard
#include <stdio.h>
#include <stdlib.h>
// acados
#include "acados_c/external_function_interface.h"
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
#include "acados/sim/sim_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/print.h"
// example specific
#include "lat_model/lat_model.h"
#include "acados_sim_solver_lat.h"
// ** solver data **
sim_solver_capsule * lat_acados_sim_solver_create_capsule()
{
void* capsule_mem = malloc(sizeof(sim_solver_capsule));
sim_solver_capsule *capsule = (sim_solver_capsule *) capsule_mem;
return capsule;
}
int lat_acados_sim_solver_free_capsule(sim_solver_capsule * capsule)
{
free(capsule);
return 0;
}
int lat_acados_sim_create(sim_solver_capsule * capsule)
{
// initialize
const int nx = LAT_NX;
const int nu = LAT_NU;
const int nz = LAT_NZ;
const int np = LAT_NP;
bool tmp_bool;
double Tsim = 0.009765625;
// explicit ode
capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
capsule->sim_forw_vde_casadi->casadi_fun = &lat_expl_vde_forw;
capsule->sim_forw_vde_casadi->casadi_n_in = &lat_expl_vde_forw_n_in;
capsule->sim_forw_vde_casadi->casadi_n_out = &lat_expl_vde_forw_n_out;
capsule->sim_forw_vde_casadi->casadi_sparsity_in = &lat_expl_vde_forw_sparsity_in;
capsule->sim_forw_vde_casadi->casadi_sparsity_out = &lat_expl_vde_forw_sparsity_out;
capsule->sim_forw_vde_casadi->casadi_work = &lat_expl_vde_forw_work;
external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np);
capsule->sim_expl_ode_fun_casadi->casadi_fun = &lat_expl_ode_fun;
capsule->sim_expl_ode_fun_casadi->casadi_n_in = &lat_expl_ode_fun_n_in;
capsule->sim_expl_ode_fun_casadi->casadi_n_out = &lat_expl_ode_fun_n_out;
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &lat_expl_ode_fun_sparsity_in;
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &lat_expl_ode_fun_sparsity_out;
capsule->sim_expl_ode_fun_casadi->casadi_work = &lat_expl_ode_fun_work;
external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np);
// sim plan & config
sim_solver_plan_t plan;
plan.sim_solver = ERK;
// create correct config based on plan
sim_config * lat_sim_config = sim_config_create(plan);
capsule->acados_sim_config = lat_sim_config;
// sim dims
void *lat_sim_dims = sim_dims_create(lat_sim_config);
capsule->acados_sim_dims = lat_sim_dims;
sim_dims_set(lat_sim_config, lat_sim_dims, "nx", &nx);
sim_dims_set(lat_sim_config, lat_sim_dims, "nu", &nu);
sim_dims_set(lat_sim_config, lat_sim_dims, "nz", &nz);
// sim opts
sim_opts *lat_sim_opts = sim_opts_create(lat_sim_config, lat_sim_dims);
capsule->acados_sim_opts = lat_sim_opts;
int tmp_int = 3;
sim_opts_set(lat_sim_config, lat_sim_opts, "newton_iter", &tmp_int);
sim_collocation_type collocation_type = GAUSS_LEGENDRE;
sim_opts_set(lat_sim_config, lat_sim_opts, "collocation_type", &collocation_type);
tmp_int = 4;
sim_opts_set(lat_sim_config, lat_sim_opts, "num_stages", &tmp_int);
tmp_int = 1;
sim_opts_set(lat_sim_config, lat_sim_opts, "num_steps", &tmp_int);
tmp_bool = 0;
sim_opts_set(lat_sim_config, lat_sim_opts, "jac_reuse", &tmp_bool);
// sim in / out
sim_in *lat_sim_in = sim_in_create(lat_sim_config, lat_sim_dims);
capsule->acados_sim_in = lat_sim_in;
sim_out *lat_sim_out = sim_out_create(lat_sim_config, lat_sim_dims);
capsule->acados_sim_out = lat_sim_out;
sim_in_set(lat_sim_config, lat_sim_dims,
lat_sim_in, "T", &Tsim);
// model functions
lat_sim_config->model_set(lat_sim_in->model,
"expl_vde_for", capsule->sim_forw_vde_casadi);
lat_sim_config->model_set(lat_sim_in->model,
"expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
// sim solver
sim_solver *lat_sim_solver = sim_solver_create(lat_sim_config,
lat_sim_dims, lat_sim_opts);
capsule->acados_sim_solver = lat_sim_solver;
/* initialize parameter values */
double* p = calloc(np, sizeof(double));
lat_acados_sim_update_params(capsule, p, np);
free(p);
/* initialize input */
// x
double x0[4];
for (int ii = 0; ii < 4; ii++)
x0[ii] = 0.0;
sim_in_set(lat_sim_config, lat_sim_dims,
lat_sim_in, "x", x0);
// u
double u0[1];
for (int ii = 0; ii < 1; ii++)
u0[ii] = 0.0;
sim_in_set(lat_sim_config, lat_sim_dims,
lat_sim_in, "u", u0);
// S_forw
double S_forw[20];
for (int ii = 0; ii < 20; ii++)
S_forw[ii] = 0.0;
for (int ii = 0; ii < 4; ii++)
S_forw[ii + ii * 4 ] = 1.0;
sim_in_set(lat_sim_config, lat_sim_dims,
lat_sim_in, "S_forw", S_forw);
int status = sim_precompute(lat_sim_solver, lat_sim_in, lat_sim_out);
return status;
}
int lat_acados_sim_solve(sim_solver_capsule *capsule)
{
// integrate dynamics using acados sim_solver
int status = sim_solve(capsule->acados_sim_solver,
capsule->acados_sim_in, capsule->acados_sim_out);
if (status != 0)
printf("error in lat_acados_sim_solve()! Exiting.\n");
return status;
}
int lat_acados_sim_free(sim_solver_capsule *capsule)
{
// free memory
sim_solver_destroy(capsule->acados_sim_solver);
sim_in_destroy(capsule->acados_sim_in);
sim_out_destroy(capsule->acados_sim_out);
sim_opts_destroy(capsule->acados_sim_opts);
sim_dims_destroy(capsule->acados_sim_dims);
sim_config_destroy(capsule->acados_sim_config);
// free external function
external_function_param_casadi_free(capsule->sim_forw_vde_casadi);
external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi);
return 0;
}
int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np)
{
int status = 0;
int casadi_np = LAT_NP;
if (casadi_np != np) {
printf("lat_acados_sim_update_params: trying to set %i parameters for external functions."
" External function has %i parameters. Exiting.\n", np, casadi_np);
exit(1);
}
capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
return status;
}
/* getters pointers to C objects*/
sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule)
{
return capsule->acados_sim_config;
};
sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule)
{
return capsule->acados_sim_in;
};
sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule)
{
return capsule->acados_sim_out;
};
void * lat_acados_get_sim_dims(sim_solver_capsule *capsule)
{
return capsule->acados_sim_dims;
};
sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule)
{
return capsule->acados_sim_opts;
};
sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule)
{
return capsule->acados_sim_solver;
};
selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h
+103
View File
@@ -0,0 +1,103 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_SIM_lat_H_
#define ACADOS_SIM_lat_H_
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
#define LAT_NX 4
#define LAT_NZ 0
#define LAT_NU 1
#define LAT_NP 2
#ifdef __cplusplus
extern "C" {
#endif
// ** capsule for solver data **
typedef struct sim_solver_capsule
{
// acados objects
sim_in *acados_sim_in;
sim_out *acados_sim_out;
sim_solver *acados_sim_solver;
sim_opts *acados_sim_opts;
sim_config *acados_sim_config;
void *acados_sim_dims;
/* external functions */
// ERK
external_function_param_casadi * sim_forw_vde_casadi;
external_function_param_casadi * sim_expl_ode_fun_casadi;
external_function_param_casadi * sim_expl_ode_hess;
// IRK
external_function_param_casadi * sim_impl_dae_fun;
external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z;
external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z;
external_function_param_casadi * sim_impl_dae_hess;
// GNSF
external_function_param_casadi * sim_gnsf_phi_fun;
external_function_param_casadi * sim_gnsf_phi_fun_jac_y;
external_function_param_casadi * sim_gnsf_phi_jac_y_uhat;
external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z;
external_function_param_casadi * sim_gnsf_get_matrices_fun;
} sim_solver_capsule;
ACADOS_SYMBOL_EXPORT int lat_acados_sim_create(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_sim_solve(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_sim_free(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np);
ACADOS_SYMBOL_EXPORT sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT void * lat_acados_get_sim_dims(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_solver_capsule * lat_acados_sim_solver_create_capsule(void);
ACADOS_SYMBOL_EXPORT int lat_acados_sim_solver_free_capsule(sim_solver_capsule *capsule);
#ifdef __cplusplus
}
#endif
#endif // ACADOS_SIM_lat_H_
selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c
+264
View File
@@ -0,0 +1,264 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#define S_FUNCTION_NAME acados_solver_sfunction_lat
#define S_FUNCTION_LEVEL 2
#define MDL_START
// acados
// #include "acados/utils/print.h"
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
// example specific
#include "lat_model/lat_model.h"
#include "acados_solver_lat.h"
#include "simstruc.h"
#define SAMPLINGTIME 0.009765625
static void mdlInitializeSizes (SimStruct *S)
{
// specify the number of continuous and discrete states
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);// specify the number of input ports
if ( !ssSetNumInputPorts(S, 8) )
return;
// specify the number of output ports
if ( !ssSetNumOutputPorts(S, 6) )
return;
// specify dimension information for the input ports
// lbx_0
ssSetInputPortVectorDimension(S, 0, 4);
// ubx_0
ssSetInputPortVectorDimension(S, 1, 4);
// parameters
ssSetInputPortVectorDimension(S, 2, (16+1) * 2);
// y_ref_0
ssSetInputPortVectorDimension(S, 3, 3);
// y_ref
ssSetInputPortVectorDimension(S, 4, 45);
// y_ref_e
ssSetInputPortVectorDimension(S, 5, 2);
// lbx
ssSetInputPortVectorDimension(S, 6, 30);
// ubx
ssSetInputPortVectorDimension(S, 7, 30);/* specify dimension information for the OUTPUT ports */
ssSetOutputPortVectorDimension(S, 0, 1 );
ssSetOutputPortVectorDimension(S, 1, 1 );
ssSetOutputPortVectorDimension(S, 2, 1 );
ssSetOutputPortVectorDimension(S, 3, 4 ); // state at shooting node 1
ssSetOutputPortVectorDimension(S, 4, 1);
ssSetOutputPortVectorDimension(S, 5, 1 );
// specify the direct feedthrough status
// should be set to 1 for all inputs used in mdlOutputs
ssSetInputPortDirectFeedThrough(S, 0, 1);
ssSetInputPortDirectFeedThrough(S, 1, 1);
ssSetInputPortDirectFeedThrough(S, 2, 1);
ssSetInputPortDirectFeedThrough(S, 3, 1);
ssSetInputPortDirectFeedThrough(S, 4, 1);
ssSetInputPortDirectFeedThrough(S, 5, 1);
ssSetInputPortDirectFeedThrough(S, 6, 1);
ssSetInputPortDirectFeedThrough(S, 7, 1);
// one sample time
ssSetNumSampleTimes(S, 1);
}
#if defined(MATLAB_MEX_FILE)
#define MDL_SET_INPUT_PORT_DIMENSION_INFO
#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
{
if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
return;
}
static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
{
if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
return;
}
#endif /* MATLAB_MEX_FILE */
static void mdlInitializeSampleTimes(SimStruct *S)
{
ssSetSampleTime(S, 0, SAMPLINGTIME);
ssSetOffsetTime(S, 0, 0.0);
}
static void mdlStart(SimStruct *S)
{
lat_solver_capsule *capsule = lat_acados_create_capsule();
lat_acados_create(capsule);
ssSetUserData(S, (void*)capsule);
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
lat_solver_capsule *capsule = ssGetUserData(S);
ocp_nlp_config *nlp_config = lat_acados_get_nlp_config(capsule);
ocp_nlp_dims *nlp_dims = lat_acados_get_nlp_dims(capsule);
ocp_nlp_in *nlp_in = lat_acados_get_nlp_in(capsule);
ocp_nlp_out *nlp_out = lat_acados_get_nlp_out(capsule);
InputRealPtrsType in_sign;
// local buffer
real_t buffer[4];
/* go through inputs */
// lbx_0
in_sign = ssGetInputPortRealSignalPtrs(S, 0);
for (int i = 0; i < 4; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer);
// ubx_0
in_sign = ssGetInputPortRealSignalPtrs(S, 1);
for (int i = 0; i < 4; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer);
// parameters - stage-variant !!!
in_sign = ssGetInputPortRealSignalPtrs(S, 2);
// update value of parameters
for (int ii = 0; ii <= 16; ii++)
{
for (int jj = 0; jj < 2; jj++)
buffer[jj] = (double)(*in_sign[ii*2+jj]);
lat_acados_update_params(capsule, ii, buffer, 2);
}
// y_ref_0
in_sign = ssGetInputPortRealSignalPtrs(S, 3);
for (int i = 0; i < 3; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", (void *) buffer);
// y_ref - for stages 1 to N-1
in_sign = ssGetInputPortRealSignalPtrs(S, 4);
for (int ii = 1; ii < 16; ii++)
{
for (int jj = 0; jj < 3; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*3+jj]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer);
}
// y_ref_e
in_sign = ssGetInputPortRealSignalPtrs(S, 5);
for (int i = 0; i < 2; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 16, "yref", (void *) buffer);
// lbx
in_sign = ssGetInputPortRealSignalPtrs(S, 6);
for (int ii = 1; ii < 16; ii++)
{
for (int jj = 0; jj < 2; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbx", (void *) buffer);
}
// ubx
in_sign = ssGetInputPortRealSignalPtrs(S, 7);
for (int ii = 1; ii < 16; ii++)
{
for (int jj = 0; jj < 2; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubx", (void *) buffer);
}
/* call solver */
int rti_phase = 0;
ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase);
int acados_status = lat_acados_solve(capsule);
/* set outputs */
// assign pointers to output signals
real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin;
int tmp_int;
out_u0 = ssGetOutputPortRealSignal(S, 0);
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0);
out_status = ssGetOutputPortRealSignal(S, 1);
*out_status = (real_t) acados_status;
out_KKT_res = ssGetOutputPortRealSignal(S, 2);
*out_KKT_res = (real_t) nlp_out->inf_norm_res;
out_x1 = ssGetOutputPortRealSignal(S, 3);
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1);
out_cpu_time = ssGetOutputPortRealSignal(S, 4);
// get solution time
ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_tot", (void *) out_cpu_time);
out_sqp_iter = ssGetOutputPortRealSignal(S, 5);
// get sqp iter
ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int);
*out_sqp_iter = (real_t) tmp_int;
}
static void mdlTerminate(SimStruct *S)
{
lat_solver_capsule *capsule = ssGetUserData(S);
lat_acados_free(capsule);
lat_acados_free_capsule(capsule);
}
#ifdef MATLAB_MEX_FILE
#include "simulink.c"
#else
#include "cg_sfun.h"
#endif
selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c
+128
View File
@@ -0,0 +1,128 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
// standard
#include <stdio.h>
#include <stdlib.h>
// acados
#include "acados/utils/print.h"
#include "acados/utils/math.h"
#include "acados_c/sim_interface.h"
#include "acados_sim_solver_lat.h"
#define NX LAT_NX
#define NZ LAT_NZ
#define NU LAT_NU
#define NP LAT_NP
int main()
{
int status = 0;
sim_solver_capsule *capsule = lat_acados_sim_solver_create_capsule();
status = lat_acados_sim_create(capsule);
if (status)
{
printf("acados_create() returned status %d. Exiting.\n", status);
exit(1);
}
sim_config *acados_sim_config = lat_acados_get_sim_config(capsule);
sim_in *acados_sim_in = lat_acados_get_sim_in(capsule);
sim_out *acados_sim_out = lat_acados_get_sim_out(capsule);
void *acados_sim_dims = lat_acados_get_sim_dims(capsule);
// initial condition
double x_current[NX];
x_current[0] = 0.0;
x_current[1] = 0.0;
x_current[2] = 0.0;
x_current[3] = 0.0;
x_current[0] = 0;
x_current[1] = 0;
x_current[2] = 0;
x_current[3] = 0;
// initial value for control input
double u0[NU];
u0[0] = 0.0;
// set parameters
double p[NP];
p[0] = 0;
p[1] = 0;
lat_acados_sim_update_params(capsule, p, NP);
int n_sim_steps = 3;
// solve ocp in loop
for (int ii = 0; ii < n_sim_steps; ii++)
{
sim_in_set(acados_sim_config, acados_sim_dims,
acados_sim_in, "x", x_current);
status = lat_acados_sim_solve(capsule);
if (status != ACADOS_SUCCESS)
{
printf("acados_solve() failed with status %d.\n", status);
}
sim_out_get(acados_sim_config, acados_sim_dims,
acados_sim_out, "x", x_current);
printf("\nx_current, %d\n", ii);
for (int jj = 0; jj < NX; jj++)
{
printf("%e\n", x_current[jj]);
}
}
printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps);
// free solver
status = lat_acados_sim_free(capsule);
if (status) {
printf("lat_acados_sim_free() returned status %d. \n", status);
}
lat_acados_sim_solver_free_capsule(capsule);
return status;
}
selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m
+125
View File
@@ -0,0 +1,125 @@
%
% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
%
% This file is part of acados.
%
% The 2-Clause BSD License
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGE.;
%
SOURCES = { ...
'lat_model/lat_expl_ode_fun.c', ...
'lat_model/lat_expl_vde_forw.c',...
'lat_cost/lat_cost_y_0_fun.c',...
'lat_cost/lat_cost_y_0_fun_jac_ut_xt.c',...
'lat_cost/lat_cost_y_0_hess.c',...
'lat_cost/lat_cost_y_fun.c',...
'lat_cost/lat_cost_y_fun_jac_ut_xt.c',...
'lat_cost/lat_cost_y_hess.c',...
'lat_cost/lat_cost_y_e_fun.c',...
'lat_cost/lat_cost_y_e_fun_jac_ut_xt.c',...
'lat_cost/lat_cost_y_e_hess.c',...
'acados_solver_sfunction_lat.c', ...
'acados_solver_lat.c'
};
INC_PATH = '/data/openpilot/third_party/acados/include/acados/include';
INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ...
['-I', fullfile(INC_PATH, 'hpipm', 'include')], ...
['-I', fullfile(INC_PATH, 'acados')], ...
['-I', fullfile(INC_PATH)]};
CFLAGS = 'CFLAGS=$CFLAGS';
LDFLAGS = 'LDFLAGS=$LDFLAGS';
COMPFLAGS = 'COMPFLAGS=$COMPFLAGS';
COMPDEFINES = 'COMPDEFINES=$COMPDEFINES';
LIB_PATH = ['-L', fullfile('/data/openpilot/third_party/acados/include/acados/lib')];
LIBS = {'-lacados', '-lhpipm', '-lblasfeo'};
% acados linking libraries and flags
mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
LIB_PATH, LIBS{:}, SOURCES{:}, ...
'-output', 'acados_solver_sfunction_lat' );
fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_lat', '.', ...
eval('mexext')] );
%% print note on usage of s-function
fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
input_note = 'Inputs are:\n';
i_in = 1;
input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',...
' size [4]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
' size [4]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N+1,',...
' size [34]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [3]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',...
' size [45]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [2]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') lbx for shooting nodes 1 to N-1, size [30]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') ubx for shooting nodes 1 to N-1, size [30]\n ');
i_in = i_in + 1;
fprintf(input_note)
disp(' ')
output_note = 'Outputs are:\n';
i_out = 0;
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [1]\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') KKT residual\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') CPU time\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n ');
fprintf(output_note)
selfdrive/dragonpilot/controls_0816/lib/lateral_planner.py
+6 -3
View File
@@ -77,10 +77,13 @@ class LateralPlanner:
# dynamic laneline/laneless logic
# decide if we want to use lanelines or laneless
self.dp_lanelines_active = get_lane_laneless_mode(self.LP.lll_prob, self.LP.rll_prob, self.dp_lanelines_active)
if self.dp_lanelines_enable:
self.dp_lanelines_active = get_lane_laneless_mode(self.LP.lll_prob, self.LP.rll_prob, self.dp_lanelines_active)
else:
self.dp_lanelines_active = False
# Calculate final driving path and set MPC costs
if self.dp_lanelines_enable and self.dp_lanelines_active:
if self.dp_lanelines_active:
self.d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, MPC_COST_LAT.STEER_RATE)
else:
@@ -141,7 +144,7 @@ class LateralPlanner:
lateralPlan.solverExecutionTime = self.lat_mpc.solve_time
lateralPlan.desire = self.DH.desire
lateralPlan.useLaneLines = self.dp_lanelines_enable and self.dp_lanelines_active
lateralPlan.useLaneLines = self.dp_lanelines_active
lateralPlan.laneChangeState = self.DH.lane_change_state
lateralPlan.laneChangeDirection = self.DH.lane_change_direction
selfdrive/locationd/locationd
BIN
View File
Binary file not shown.
selfdrive/locationd/models/generated/car.cpp
+342 -342
View File
@@ -45,326 +45,326 @@ const static double MAHA_THRESH_31 = 3.8414588206941227;
* *
* This file is part of 'ekf' *
******************************************************************************/
void err_fun(double *nom_x, double *delta_x, double *out_5634330020296848070) {
out_5634330020296848070[0] = delta_x[0] + nom_x[0];
out_5634330020296848070[1] = delta_x[1] + nom_x[1];
out_5634330020296848070[2] = delta_x[2] + nom_x[2];
out_5634330020296848070[3] = delta_x[3] + nom_x[3];
out_5634330020296848070[4] = delta_x[4] + nom_x[4];
out_5634330020296848070[5] = delta_x[5] + nom_x[5];
out_5634330020296848070[6] = delta_x[6] + nom_x[6];
out_5634330020296848070[7] = delta_x[7] + nom_x[7];
out_5634330020296848070[8] = delta_x[8] + nom_x[8];
void err_fun(double *nom_x, double *delta_x, double *out_9008082079438066370) {
out_9008082079438066370[0] = delta_x[0] + nom_x[0];
out_9008082079438066370[1] = delta_x[1] + nom_x[1];
out_9008082079438066370[2] = delta_x[2] + nom_x[2];
out_9008082079438066370[3] = delta_x[3] + nom_x[3];
out_9008082079438066370[4] = delta_x[4] + nom_x[4];
out_9008082079438066370[5] = delta_x[5] + nom_x[5];
out_9008082079438066370[6] = delta_x[6] + nom_x[6];
out_9008082079438066370[7] = delta_x[7] + nom_x[7];
out_9008082079438066370[8] = delta_x[8] + nom_x[8];
}
void inv_err_fun(double *nom_x, double *true_x, double *out_1061559027503315281) {
out_1061559027503315281[0] = -nom_x[0] + true_x[0];
out_1061559027503315281[1] = -nom_x[1] + true_x[1];
out_1061559027503315281[2] = -nom_x[2] + true_x[2];
out_1061559027503315281[3] = -nom_x[3] + true_x[3];
out_1061559027503315281[4] = -nom_x[4] + true_x[4];
out_1061559027503315281[5] = -nom_x[5] + true_x[5];
out_1061559027503315281[6] = -nom_x[6] + true_x[6];
out_1061559027503315281[7] = -nom_x[7] + true_x[7];
out_1061559027503315281[8] = -nom_x[8] + true_x[8];
void inv_err_fun(double *nom_x, double *true_x, double *out_7705390806256602072) {
out_7705390806256602072[0] = -nom_x[0] + true_x[0];
out_7705390806256602072[1] = -nom_x[1] + true_x[1];
out_7705390806256602072[2] = -nom_x[2] + true_x[2];
out_7705390806256602072[3] = -nom_x[3] + true_x[3];
out_7705390806256602072[4] = -nom_x[4] + true_x[4];
out_7705390806256602072[5] = -nom_x[5] + true_x[5];
out_7705390806256602072[6] = -nom_x[6] + true_x[6];
out_7705390806256602072[7] = -nom_x[7] + true_x[7];
out_7705390806256602072[8] = -nom_x[8] + true_x[8];
}
void H_mod_fun(double *state, double *out_1567569310651922730) {
out_1567569310651922730[0] = 1.0;
out_1567569310651922730[1] = 0;
out_1567569310651922730[2] = 0;
out_1567569310651922730[3] = 0;
out_1567569310651922730[4] = 0;
out_1567569310651922730[5] = 0;
out_1567569310651922730[6] = 0;
out_1567569310651922730[7] = 0;
out_1567569310651922730[8] = 0;
out_1567569310651922730[9] = 0;
out_1567569310651922730[10] = 1.0;
out_1567569310651922730[11] = 0;
out_1567569310651922730[12] = 0;
out_1567569310651922730[13] = 0;
out_1567569310651922730[14] = 0;
out_1567569310651922730[15] = 0;
out_1567569310651922730[16] = 0;
out_1567569310651922730[17] = 0;
out_1567569310651922730[18] = 0;
out_1567569310651922730[19] = 0;
out_1567569310651922730[20] = 1.0;
out_1567569310651922730[21] = 0;
out_1567569310651922730[22] = 0;
out_1567569310651922730[23] = 0;
out_1567569310651922730[24] = 0;
out_1567569310651922730[25] = 0;
out_1567569310651922730[26] = 0;
out_1567569310651922730[27] = 0;
out_1567569310651922730[28] = 0;
out_1567569310651922730[29] = 0;
out_1567569310651922730[30] = 1.0;
out_1567569310651922730[31] = 0;
out_1567569310651922730[32] = 0;
out_1567569310651922730[33] = 0;
out_1567569310651922730[34] = 0;
out_1567569310651922730[35] = 0;
out_1567569310651922730[36] = 0;
out_1567569310651922730[37] = 0;
out_1567569310651922730[38] = 0;
out_1567569310651922730[39] = 0;
out_1567569310651922730[40] = 1.0;
out_1567569310651922730[41] = 0;
out_1567569310651922730[42] = 0;
out_1567569310651922730[43] = 0;
out_1567569310651922730[44] = 0;
out_1567569310651922730[45] = 0;
out_1567569310651922730[46] = 0;
out_1567569310651922730[47] = 0;
out_1567569310651922730[48] = 0;
out_1567569310651922730[49] = 0;
out_1567569310651922730[50] = 1.0;
out_1567569310651922730[51] = 0;
out_1567569310651922730[52] = 0;
out_1567569310651922730[53] = 0;
out_1567569310651922730[54] = 0;
out_1567569310651922730[55] = 0;
out_1567569310651922730[56] = 0;
out_1567569310651922730[57] = 0;
out_1567569310651922730[58] = 0;
out_1567569310651922730[59] = 0;
out_1567569310651922730[60] = 1.0;
out_1567569310651922730[61] = 0;
out_1567569310651922730[62] = 0;
out_1567569310651922730[63] = 0;
out_1567569310651922730[64] = 0;
out_1567569310651922730[65] = 0;
out_1567569310651922730[66] = 0;
out_1567569310651922730[67] = 0;
out_1567569310651922730[68] = 0;
out_1567569310651922730[69] = 0;
out_1567569310651922730[70] = 1.0;
out_1567569310651922730[71] = 0;
out_1567569310651922730[72] = 0;
out_1567569310651922730[73] = 0;
out_1567569310651922730[74] = 0;
out_1567569310651922730[75] = 0;
out_1567569310651922730[76] = 0;
out_1567569310651922730[77] = 0;
out_1567569310651922730[78] = 0;
out_1567569310651922730[79] = 0;
out_1567569310651922730[80] = 1.0;
void H_mod_fun(double *state, double *out_4044238657628669408) {
out_4044238657628669408[0] = 1.0;
out_4044238657628669408[1] = 0;
out_4044238657628669408[2] = 0;
out_4044238657628669408[3] = 0;
out_4044238657628669408[4] = 0;
out_4044238657628669408[5] = 0;
out_4044238657628669408[6] = 0;
out_4044238657628669408[7] = 0;
out_4044238657628669408[8] = 0;
out_4044238657628669408[9] = 0;
out_4044238657628669408[10] = 1.0;
out_4044238657628669408[11] = 0;
out_4044238657628669408[12] = 0;
out_4044238657628669408[13] = 0;
out_4044238657628669408[14] = 0;
out_4044238657628669408[15] = 0;
out_4044238657628669408[16] = 0;
out_4044238657628669408[17] = 0;
out_4044238657628669408[18] = 0;
out_4044238657628669408[19] = 0;
out_4044238657628669408[20] = 1.0;
out_4044238657628669408[21] = 0;
out_4044238657628669408[22] = 0;
out_4044238657628669408[23] = 0;
out_4044238657628669408[24] = 0;
out_4044238657628669408[25] = 0;
out_4044238657628669408[26] = 0;
out_4044238657628669408[27] = 0;
out_4044238657628669408[28] = 0;
out_4044238657628669408[29] = 0;
out_4044238657628669408[30] = 1.0;
out_4044238657628669408[31] = 0;
out_4044238657628669408[32] = 0;
out_4044238657628669408[33] = 0;
out_4044238657628669408[34] = 0;
out_4044238657628669408[35] = 0;
out_4044238657628669408[36] = 0;
out_4044238657628669408[37] = 0;
out_4044238657628669408[38] = 0;
out_4044238657628669408[39] = 0;
out_4044238657628669408[40] = 1.0;
out_4044238657628669408[41] = 0;
out_4044238657628669408[42] = 0;
out_4044238657628669408[43] = 0;
out_4044238657628669408[44] = 0;
out_4044238657628669408[45] = 0;
out_4044238657628669408[46] = 0;
out_4044238657628669408[47] = 0;
out_4044238657628669408[48] = 0;
out_4044238657628669408[49] = 0;
out_4044238657628669408[50] = 1.0;
out_4044238657628669408[51] = 0;
out_4044238657628669408[52] = 0;
out_4044238657628669408[53] = 0;
out_4044238657628669408[54] = 0;
out_4044238657628669408[55] = 0;
out_4044238657628669408[56] = 0;
out_4044238657628669408[57] = 0;
out_4044238657628669408[58] = 0;
out_4044238657628669408[59] = 0;
out_4044238657628669408[60] = 1.0;
out_4044238657628669408[61] = 0;
out_4044238657628669408[62] = 0;
out_4044238657628669408[63] = 0;
out_4044238657628669408[64] = 0;
out_4044238657628669408[65] = 0;
out_4044238657628669408[66] = 0;
out_4044238657628669408[67] = 0;
out_4044238657628669408[68] = 0;
out_4044238657628669408[69] = 0;
out_4044238657628669408[70] = 1.0;
out_4044238657628669408[71] = 0;
out_4044238657628669408[72] = 0;
out_4044238657628669408[73] = 0;
out_4044238657628669408[74] = 0;
out_4044238657628669408[75] = 0;
out_4044238657628669408[76] = 0;
out_4044238657628669408[77] = 0;
out_4044238657628669408[78] = 0;
out_4044238657628669408[79] = 0;
out_4044238657628669408[80] = 1.0;
}
void f_fun(double *state, double dt, double *out_1208381054458531187) {
out_1208381054458531187[0] = state[0];
out_1208381054458531187[1] = state[1];
out_1208381054458531187[2] = state[2];
out_1208381054458531187[3] = state[3];
out_1208381054458531187[4] = state[4];
out_1208381054458531187[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8000000000000007*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
out_1208381054458531187[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
out_1208381054458531187[7] = state[7];
out_1208381054458531187[8] = state[8];
void f_fun(double *state, double dt, double *out_6587028471138044914) {
out_6587028471138044914[0] = state[0];
out_6587028471138044914[1] = state[1];
out_6587028471138044914[2] = state[2];
out_6587028471138044914[3] = state[3];
out_6587028471138044914[4] = state[4];
out_6587028471138044914[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8000000000000007*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
out_6587028471138044914[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
out_6587028471138044914[7] = state[7];
out_6587028471138044914[8] = state[8];
}
void F_fun(double *state, double dt, double *out_8824004521572161416) {
out_8824004521572161416[0] = 1;
out_8824004521572161416[1] = 0;
out_8824004521572161416[2] = 0;
out_8824004521572161416[3] = 0;
out_8824004521572161416[4] = 0;
out_8824004521572161416[5] = 0;
out_8824004521572161416[6] = 0;
out_8824004521572161416[7] = 0;
out_8824004521572161416[8] = 0;
out_8824004521572161416[9] = 0;
out_8824004521572161416[10] = 1;
out_8824004521572161416[11] = 0;
out_8824004521572161416[12] = 0;
out_8824004521572161416[13] = 0;
out_8824004521572161416[14] = 0;
out_8824004521572161416[15] = 0;
out_8824004521572161416[16] = 0;
out_8824004521572161416[17] = 0;
out_8824004521572161416[18] = 0;
out_8824004521572161416[19] = 0;
out_8824004521572161416[20] = 1;
out_8824004521572161416[21] = 0;
out_8824004521572161416[22] = 0;
out_8824004521572161416[23] = 0;
out_8824004521572161416[24] = 0;
out_8824004521572161416[25] = 0;
out_8824004521572161416[26] = 0;
out_8824004521572161416[27] = 0;
out_8824004521572161416[28] = 0;
out_8824004521572161416[29] = 0;
out_8824004521572161416[30] = 1;
out_8824004521572161416[31] = 0;
out_8824004521572161416[32] = 0;
out_8824004521572161416[33] = 0;
out_8824004521572161416[34] = 0;
out_8824004521572161416[35] = 0;
out_8824004521572161416[36] = 0;
out_8824004521572161416[37] = 0;
out_8824004521572161416[38] = 0;
out_8824004521572161416[39] = 0;
out_8824004521572161416[40] = 1;
out_8824004521572161416[41] = 0;
out_8824004521572161416[42] = 0;
out_8824004521572161416[43] = 0;
out_8824004521572161416[44] = 0;
out_8824004521572161416[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
out_8824004521572161416[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
out_8824004521572161416[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_8824004521572161416[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_8824004521572161416[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
out_8824004521572161416[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
out_8824004521572161416[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
out_8824004521572161416[52] = dt*stiffness_front*state[0]/(mass*state[1]);
out_8824004521572161416[53] = -9.8000000000000007*dt;
out_8824004521572161416[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
out_8824004521572161416[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
out_8824004521572161416[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_8824004521572161416[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_8824004521572161416[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
out_8824004521572161416[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
out_8824004521572161416[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
out_8824004521572161416[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_8824004521572161416[62] = 0;
out_8824004521572161416[63] = 0;
out_8824004521572161416[64] = 0;
out_8824004521572161416[65] = 0;
out_8824004521572161416[66] = 0;
out_8824004521572161416[67] = 0;
out_8824004521572161416[68] = 0;
out_8824004521572161416[69] = 0;
out_8824004521572161416[70] = 1;
out_8824004521572161416[71] = 0;
out_8824004521572161416[72] = 0;
out_8824004521572161416[73] = 0;
out_8824004521572161416[74] = 0;
out_8824004521572161416[75] = 0;
out_8824004521572161416[76] = 0;
out_8824004521572161416[77] = 0;
out_8824004521572161416[78] = 0;
out_8824004521572161416[79] = 0;
out_8824004521572161416[80] = 1;
void F_fun(double *state, double dt, double *out_7531965082275181187) {
out_7531965082275181187[0] = 1;
out_7531965082275181187[1] = 0;
out_7531965082275181187[2] = 0;
out_7531965082275181187[3] = 0;
out_7531965082275181187[4] = 0;
out_7531965082275181187[5] = 0;
out_7531965082275181187[6] = 0;
out_7531965082275181187[7] = 0;
out_7531965082275181187[8] = 0;
out_7531965082275181187[9] = 0;
out_7531965082275181187[10] = 1;
out_7531965082275181187[11] = 0;
out_7531965082275181187[12] = 0;
out_7531965082275181187[13] = 0;
out_7531965082275181187[14] = 0;
out_7531965082275181187[15] = 0;
out_7531965082275181187[16] = 0;
out_7531965082275181187[17] = 0;
out_7531965082275181187[18] = 0;
out_7531965082275181187[19] = 0;
out_7531965082275181187[20] = 1;
out_7531965082275181187[21] = 0;
out_7531965082275181187[22] = 0;
out_7531965082275181187[23] = 0;
out_7531965082275181187[24] = 0;
out_7531965082275181187[25] = 0;
out_7531965082275181187[26] = 0;
out_7531965082275181187[27] = 0;
out_7531965082275181187[28] = 0;
out_7531965082275181187[29] = 0;
out_7531965082275181187[30] = 1;
out_7531965082275181187[31] = 0;
out_7531965082275181187[32] = 0;
out_7531965082275181187[33] = 0;
out_7531965082275181187[34] = 0;
out_7531965082275181187[35] = 0;
out_7531965082275181187[36] = 0;
out_7531965082275181187[37] = 0;
out_7531965082275181187[38] = 0;
out_7531965082275181187[39] = 0;
out_7531965082275181187[40] = 1;
out_7531965082275181187[41] = 0;
out_7531965082275181187[42] = 0;
out_7531965082275181187[43] = 0;
out_7531965082275181187[44] = 0;
out_7531965082275181187[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
out_7531965082275181187[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
out_7531965082275181187[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_7531965082275181187[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_7531965082275181187[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
out_7531965082275181187[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
out_7531965082275181187[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
out_7531965082275181187[52] = dt*stiffness_front*state[0]/(mass*state[1]);
out_7531965082275181187[53] = -9.8000000000000007*dt;
out_7531965082275181187[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
out_7531965082275181187[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
out_7531965082275181187[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_7531965082275181187[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_7531965082275181187[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
out_7531965082275181187[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
out_7531965082275181187[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
out_7531965082275181187[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_7531965082275181187[62] = 0;
out_7531965082275181187[63] = 0;
out_7531965082275181187[64] = 0;
out_7531965082275181187[65] = 0;
out_7531965082275181187[66] = 0;
out_7531965082275181187[67] = 0;
out_7531965082275181187[68] = 0;
out_7531965082275181187[69] = 0;
out_7531965082275181187[70] = 1;
out_7531965082275181187[71] = 0;
out_7531965082275181187[72] = 0;
out_7531965082275181187[73] = 0;
out_7531965082275181187[74] = 0;
out_7531965082275181187[75] = 0;
out_7531965082275181187[76] = 0;
out_7531965082275181187[77] = 0;
out_7531965082275181187[78] = 0;
out_7531965082275181187[79] = 0;
out_7531965082275181187[80] = 1;
}
void h_25(double *state, double *unused, double *out_1789446864250922111) {
out_1789446864250922111[0] = state[6];
void h_25(double *state, double *unused, double *out_7956124124599872411) {
out_7956124124599872411[0] = state[6];
}
void H_25(double *state, double *unused, double *out_9129886545195087354) {
out_9129886545195087354[0] = 0;
out_9129886545195087354[1] = 0;
out_9129886545195087354[2] = 0;
out_9129886545195087354[3] = 0;
out_9129886545195087354[4] = 0;
out_9129886545195087354[5] = 0;
out_9129886545195087354[6] = 1;
out_9129886545195087354[7] = 0;
out_9129886545195087354[8] = 0;
void H_25(double *state, double *unused, double *out_1152149855673758191) {
out_1152149855673758191[0] = 0;
out_1152149855673758191[1] = 0;
out_1152149855673758191[2] = 0;
out_1152149855673758191[3] = 0;
out_1152149855673758191[4] = 0;
out_1152149855673758191[5] = 0;
out_1152149855673758191[6] = 1;
out_1152149855673758191[7] = 0;
out_1152149855673758191[8] = 0;
}
void h_24(double *state, double *unused, double *out_3299160385053087312) {
out_3299160385053087312[0] = state[4];
out_3299160385053087312[1] = state[5];
void h_24(double *state, double *unused, double *out_8831563141601233713) {
out_8831563141601233713[0] = state[4];
out_8831563141601233713[1] = state[5];
}
void H_24(double *state, double *unused, double *out_7144207929508964696) {
out_7144207929508964696[0] = 0;
out_7144207929508964696[1] = 0;
out_7144207929508964696[2] = 0;
out_7144207929508964696[3] = 0;
out_7144207929508964696[4] = 1;
out_7144207929508964696[5] = 0;
out_7144207929508964696[6] = 0;
out_7144207929508964696[7] = 0;
out_7144207929508964696[8] = 0;
out_7144207929508964696[9] = 0;
out_7144207929508964696[10] = 0;
out_7144207929508964696[11] = 0;
out_7144207929508964696[12] = 0;
out_7144207929508964696[13] = 0;
out_7144207929508964696[14] = 1;
out_7144207929508964696[15] = 0;
out_7144207929508964696[16] = 0;
out_7144207929508964696[17] = 0;
void H_24(double *state, double *unused, double *out_1020499743331741375) {
out_1020499743331741375[0] = 0;
out_1020499743331741375[1] = 0;
out_1020499743331741375[2] = 0;
out_1020499743331741375[3] = 0;
out_1020499743331741375[4] = 1;
out_1020499743331741375[5] = 0;
out_1020499743331741375[6] = 0;
out_1020499743331741375[7] = 0;
out_1020499743331741375[8] = 0;
out_1020499743331741375[9] = 0;
out_1020499743331741375[10] = 0;
out_1020499743331741375[11] = 0;
out_1020499743331741375[12] = 0;
out_1020499743331741375[13] = 0;
out_1020499743331741375[14] = 1;
out_1020499743331741375[15] = 0;
out_1020499743331741375[16] = 0;
out_1020499743331741375[17] = 0;
}
void h_30(double *state, double *unused, double *out_7348270420432929071) {
out_7348270420432929071[0] = state[4];
void h_30(double *state, double *unused, double *out_6628936472681177553) {
out_6628936472681177553[0] = state[4];
}
void H_30(double *state, double *unused, double *out_6611553586687838727) {
out_6611553586687838727[0] = 0;
out_6611553586687838727[1] = 0;
out_6611553586687838727[2] = 0;
out_6611553586687838727[3] = 0;
out_6611553586687838727[4] = 1;
out_6611553586687838727[5] = 0;
out_6611553586687838727[6] = 0;
out_6611553586687838727[7] = 0;
out_6611553586687838727[8] = 0;
void H_30(double *state, double *unused, double *out_3375546474453850007) {
out_3375546474453850007[0] = 0;
out_3375546474453850007[1] = 0;
out_3375546474453850007[2] = 0;
out_3375546474453850007[3] = 0;
out_3375546474453850007[4] = 1;
out_3375546474453850007[5] = 0;
out_3375546474453850007[6] = 0;
out_3375546474453850007[7] = 0;
out_3375546474453850007[8] = 0;
}
void h_26(double *state, double *unused, double *out_6096800385615776900) {
out_6096800385615776900[0] = state[7];
void h_26(double *state, double *unused, double *out_3816384369411324170) {
out_3816384369411324170[0] = state[7];
}
void H_26(double *state, double *unused, double *out_5575354209640408038) {
out_5575354209640408038[0] = 0;
out_5575354209640408038[1] = 0;
out_5575354209640408038[2] = 0;
out_5575354209640408038[3] = 0;
out_5575354209640408038[4] = 0;
out_5575354209640408038[5] = 0;
out_5575354209640408038[6] = 0;
out_5575354209640408038[7] = 1;
out_5575354209640408038[8] = 0;
void H_26(double *state, double *unused, double *out_2589353463200298033) {
out_2589353463200298033[0] = 0;
out_2589353463200298033[1] = 0;
out_2589353463200298033[2] = 0;
out_2589353463200298033[3] = 0;
out_2589353463200298033[4] = 0;
out_2589353463200298033[5] = 0;
out_2589353463200298033[6] = 0;
out_2589353463200298033[7] = 1;
out_2589353463200298033[8] = 0;
}
void h_27(double *state, double *unused, double *out_1247339187193410574) {
out_1247339187193410574[0] = state[3];
void h_27(double *state, double *unused, double *out_7012614271698209976) {
out_7012614271698209976[0] = state[3];
}
void H_27(double *state, double *unused, double *out_2614397886586431153) {
out_2614397886586431153[0] = 0;
out_2614397886586431153[1] = 0;
out_2614397886586431153[2] = 0;
out_2614397886586431153[3] = 1;
out_2614397886586431153[4] = 0;
out_2614397886586431153[5] = 0;
out_2614397886586431153[6] = 0;
out_2614397886586431153[7] = 0;
out_2614397886586431153[8] = 0;
void H_27(double *state, double *unused, double *out_5550309786254274918) {
out_5550309786254274918[0] = 0;
out_5550309786254274918[1] = 0;
out_5550309786254274918[2] = 0;
out_5550309786254274918[3] = 1;
out_5550309786254274918[4] = 0;
out_5550309786254274918[5] = 0;
out_5550309786254274918[6] = 0;
out_5550309786254274918[7] = 0;
out_5550309786254274918[8] = 0;
}
void h_29(double *state, double *unused, double *out_1705540549351972236) {
out_1705540549351972236[0] = state[1];
void h_29(double *state, double *unused, double *out_1627263205020331908) {
out_1627263205020331908[0] = state[1];
}
void H_29(double *state, double *unused, double *out_5299392542701248248) {
out_5299392542701248248[0] = 0;
out_5299392542701248248[1] = 1;
out_5299392542701248248[2] = 0;
out_5299392542701248248[3] = 0;
out_5299392542701248248[4] = 0;
out_5299392542701248248[5] = 0;
out_5299392542701248248[6] = 0;
out_5299392542701248248[7] = 0;
out_5299392542701248248[8] = 0;
void H_29(double *state, double *unused, double *out_2865315130139457823) {
out_2865315130139457823[0] = 0;
out_2865315130139457823[1] = 1;
out_2865315130139457823[2] = 0;
out_2865315130139457823[3] = 0;
out_2865315130139457823[4] = 0;
out_2865315130139457823[5] = 0;
out_2865315130139457823[6] = 0;
out_2865315130139457823[7] = 0;
out_2865315130139457823[8] = 0;
}
void h_28(double *state, double *unused, double *out_4170451758575142464) {
out_4170451758575142464[0] = state[0];
void h_28(double *state, double *unused, double *out_7994698365033508088) {
out_7994698365033508088[0] = state[0];
}
void H_28(double *state, double *unused, double *out_7263022814266574499) {
out_7263022814266574499[0] = 1;
out_7263022814266574499[1] = 0;
out_7263022814266574499[2] = 0;
out_7263022814266574499[3] = 0;
out_7263022814266574499[4] = 0;
out_7263022814266574499[5] = 0;
out_7263022814266574499[6] = 0;
out_7263022814266574499[7] = 0;
out_7263022814266574499[8] = 0;
void H_28(double *state, double *unused, double *out_901684858574131572) {
out_901684858574131572[0] = 1;
out_901684858574131572[1] = 0;
out_901684858574131572[2] = 0;
out_901684858574131572[3] = 0;
out_901684858574131572[4] = 0;
out_901684858574131572[5] = 0;
out_901684858574131572[6] = 0;
out_901684858574131572[7] = 0;
out_901684858574131572[8] = 0;
}
void h_31(double *state, double *unused, double *out_1514252801966416222) {
out_1514252801966416222[0] = state[8];
void h_31(double *state, double *unused, double *out_988808959918839917) {
out_988808959918839917[0] = state[8];
}
void H_31(double *state, double *unused, double *out_4949146107407056562) {
out_4949146107407056562[0] = 0;
out_4949146107407056562[1] = 0;
out_4949146107407056562[2] = 0;
out_4949146107407056562[3] = 0;
out_4949146107407056562[4] = 0;
out_4949146107407056562[5] = 0;
out_4949146107407056562[6] = 0;
out_4949146107407056562[7] = 0;
out_4949146107407056562[8] = 1;
void H_31(double *state, double *unused, double *out_3215561565433649509) {
out_3215561565433649509[0] = 0;
out_3215561565433649509[1] = 0;
out_3215561565433649509[2] = 0;
out_3215561565433649509[3] = 0;
out_3215561565433649509[4] = 0;
out_3215561565433649509[5] = 0;
out_3215561565433649509[6] = 0;
out_3215561565433649509[7] = 0;
out_3215561565433649509[8] = 1;
}
#include <eigen3/Eigen/Dense>
#include <iostream>
@@ -518,68 +518,68 @@ void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, doubl
void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
update<1, 3, 0>(in_x, in_P, h_31, H_31, NULL, in_z, in_R, in_ea, MAHA_THRESH_31);
}
void car_err_fun(double *nom_x, double *delta_x, double *out_5634330020296848070) {
err_fun(nom_x, delta_x, out_5634330020296848070);
void car_err_fun(double *nom_x, double *delta_x, double *out_9008082079438066370) {
err_fun(nom_x, delta_x, out_9008082079438066370);
}
void car_inv_err_fun(double *nom_x, double *true_x, double *out_1061559027503315281) {
inv_err_fun(nom_x, true_x, out_1061559027503315281);
void car_inv_err_fun(double *nom_x, double *true_x, double *out_7705390806256602072) {
inv_err_fun(nom_x, true_x, out_7705390806256602072);
}
void car_H_mod_fun(double *state, double *out_1567569310651922730) {
H_mod_fun(state, out_1567569310651922730);
void car_H_mod_fun(double *state, double *out_4044238657628669408) {
H_mod_fun(state, out_4044238657628669408);
}
void car_f_fun(double *state, double dt, double *out_1208381054458531187) {
f_fun(state, dt, out_1208381054458531187);
void car_f_fun(double *state, double dt, double *out_6587028471138044914) {
f_fun(state, dt, out_6587028471138044914);
}
void car_F_fun(double *state, double dt, double *out_8824004521572161416) {
F_fun(state, dt, out_8824004521572161416);
void car_F_fun(double *state, double dt, double *out_7531965082275181187) {
F_fun(state, dt, out_7531965082275181187);
}
void car_h_25(double *state, double *unused, double *out_1789446864250922111) {
h_25(state, unused, out_1789446864250922111);
void car_h_25(double *state, double *unused, double *out_7956124124599872411) {
h_25(state, unused, out_7956124124599872411);
}
void car_H_25(double *state, double *unused, double *out_9129886545195087354) {
H_25(state, unused, out_9129886545195087354);
void car_H_25(double *state, double *unused, double *out_1152149855673758191) {
H_25(state, unused, out_1152149855673758191);
}
void car_h_24(double *state, double *unused, double *out_3299160385053087312) {
h_24(state, unused, out_3299160385053087312);
void car_h_24(double *state, double *unused, double *out_8831563141601233713) {
h_24(state, unused, out_8831563141601233713);
}
void car_H_24(double *state, double *unused, double *out_7144207929508964696) {
H_24(state, unused, out_7144207929508964696);
void car_H_24(double *state, double *unused, double *out_1020499743331741375) {
H_24(state, unused, out_1020499743331741375);
}
void car_h_30(double *state, double *unused, double *out_7348270420432929071) {
h_30(state, unused, out_7348270420432929071);
void car_h_30(double *state, double *unused, double *out_6628936472681177553) {
h_30(state, unused, out_6628936472681177553);
}
void car_H_30(double *state, double *unused, double *out_6611553586687838727) {
H_30(state, unused, out_6611553586687838727);
void car_H_30(double *state, double *unused, double *out_3375546474453850007) {
H_30(state, unused, out_3375546474453850007);
}
void car_h_26(double *state, double *unused, double *out_6096800385615776900) {
h_26(state, unused, out_6096800385615776900);
void car_h_26(double *state, double *unused, double *out_3816384369411324170) {
h_26(state, unused, out_3816384369411324170);
}
void car_H_26(double *state, double *unused, double *out_5575354209640408038) {
H_26(state, unused, out_5575354209640408038);
void car_H_26(double *state, double *unused, double *out_2589353463200298033) {
H_26(state, unused, out_2589353463200298033);
}
void car_h_27(double *state, double *unused, double *out_1247339187193410574) {
h_27(state, unused, out_1247339187193410574);
void car_h_27(double *state, double *unused, double *out_7012614271698209976) {
h_27(state, unused, out_7012614271698209976);
}
void car_H_27(double *state, double *unused, double *out_2614397886586431153) {
H_27(state, unused, out_2614397886586431153);
void car_H_27(double *state, double *unused, double *out_5550309786254274918) {
H_27(state, unused, out_5550309786254274918);
}
void car_h_29(double *state, double *unused, double *out_1705540549351972236) {
h_29(state, unused, out_1705540549351972236);
void car_h_29(double *state, double *unused, double *out_1627263205020331908) {
h_29(state, unused, out_1627263205020331908);
}
void car_H_29(double *state, double *unused, double *out_5299392542701248248) {
H_29(state, unused, out_5299392542701248248);
void car_H_29(double *state, double *unused, double *out_2865315130139457823) {
H_29(state, unused, out_2865315130139457823);
}
void car_h_28(double *state, double *unused, double *out_4170451758575142464) {
h_28(state, unused, out_4170451758575142464);
void car_h_28(double *state, double *unused, double *out_7994698365033508088) {
h_28(state, unused, out_7994698365033508088);
}
void car_H_28(double *state, double *unused, double *out_7263022814266574499) {
H_28(state, unused, out_7263022814266574499);
void car_H_28(double *state, double *unused, double *out_901684858574131572) {
H_28(state, unused, out_901684858574131572);
}
void car_h_31(double *state, double *unused, double *out_1514252801966416222) {
h_31(state, unused, out_1514252801966416222);
void car_h_31(double *state, double *unused, double *out_988808959918839917) {
h_31(state, unused, out_988808959918839917);
}
void car_H_31(double *state, double *unused, double *out_4949146107407056562) {
H_31(state, unused, out_4949146107407056562);
void car_H_31(double *state, double *unused, double *out_3215561565433649509) {
H_31(state, unused, out_3215561565433649509);
}
void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
predict(in_x, in_P, in_Q, dt);
selfdrive/locationd/models/generated/car.h
+21 -21
View File
@@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl
void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void car_err_fun(double *nom_x, double *delta_x, double *out_5634330020296848070);
void car_inv_err_fun(double *nom_x, double *true_x, double *out_1061559027503315281);
void car_H_mod_fun(double *state, double *out_1567569310651922730);
void car_f_fun(double *state, double dt, double *out_1208381054458531187);
void car_F_fun(double *state, double dt, double *out_8824004521572161416);
void car_h_25(double *state, double *unused, double *out_1789446864250922111);
void car_H_25(double *state, double *unused, double *out_9129886545195087354);
void car_h_24(double *state, double *unused, double *out_3299160385053087312);
void car_H_24(double *state, double *unused, double *out_7144207929508964696);
void car_h_30(double *state, double *unused, double *out_7348270420432929071);
void car_H_30(double *state, double *unused, double *out_6611553586687838727);
void car_h_26(double *state, double *unused, double *out_6096800385615776900);
void car_H_26(double *state, double *unused, double *out_5575354209640408038);
void car_h_27(double *state, double *unused, double *out_1247339187193410574);
void car_H_27(double *state, double *unused, double *out_2614397886586431153);
void car_h_29(double *state, double *unused, double *out_1705540549351972236);
void car_H_29(double *state, double *unused, double *out_5299392542701248248);
void car_h_28(double *state, double *unused, double *out_4170451758575142464);
void car_H_28(double *state, double *unused, double *out_7263022814266574499);
void car_h_31(double *state, double *unused, double *out_1514252801966416222);
void car_H_31(double *state, double *unused, double *out_4949146107407056562);
void car_err_fun(double *nom_x, double *delta_x, double *out_9008082079438066370);
void car_inv_err_fun(double *nom_x, double *true_x, double *out_7705390806256602072);
void car_H_mod_fun(double *state, double *out_4044238657628669408);
void car_f_fun(double *state, double dt, double *out_6587028471138044914);
void car_F_fun(double *state, double dt, double *out_7531965082275181187);
void car_h_25(double *state, double *unused, double *out_7956124124599872411);
void car_H_25(double *state, double *unused, double *out_1152149855673758191);
void car_h_24(double *state, double *unused, double *out_8831563141601233713);
void car_H_24(double *state, double *unused, double *out_1020499743331741375);
void car_h_30(double *state, double *unused, double *out_6628936472681177553);
void car_H_30(double *state, double *unused, double *out_3375546474453850007);
void car_h_26(double *state, double *unused, double *out_3816384369411324170);
void car_H_26(double *state, double *unused, double *out_2589353463200298033);
void car_h_27(double *state, double *unused, double *out_7012614271698209976);
void car_H_27(double *state, double *unused, double *out_5550309786254274918);
void car_h_29(double *state, double *unused, double *out_1627263205020331908);
void car_H_29(double *state, double *unused, double *out_2865315130139457823);
void car_h_28(double *state, double *unused, double *out_7994698365033508088);
void car_H_28(double *state, double *unused, double *out_901684858574131572);
void car_h_31(double *state, double *unused, double *out_988808959918839917);
void car_H_31(double *state, double *unused, double *out_3215561565433649509);
void car_predict(double *in_x, double *in_P, double *in_Q, double dt);
void car_set_mass(double x);
void car_set_rotational_inertia(double x);
selfdrive/locationd/models/generated/gnss.cpp
+362 -362
View File
@@ -17,354 +17,354 @@ const static double MAHA_THRESH_21 = 3.8414588206941227;
* *
* This file is part of 'ekf' *
******************************************************************************/
void err_fun(double *nom_x, double *delta_x, double *out_8218999594846759964) {
out_8218999594846759964[0] = delta_x[0] + nom_x[0];
out_8218999594846759964[1] = delta_x[1] + nom_x[1];
out_8218999594846759964[2] = delta_x[2] + nom_x[2];
out_8218999594846759964[3] = delta_x[3] + nom_x[3];
out_8218999594846759964[4] = delta_x[4] + nom_x[4];
out_8218999594846759964[5] = delta_x[5] + nom_x[5];
out_8218999594846759964[6] = delta_x[6] + nom_x[6];
out_8218999594846759964[7] = delta_x[7] + nom_x[7];
out_8218999594846759964[8] = delta_x[8] + nom_x[8];
out_8218999594846759964[9] = delta_x[9] + nom_x[9];
out_8218999594846759964[10] = delta_x[10] + nom_x[10];
void err_fun(double *nom_x, double *delta_x, double *out_9093778295671839447) {
out_9093778295671839447[0] = delta_x[0] + nom_x[0];
out_9093778295671839447[1] = delta_x[1] + nom_x[1];
out_9093778295671839447[2] = delta_x[2] + nom_x[2];
out_9093778295671839447[3] = delta_x[3] + nom_x[3];
out_9093778295671839447[4] = delta_x[4] + nom_x[4];
out_9093778295671839447[5] = delta_x[5] + nom_x[5];
out_9093778295671839447[6] = delta_x[6] + nom_x[6];
out_9093778295671839447[7] = delta_x[7] + nom_x[7];
out_9093778295671839447[8] = delta_x[8] + nom_x[8];
out_9093778295671839447[9] = delta_x[9] + nom_x[9];
out_9093778295671839447[10] = delta_x[10] + nom_x[10];
}
void inv_err_fun(double *nom_x, double *true_x, double *out_6004490369099356165) {
out_6004490369099356165[0] = -nom_x[0] + true_x[0];
out_6004490369099356165[1] = -nom_x[1] + true_x[1];
out_6004490369099356165[2] = -nom_x[2] + true_x[2];
out_6004490369099356165[3] = -nom_x[3] + true_x[3];
out_6004490369099356165[4] = -nom_x[4] + true_x[4];
out_6004490369099356165[5] = -nom_x[5] + true_x[5];
out_6004490369099356165[6] = -nom_x[6] + true_x[6];
out_6004490369099356165[7] = -nom_x[7] + true_x[7];
out_6004490369099356165[8] = -nom_x[8] + true_x[8];
out_6004490369099356165[9] = -nom_x[9] + true_x[9];
out_6004490369099356165[10] = -nom_x[10] + true_x[10];
void inv_err_fun(double *nom_x, double *true_x, double *out_9126699124174503695) {
out_9126699124174503695[0] = -nom_x[0] + true_x[0];
out_9126699124174503695[1] = -nom_x[1] + true_x[1];
out_9126699124174503695[2] = -nom_x[2] + true_x[2];
out_9126699124174503695[3] = -nom_x[3] + true_x[3];
out_9126699124174503695[4] = -nom_x[4] + true_x[4];
out_9126699124174503695[5] = -nom_x[5] + true_x[5];
out_9126699124174503695[6] = -nom_x[6] + true_x[6];
out_9126699124174503695[7] = -nom_x[7] + true_x[7];
out_9126699124174503695[8] = -nom_x[8] + true_x[8];
out_9126699124174503695[9] = -nom_x[9] + true_x[9];
out_9126699124174503695[10] = -nom_x[10] + true_x[10];
}
void H_mod_fun(double *state, double *out_5465143194872640210) {
out_5465143194872640210[0] = 1.0;
out_5465143194872640210[1] = 0;
out_5465143194872640210[2] = 0;
out_5465143194872640210[3] = 0;
out_5465143194872640210[4] = 0;
out_5465143194872640210[5] = 0;
out_5465143194872640210[6] = 0;
out_5465143194872640210[7] = 0;
out_5465143194872640210[8] = 0;
out_5465143194872640210[9] = 0;
out_5465143194872640210[10] = 0;
out_5465143194872640210[11] = 0;
out_5465143194872640210[12] = 1.0;
out_5465143194872640210[13] = 0;
out_5465143194872640210[14] = 0;
out_5465143194872640210[15] = 0;
out_5465143194872640210[16] = 0;
out_5465143194872640210[17] = 0;
out_5465143194872640210[18] = 0;
out_5465143194872640210[19] = 0;
out_5465143194872640210[20] = 0;
out_5465143194872640210[21] = 0;
out_5465143194872640210[22] = 0;
out_5465143194872640210[23] = 0;
out_5465143194872640210[24] = 1.0;
out_5465143194872640210[25] = 0;
out_5465143194872640210[26] = 0;
out_5465143194872640210[27] = 0;
out_5465143194872640210[28] = 0;
out_5465143194872640210[29] = 0;
out_5465143194872640210[30] = 0;
out_5465143194872640210[31] = 0;
out_5465143194872640210[32] = 0;
out_5465143194872640210[33] = 0;
out_5465143194872640210[34] = 0;
out_5465143194872640210[35] = 0;
out_5465143194872640210[36] = 1.0;
out_5465143194872640210[37] = 0;
out_5465143194872640210[38] = 0;
out_5465143194872640210[39] = 0;
out_5465143194872640210[40] = 0;
out_5465143194872640210[41] = 0;
out_5465143194872640210[42] = 0;
out_5465143194872640210[43] = 0;
out_5465143194872640210[44] = 0;
out_5465143194872640210[45] = 0;
out_5465143194872640210[46] = 0;
out_5465143194872640210[47] = 0;
out_5465143194872640210[48] = 1.0;
out_5465143194872640210[49] = 0;
out_5465143194872640210[50] = 0;
out_5465143194872640210[51] = 0;
out_5465143194872640210[52] = 0;
out_5465143194872640210[53] = 0;
out_5465143194872640210[54] = 0;
out_5465143194872640210[55] = 0;
out_5465143194872640210[56] = 0;
out_5465143194872640210[57] = 0;
out_5465143194872640210[58] = 0;
out_5465143194872640210[59] = 0;
out_5465143194872640210[60] = 1.0;
out_5465143194872640210[61] = 0;
out_5465143194872640210[62] = 0;
out_5465143194872640210[63] = 0;
out_5465143194872640210[64] = 0;
out_5465143194872640210[65] = 0;
out_5465143194872640210[66] = 0;
out_5465143194872640210[67] = 0;
out_5465143194872640210[68] = 0;
out_5465143194872640210[69] = 0;
out_5465143194872640210[70] = 0;
out_5465143194872640210[71] = 0;
out_5465143194872640210[72] = 1.0;
out_5465143194872640210[73] = 0;
out_5465143194872640210[74] = 0;
out_5465143194872640210[75] = 0;
out_5465143194872640210[76] = 0;
out_5465143194872640210[77] = 0;
out_5465143194872640210[78] = 0;
out_5465143194872640210[79] = 0;
out_5465143194872640210[80] = 0;
out_5465143194872640210[81] = 0;
out_5465143194872640210[82] = 0;
out_5465143194872640210[83] = 0;
out_5465143194872640210[84] = 1.0;
out_5465143194872640210[85] = 0;
out_5465143194872640210[86] = 0;
out_5465143194872640210[87] = 0;
out_5465143194872640210[88] = 0;
out_5465143194872640210[89] = 0;
out_5465143194872640210[90] = 0;
out_5465143194872640210[91] = 0;
out_5465143194872640210[92] = 0;
out_5465143194872640210[93] = 0;
out_5465143194872640210[94] = 0;
out_5465143194872640210[95] = 0;
out_5465143194872640210[96] = 1.0;
out_5465143194872640210[97] = 0;
out_5465143194872640210[98] = 0;
out_5465143194872640210[99] = 0;
out_5465143194872640210[100] = 0;
out_5465143194872640210[101] = 0;
out_5465143194872640210[102] = 0;
out_5465143194872640210[103] = 0;
out_5465143194872640210[104] = 0;
out_5465143194872640210[105] = 0;
out_5465143194872640210[106] = 0;
out_5465143194872640210[107] = 0;
out_5465143194872640210[108] = 1.0;
out_5465143194872640210[109] = 0;
out_5465143194872640210[110] = 0;
out_5465143194872640210[111] = 0;
out_5465143194872640210[112] = 0;
out_5465143194872640210[113] = 0;
out_5465143194872640210[114] = 0;
out_5465143194872640210[115] = 0;
out_5465143194872640210[116] = 0;
out_5465143194872640210[117] = 0;
out_5465143194872640210[118] = 0;
out_5465143194872640210[119] = 0;
out_5465143194872640210[120] = 1.0;
void H_mod_fun(double *state, double *out_854507982997558257) {
out_854507982997558257[0] = 1.0;
out_854507982997558257[1] = 0;
out_854507982997558257[2] = 0;
out_854507982997558257[3] = 0;
out_854507982997558257[4] = 0;
out_854507982997558257[5] = 0;
out_854507982997558257[6] = 0;
out_854507982997558257[7] = 0;
out_854507982997558257[8] = 0;
out_854507982997558257[9] = 0;
out_854507982997558257[10] = 0;
out_854507982997558257[11] = 0;
out_854507982997558257[12] = 1.0;
out_854507982997558257[13] = 0;
out_854507982997558257[14] = 0;
out_854507982997558257[15] = 0;
out_854507982997558257[16] = 0;
out_854507982997558257[17] = 0;
out_854507982997558257[18] = 0;
out_854507982997558257[19] = 0;
out_854507982997558257[20] = 0;
out_854507982997558257[21] = 0;
out_854507982997558257[22] = 0;
out_854507982997558257[23] = 0;
out_854507982997558257[24] = 1.0;
out_854507982997558257[25] = 0;
out_854507982997558257[26] = 0;
out_854507982997558257[27] = 0;
out_854507982997558257[28] = 0;
out_854507982997558257[29] = 0;
out_854507982997558257[30] = 0;
out_854507982997558257[31] = 0;
out_854507982997558257[32] = 0;
out_854507982997558257[33] = 0;
out_854507982997558257[34] = 0;
out_854507982997558257[35] = 0;
out_854507982997558257[36] = 1.0;
out_854507982997558257[37] = 0;
out_854507982997558257[38] = 0;
out_854507982997558257[39] = 0;
out_854507982997558257[40] = 0;
out_854507982997558257[41] = 0;
out_854507982997558257[42] = 0;
out_854507982997558257[43] = 0;
out_854507982997558257[44] = 0;
out_854507982997558257[45] = 0;
out_854507982997558257[46] = 0;
out_854507982997558257[47] = 0;
out_854507982997558257[48] = 1.0;
out_854507982997558257[49] = 0;
out_854507982997558257[50] = 0;
out_854507982997558257[51] = 0;
out_854507982997558257[52] = 0;
out_854507982997558257[53] = 0;
out_854507982997558257[54] = 0;
out_854507982997558257[55] = 0;
out_854507982997558257[56] = 0;
out_854507982997558257[57] = 0;
out_854507982997558257[58] = 0;
out_854507982997558257[59] = 0;
out_854507982997558257[60] = 1.0;
out_854507982997558257[61] = 0;
out_854507982997558257[62] = 0;
out_854507982997558257[63] = 0;
out_854507982997558257[64] = 0;
out_854507982997558257[65] = 0;
out_854507982997558257[66] = 0;
out_854507982997558257[67] = 0;
out_854507982997558257[68] = 0;
out_854507982997558257[69] = 0;
out_854507982997558257[70] = 0;
out_854507982997558257[71] = 0;
out_854507982997558257[72] = 1.0;
out_854507982997558257[73] = 0;
out_854507982997558257[74] = 0;
out_854507982997558257[75] = 0;
out_854507982997558257[76] = 0;
out_854507982997558257[77] = 0;
out_854507982997558257[78] = 0;
out_854507982997558257[79] = 0;
out_854507982997558257[80] = 0;
out_854507982997558257[81] = 0;
out_854507982997558257[82] = 0;
out_854507982997558257[83] = 0;
out_854507982997558257[84] = 1.0;
out_854507982997558257[85] = 0;
out_854507982997558257[86] = 0;
out_854507982997558257[87] = 0;
out_854507982997558257[88] = 0;
out_854507982997558257[89] = 0;
out_854507982997558257[90] = 0;
out_854507982997558257[91] = 0;
out_854507982997558257[92] = 0;
out_854507982997558257[93] = 0;
out_854507982997558257[94] = 0;
out_854507982997558257[95] = 0;
out_854507982997558257[96] = 1.0;
out_854507982997558257[97] = 0;
out_854507982997558257[98] = 0;
out_854507982997558257[99] = 0;
out_854507982997558257[100] = 0;
out_854507982997558257[101] = 0;
out_854507982997558257[102] = 0;
out_854507982997558257[103] = 0;
out_854507982997558257[104] = 0;
out_854507982997558257[105] = 0;
out_854507982997558257[106] = 0;
out_854507982997558257[107] = 0;
out_854507982997558257[108] = 1.0;
out_854507982997558257[109] = 0;
out_854507982997558257[110] = 0;
out_854507982997558257[111] = 0;
out_854507982997558257[112] = 0;
out_854507982997558257[113] = 0;
out_854507982997558257[114] = 0;
out_854507982997558257[115] = 0;
out_854507982997558257[116] = 0;
out_854507982997558257[117] = 0;
out_854507982997558257[118] = 0;
out_854507982997558257[119] = 0;
out_854507982997558257[120] = 1.0;
}
void f_fun(double *state, double dt, double *out_6477589900664112980) {
out_6477589900664112980[0] = dt*state[3] + state[0];
out_6477589900664112980[1] = dt*state[4] + state[1];
out_6477589900664112980[2] = dt*state[5] + state[2];
out_6477589900664112980[3] = state[3];
out_6477589900664112980[4] = state[4];
out_6477589900664112980[5] = state[5];
out_6477589900664112980[6] = dt*state[7] + state[6];
out_6477589900664112980[7] = dt*state[8] + state[7];
out_6477589900664112980[8] = state[8];
out_6477589900664112980[9] = state[9];
out_6477589900664112980[10] = state[10];
void f_fun(double *state, double dt, double *out_7770403001027613956) {
out_7770403001027613956[0] = dt*state[3] + state[0];
out_7770403001027613956[1] = dt*state[4] + state[1];
out_7770403001027613956[2] = dt*state[5] + state[2];
out_7770403001027613956[3] = state[3];
out_7770403001027613956[4] = state[4];
out_7770403001027613956[5] = state[5];
out_7770403001027613956[6] = dt*state[7] + state[6];
out_7770403001027613956[7] = dt*state[8] + state[7];
out_7770403001027613956[8] = state[8];
out_7770403001027613956[9] = state[9];
out_7770403001027613956[10] = state[10];
}
void F_fun(double *state, double dt, double *out_159332577828599777) {
out_159332577828599777[0] = 1;
out_159332577828599777[1] = 0;
out_159332577828599777[2] = 0;
out_159332577828599777[3] = dt;
out_159332577828599777[4] = 0;
out_159332577828599777[5] = 0;
out_159332577828599777[6] = 0;
out_159332577828599777[7] = 0;
out_159332577828599777[8] = 0;
out_159332577828599777[9] = 0;
out_159332577828599777[10] = 0;
out_159332577828599777[11] = 0;
out_159332577828599777[12] = 1;
out_159332577828599777[13] = 0;
out_159332577828599777[14] = 0;
out_159332577828599777[15] = dt;
out_159332577828599777[16] = 0;
out_159332577828599777[17] = 0;
out_159332577828599777[18] = 0;
out_159332577828599777[19] = 0;
out_159332577828599777[20] = 0;
out_159332577828599777[21] = 0;
out_159332577828599777[22] = 0;
out_159332577828599777[23] = 0;
out_159332577828599777[24] = 1;
out_159332577828599777[25] = 0;
out_159332577828599777[26] = 0;
out_159332577828599777[27] = dt;
out_159332577828599777[28] = 0;
out_159332577828599777[29] = 0;
out_159332577828599777[30] = 0;
out_159332577828599777[31] = 0;
out_159332577828599777[32] = 0;
out_159332577828599777[33] = 0;
out_159332577828599777[34] = 0;
out_159332577828599777[35] = 0;
out_159332577828599777[36] = 1;
out_159332577828599777[37] = 0;
out_159332577828599777[38] = 0;
out_159332577828599777[39] = 0;
out_159332577828599777[40] = 0;
out_159332577828599777[41] = 0;
out_159332577828599777[42] = 0;
out_159332577828599777[43] = 0;
out_159332577828599777[44] = 0;
out_159332577828599777[45] = 0;
out_159332577828599777[46] = 0;
out_159332577828599777[47] = 0;
out_159332577828599777[48] = 1;
out_159332577828599777[49] = 0;
out_159332577828599777[50] = 0;
out_159332577828599777[51] = 0;
out_159332577828599777[52] = 0;
out_159332577828599777[53] = 0;
out_159332577828599777[54] = 0;
out_159332577828599777[55] = 0;
out_159332577828599777[56] = 0;
out_159332577828599777[57] = 0;
out_159332577828599777[58] = 0;
out_159332577828599777[59] = 0;
out_159332577828599777[60] = 1;
out_159332577828599777[61] = 0;
out_159332577828599777[62] = 0;
out_159332577828599777[63] = 0;
out_159332577828599777[64] = 0;
out_159332577828599777[65] = 0;
out_159332577828599777[66] = 0;
out_159332577828599777[67] = 0;
out_159332577828599777[68] = 0;
out_159332577828599777[69] = 0;
out_159332577828599777[70] = 0;
out_159332577828599777[71] = 0;
out_159332577828599777[72] = 1;
out_159332577828599777[73] = dt;
out_159332577828599777[74] = 0;
out_159332577828599777[75] = 0;
out_159332577828599777[76] = 0;
out_159332577828599777[77] = 0;
out_159332577828599777[78] = 0;
out_159332577828599777[79] = 0;
out_159332577828599777[80] = 0;
out_159332577828599777[81] = 0;
out_159332577828599777[82] = 0;
out_159332577828599777[83] = 0;
out_159332577828599777[84] = 1;
out_159332577828599777[85] = dt;
out_159332577828599777[86] = 0;
out_159332577828599777[87] = 0;
out_159332577828599777[88] = 0;
out_159332577828599777[89] = 0;
out_159332577828599777[90] = 0;
out_159332577828599777[91] = 0;
out_159332577828599777[92] = 0;
out_159332577828599777[93] = 0;
out_159332577828599777[94] = 0;
out_159332577828599777[95] = 0;
out_159332577828599777[96] = 1;
out_159332577828599777[97] = 0;
out_159332577828599777[98] = 0;
out_159332577828599777[99] = 0;
out_159332577828599777[100] = 0;
out_159332577828599777[101] = 0;
out_159332577828599777[102] = 0;
out_159332577828599777[103] = 0;
out_159332577828599777[104] = 0;
out_159332577828599777[105] = 0;
out_159332577828599777[106] = 0;
out_159332577828599777[107] = 0;
out_159332577828599777[108] = 1;
out_159332577828599777[109] = 0;
out_159332577828599777[110] = 0;
out_159332577828599777[111] = 0;
out_159332577828599777[112] = 0;
out_159332577828599777[113] = 0;
out_159332577828599777[114] = 0;
out_159332577828599777[115] = 0;
out_159332577828599777[116] = 0;
out_159332577828599777[117] = 0;
out_159332577828599777[118] = 0;
out_159332577828599777[119] = 0;
out_159332577828599777[120] = 1;
void F_fun(double *state, double dt, double *out_7798555812212859819) {
out_7798555812212859819[0] = 1;
out_7798555812212859819[1] = 0;
out_7798555812212859819[2] = 0;
out_7798555812212859819[3] = dt;
out_7798555812212859819[4] = 0;
out_7798555812212859819[5] = 0;
out_7798555812212859819[6] = 0;
out_7798555812212859819[7] = 0;
out_7798555812212859819[8] = 0;
out_7798555812212859819[9] = 0;
out_7798555812212859819[10] = 0;
out_7798555812212859819[11] = 0;
out_7798555812212859819[12] = 1;
out_7798555812212859819[13] = 0;
out_7798555812212859819[14] = 0;
out_7798555812212859819[15] = dt;
out_7798555812212859819[16] = 0;
out_7798555812212859819[17] = 0;
out_7798555812212859819[18] = 0;
out_7798555812212859819[19] = 0;
out_7798555812212859819[20] = 0;
out_7798555812212859819[21] = 0;
out_7798555812212859819[22] = 0;
out_7798555812212859819[23] = 0;
out_7798555812212859819[24] = 1;
out_7798555812212859819[25] = 0;
out_7798555812212859819[26] = 0;
out_7798555812212859819[27] = dt;
out_7798555812212859819[28] = 0;
out_7798555812212859819[29] = 0;
out_7798555812212859819[30] = 0;
out_7798555812212859819[31] = 0;
out_7798555812212859819[32] = 0;
out_7798555812212859819[33] = 0;
out_7798555812212859819[34] = 0;
out_7798555812212859819[35] = 0;
out_7798555812212859819[36] = 1;
out_7798555812212859819[37] = 0;
out_7798555812212859819[38] = 0;
out_7798555812212859819[39] = 0;
out_7798555812212859819[40] = 0;
out_7798555812212859819[41] = 0;
out_7798555812212859819[42] = 0;
out_7798555812212859819[43] = 0;
out_7798555812212859819[44] = 0;
out_7798555812212859819[45] = 0;
out_7798555812212859819[46] = 0;
out_7798555812212859819[47] = 0;
out_7798555812212859819[48] = 1;
out_7798555812212859819[49] = 0;
out_7798555812212859819[50] = 0;
out_7798555812212859819[51] = 0;
out_7798555812212859819[52] = 0;
out_7798555812212859819[53] = 0;
out_7798555812212859819[54] = 0;
out_7798555812212859819[55] = 0;
out_7798555812212859819[56] = 0;
out_7798555812212859819[57] = 0;
out_7798555812212859819[58] = 0;
out_7798555812212859819[59] = 0;
out_7798555812212859819[60] = 1;
out_7798555812212859819[61] = 0;
out_7798555812212859819[62] = 0;
out_7798555812212859819[63] = 0;
out_7798555812212859819[64] = 0;
out_7798555812212859819[65] = 0;
out_7798555812212859819[66] = 0;
out_7798555812212859819[67] = 0;
out_7798555812212859819[68] = 0;
out_7798555812212859819[69] = 0;
out_7798555812212859819[70] = 0;
out_7798555812212859819[71] = 0;
out_7798555812212859819[72] = 1;
out_7798555812212859819[73] = dt;
out_7798555812212859819[74] = 0;
out_7798555812212859819[75] = 0;
out_7798555812212859819[76] = 0;
out_7798555812212859819[77] = 0;
out_7798555812212859819[78] = 0;
out_7798555812212859819[79] = 0;
out_7798555812212859819[80] = 0;
out_7798555812212859819[81] = 0;
out_7798555812212859819[82] = 0;
out_7798555812212859819[83] = 0;
out_7798555812212859819[84] = 1;
out_7798555812212859819[85] = dt;
out_7798555812212859819[86] = 0;
out_7798555812212859819[87] = 0;
out_7798555812212859819[88] = 0;
out_7798555812212859819[89] = 0;
out_7798555812212859819[90] = 0;
out_7798555812212859819[91] = 0;
out_7798555812212859819[92] = 0;
out_7798555812212859819[93] = 0;
out_7798555812212859819[94] = 0;
out_7798555812212859819[95] = 0;
out_7798555812212859819[96] = 1;
out_7798555812212859819[97] = 0;
out_7798555812212859819[98] = 0;
out_7798555812212859819[99] = 0;
out_7798555812212859819[100] = 0;
out_7798555812212859819[101] = 0;
out_7798555812212859819[102] = 0;
out_7798555812212859819[103] = 0;
out_7798555812212859819[104] = 0;
out_7798555812212859819[105] = 0;
out_7798555812212859819[106] = 0;
out_7798555812212859819[107] = 0;
out_7798555812212859819[108] = 1;
out_7798555812212859819[109] = 0;
out_7798555812212859819[110] = 0;
out_7798555812212859819[111] = 0;
out_7798555812212859819[112] = 0;
out_7798555812212859819[113] = 0;
out_7798555812212859819[114] = 0;
out_7798555812212859819[115] = 0;
out_7798555812212859819[116] = 0;
out_7798555812212859819[117] = 0;
out_7798555812212859819[118] = 0;
out_7798555812212859819[119] = 0;
out_7798555812212859819[120] = 1;
}
void h_6(double *state, double *sat_pos, double *out_822185378282268145) {
out_822185378282268145[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + state[6];
void h_6(double *state, double *sat_pos, double *out_9201175564716128809) {
out_9201175564716128809[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + state[6];
}
void H_6(double *state, double *sat_pos, double *out_2954217689973778176) {
out_2954217689973778176[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_2954217689973778176[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_2954217689973778176[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_2954217689973778176[3] = 0;
out_2954217689973778176[4] = 0;
out_2954217689973778176[5] = 0;
out_2954217689973778176[6] = 1;
out_2954217689973778176[7] = 0;
out_2954217689973778176[8] = 0;
out_2954217689973778176[9] = 0;
out_2954217689973778176[10] = 0;
void H_6(double *state, double *sat_pos, double *out_1419005819748336970) {
out_1419005819748336970[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_1419005819748336970[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_1419005819748336970[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_1419005819748336970[3] = 0;
out_1419005819748336970[4] = 0;
out_1419005819748336970[5] = 0;
out_1419005819748336970[6] = 1;
out_1419005819748336970[7] = 0;
out_1419005819748336970[8] = 0;
out_1419005819748336970[9] = 0;
out_1419005819748336970[10] = 0;
}
void h_20(double *state, double *sat_pos, double *out_3775340380998125465) {
out_3775340380998125465[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + sat_pos[3]*state[10] + state[6] + state[9];
void h_20(double *state, double *sat_pos, double *out_8590157039648027835) {
out_8590157039648027835[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + sat_pos[3]*state[10] + state[6] + state[9];
}
void H_20(double *state, double *sat_pos, double *out_4124080333257632986) {
out_4124080333257632986[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_4124080333257632986[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_4124080333257632986[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_4124080333257632986[3] = 0;
out_4124080333257632986[4] = 0;
out_4124080333257632986[5] = 0;
out_4124080333257632986[6] = 1;
out_4124080333257632986[7] = 0;
out_4124080333257632986[8] = 0;
out_4124080333257632986[9] = 1;
out_4124080333257632986[10] = sat_pos[3];
void H_20(double *state, double *sat_pos, double *out_5617129456073796795) {
out_5617129456073796795[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_5617129456073796795[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_5617129456073796795[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_5617129456073796795[3] = 0;
out_5617129456073796795[4] = 0;
out_5617129456073796795[5] = 0;
out_5617129456073796795[6] = 1;
out_5617129456073796795[7] = 0;
out_5617129456073796795[8] = 0;
out_5617129456073796795[9] = 1;
out_5617129456073796795[10] = sat_pos[3];
}
void h_7(double *state, double *sat_pos_vel, double *out_750344228235882460) {
out_750344228235882460[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
void h_7(double *state, double *sat_pos_vel, double *out_8069802722407087087) {
out_8069802722407087087[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
}
void H_7(double *state, double *sat_pos_vel, double *out_1618262330099536806) {
out_1618262330099536806[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_1618262330099536806[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_1618262330099536806[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_1618262330099536806[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_1618262330099536806[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_1618262330099536806[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_1618262330099536806[6] = 0;
out_1618262330099536806[7] = 1;
out_1618262330099536806[8] = 0;
out_1618262330099536806[9] = 0;
out_1618262330099536806[10] = 0;
void H_7(double *state, double *sat_pos_vel, double *out_7177713928341367106) {
out_7177713928341367106[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7177713928341367106[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7177713928341367106[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7177713928341367106[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7177713928341367106[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7177713928341367106[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7177713928341367106[6] = 0;
out_7177713928341367106[7] = 1;
out_7177713928341367106[8] = 0;
out_7177713928341367106[9] = 0;
out_7177713928341367106[10] = 0;
}
void h_21(double *state, double *sat_pos_vel, double *out_750344228235882460) {
out_750344228235882460[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
void h_21(double *state, double *sat_pos_vel, double *out_8069802722407087087) {
out_8069802722407087087[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
}
void H_21(double *state, double *sat_pos_vel, double *out_1618262330099536806) {
out_1618262330099536806[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_1618262330099536806[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_1618262330099536806[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_1618262330099536806[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_1618262330099536806[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_1618262330099536806[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_1618262330099536806[6] = 0;
out_1618262330099536806[7] = 1;
out_1618262330099536806[8] = 0;
out_1618262330099536806[9] = 0;
out_1618262330099536806[10] = 0;
void H_21(double *state, double *sat_pos_vel, double *out_7177713928341367106) {
out_7177713928341367106[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7177713928341367106[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7177713928341367106[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7177713928341367106[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7177713928341367106[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7177713928341367106[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7177713928341367106[6] = 0;
out_7177713928341367106[7] = 1;
out_7177713928341367106[8] = 0;
out_7177713928341367106[9] = 0;
out_7177713928341367106[10] = 0;
}
#include <eigen3/Eigen/Dense>
#include <iostream>
@@ -506,44 +506,44 @@ void gnss_update_7(double *in_x, double *in_P, double *in_z, double *in_R, doubl
void gnss_update_21(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
update<1, 3, 0>(in_x, in_P, h_21, H_21, NULL, in_z, in_R, in_ea, MAHA_THRESH_21);
}
void gnss_err_fun(double *nom_x, double *delta_x, double *out_8218999594846759964) {
err_fun(nom_x, delta_x, out_8218999594846759964);
void gnss_err_fun(double *nom_x, double *delta_x, double *out_9093778295671839447) {
err_fun(nom_x, delta_x, out_9093778295671839447);
}
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_6004490369099356165) {
inv_err_fun(nom_x, true_x, out_6004490369099356165);
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_9126699124174503695) {
inv_err_fun(nom_x, true_x, out_9126699124174503695);
}
void gnss_H_mod_fun(double *state, double *out_5465143194872640210) {
H_mod_fun(state, out_5465143194872640210);
void gnss_H_mod_fun(double *state, double *out_854507982997558257) {
H_mod_fun(state, out_854507982997558257);
}
void gnss_f_fun(double *state, double dt, double *out_6477589900664112980) {
f_fun(state, dt, out_6477589900664112980);
void gnss_f_fun(double *state, double dt, double *out_7770403001027613956) {
f_fun(state, dt, out_7770403001027613956);
}
void gnss_F_fun(double *state, double dt, double *out_159332577828599777) {
F_fun(state, dt, out_159332577828599777);
void gnss_F_fun(double *state, double dt, double *out_7798555812212859819) {
F_fun(state, dt, out_7798555812212859819);
}
void gnss_h_6(double *state, double *sat_pos, double *out_822185378282268145) {
h_6(state, sat_pos, out_822185378282268145);
void gnss_h_6(double *state, double *sat_pos, double *out_9201175564716128809) {
h_6(state, sat_pos, out_9201175564716128809);
}
void gnss_H_6(double *state, double *sat_pos, double *out_2954217689973778176) {
H_6(state, sat_pos, out_2954217689973778176);
void gnss_H_6(double *state, double *sat_pos, double *out_1419005819748336970) {
H_6(state, sat_pos, out_1419005819748336970);
}
void gnss_h_20(double *state, double *sat_pos, double *out_3775340380998125465) {
h_20(state, sat_pos, out_3775340380998125465);
void gnss_h_20(double *state, double *sat_pos, double *out_8590157039648027835) {
h_20(state, sat_pos, out_8590157039648027835);
}
void gnss_H_20(double *state, double *sat_pos, double *out_4124080333257632986) {
H_20(state, sat_pos, out_4124080333257632986);
void gnss_H_20(double *state, double *sat_pos, double *out_5617129456073796795) {
H_20(state, sat_pos, out_5617129456073796795);
}
void gnss_h_7(double *state, double *sat_pos_vel, double *out_750344228235882460) {
h_7(state, sat_pos_vel, out_750344228235882460);
void gnss_h_7(double *state, double *sat_pos_vel, double *out_8069802722407087087) {
h_7(state, sat_pos_vel, out_8069802722407087087);
}
void gnss_H_7(double *state, double *sat_pos_vel, double *out_1618262330099536806) {
H_7(state, sat_pos_vel, out_1618262330099536806);
void gnss_H_7(double *state, double *sat_pos_vel, double *out_7177713928341367106) {
H_7(state, sat_pos_vel, out_7177713928341367106);
}
void gnss_h_21(double *state, double *sat_pos_vel, double *out_750344228235882460) {
h_21(state, sat_pos_vel, out_750344228235882460);
void gnss_h_21(double *state, double *sat_pos_vel, double *out_8069802722407087087) {
h_21(state, sat_pos_vel, out_8069802722407087087);
}
void gnss_H_21(double *state, double *sat_pos_vel, double *out_1618262330099536806) {
H_21(state, sat_pos_vel, out_1618262330099536806);
void gnss_H_21(double *state, double *sat_pos_vel, double *out_7177713928341367106) {
H_21(state, sat_pos_vel, out_7177713928341367106);
}
void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt) {
predict(in_x, in_P, in_Q, dt);
selfdrive/locationd/models/generated/gnss.h
+13 -13
View File
@@ -5,18 +5,18 @@ void gnss_update_6(double *in_x, double *in_P, double *in_z, double *in_R, doubl
void gnss_update_20(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void gnss_update_7(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void gnss_update_21(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void gnss_err_fun(double *nom_x, double *delta_x, double *out_8218999594846759964);
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_6004490369099356165);
void gnss_H_mod_fun(double *state, double *out_5465143194872640210);
void gnss_f_fun(double *state, double dt, double *out_6477589900664112980);
void gnss_F_fun(double *state, double dt, double *out_159332577828599777);
void gnss_h_6(double *state, double *sat_pos, double *out_822185378282268145);
void gnss_H_6(double *state, double *sat_pos, double *out_2954217689973778176);
void gnss_h_20(double *state, double *sat_pos, double *out_3775340380998125465);
void gnss_H_20(double *state, double *sat_pos, double *out_4124080333257632986);
void gnss_h_7(double *state, double *sat_pos_vel, double *out_750344228235882460);
void gnss_H_7(double *state, double *sat_pos_vel, double *out_1618262330099536806);
void gnss_h_21(double *state, double *sat_pos_vel, double *out_750344228235882460);
void gnss_H_21(double *state, double *sat_pos_vel, double *out_1618262330099536806);
void gnss_err_fun(double *nom_x, double *delta_x, double *out_9093778295671839447);
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_9126699124174503695);
void gnss_H_mod_fun(double *state, double *out_854507982997558257);
void gnss_f_fun(double *state, double dt, double *out_7770403001027613956);
void gnss_F_fun(double *state, double dt, double *out_7798555812212859819);
void gnss_h_6(double *state, double *sat_pos, double *out_9201175564716128809);
void gnss_H_6(double *state, double *sat_pos, double *out_1419005819748336970);
void gnss_h_20(double *state, double *sat_pos, double *out_8590157039648027835);
void gnss_H_20(double *state, double *sat_pos, double *out_5617129456073796795);
void gnss_h_7(double *state, double *sat_pos_vel, double *out_8069802722407087087);
void gnss_H_7(double *state, double *sat_pos_vel, double *out_7177713928341367106);
void gnss_h_21(double *state, double *sat_pos_vel, double *out_8069802722407087087);
void gnss_H_21(double *state, double *sat_pos_vel, double *out_7177713928341367106);
void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt);
}
selfdrive/locationd/models/generated/libkf.so
BIN
View File
Binary file not shown.
selfdrive/locationd/models/generated/live.cpp
+1702 -1702
View File
File diff suppressed because it is too large Load Diff
selfdrive/locationd/models/generated/live.h
+24 -24
View File
@@ -10,29 +10,29 @@ void live_update_32(double *in_x, double *in_P, double *in_z, double *in_R, doub
void live_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void live_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void live_update_33(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void live_H(double *in_vec, double *out_7959907782308194627);
void live_err_fun(double *nom_x, double *delta_x, double *out_6929774310989775988);
void live_inv_err_fun(double *nom_x, double *true_x, double *out_6193789776824585571);
void live_H_mod_fun(double *state, double *out_8547509152961498922);
void live_f_fun(double *state, double dt, double *out_8100393315288221642);
void live_F_fun(double *state, double dt, double *out_7769089323050102648);
void live_h_4(double *state, double *unused, double *out_6579737165675804813);
void live_H_4(double *state, double *unused, double *out_8985066658053328462);
void live_h_9(double *state, double *unused, double *out_6592276556226636800);
void live_H_9(double *state, double *unused, double *out_6096205105773249120);
void live_h_10(double *state, double *unused, double *out_3043512482999151809);
void live_H_10(double *state, double *unused, double *out_7192958685017806166);
void live_h_12(double *state, double *unused, double *out_6151089362108428494);
void live_H_12(double *state, double *unused, double *out_1317938344370877970);
void live_h_35(double *state, double *unused, double *out_1134950865722680502);
void live_H_35(double *state, double *unused, double *out_1427624687954135739);
void live_h_32(double *state, double *unused, double *out_4721207292266360875);
void live_H_32(double *state, double *unused, double *out_2913380087778971149);
void live_h_13(double *state, double *unused, double *out_364134050257467252);
void live_H_13(double *state, double *unused, double *out_5486592313647626777);
void live_h_14(double *state, double *unused, double *out_6592276556226636800);
void live_H_14(double *state, double *unused, double *out_6096205105773249120);
void live_h_33(double *state, double *unused, double *out_3737322617129334857);
void live_H_33(double *state, double *unused, double *out_4578181692592993343);
void live_H(double *in_vec, double *out_7310749464307319056);
void live_err_fun(double *nom_x, double *delta_x, double *out_8236526131657428764);
void live_inv_err_fun(double *nom_x, double *true_x, double *out_4747477926419252764);
void live_H_mod_fun(double *state, double *out_341303559384211768);
void live_f_fun(double *state, double dt, double *out_4549525036970607503);
void live_F_fun(double *state, double dt, double *out_5133759507898990925);
void live_h_4(double *state, double *unused, double *out_6872450210111746980);
void live_H_4(double *state, double *unused, double *out_2937396917593955740);
void live_h_9(double *state, double *unused, double *out_8735722694958550472);
void live_H_9(double *state, double *unused, double *out_3178586564223546385);
void live_h_10(double *state, double *unused, double *out_4239705948005809237);
void live_H_10(double *state, double *unused, double *out_4956943889339789869);
void live_h_12(double *state, double *unused, double *out_3848117259651593996);
void live_H_12(double *state, double *unused, double *out_3558495942641549407);
void live_h_35(double *state, double *unused, double *out_854595621797169640);
void live_H_35(double *state, double *unused, double *out_6304058974966563116);
void live_h_32(double *state, double *unused, double *out_2979708909891080243);
void live_H_32(double *state, double *unused, double *out_9087717139384980709);
void live_h_13(double *state, double *unused, double *out_7896661243212334910);
void live_H_13(double *state, double *unused, double *out_83856576196215707);
void live_h_14(double *state, double *unused, double *out_8735722694958550472);
void live_H_14(double *state, double *unused, double *out_3178586564223546385);
void live_h_33(double *state, double *unused, double *out_9184153656412830582);
void live_H_33(double *state, double *unused, double *out_8992128094104130896);
void live_predict(double *in_x, double *in_P, double *in_Q, double dt);
}
selfdrive/loggerd/bootlog
BIN
View File
Binary file not shown.
selfdrive/loggerd/loggerd
BIN
View File
Binary file not shown.
selfdrive/modeld/_dmonitoringmodeld
BIN
View File
Binary file not shown.
selfdrive/modeld/_modeld
BIN
View File
Binary file not shown.
selfdrive/modeld/models/supercombo.thneed
BIN
View File
Binary file not shown.
selfdrive/proclogd/proclogd
BIN
View File
Binary file not shown.
selfdrive/ui/_ui
BIN
View File
Binary file not shown.
selfdrive/ui/_ui_nonav
BIN
View File
Binary file not shown.
selfdrive/ui/qt/spinner
BIN
View File
Binary file not shown.
selfdrive/ui/qt/text
BIN
View File
Binary file not shown.
selfdrive/ui/soundd/_soundd
BIN
View File
Binary file not shown.
system/camerad/camerad
BIN
View File
Binary file not shown.
system/sensord/_sensord
BIN
View File
Binary file not shown.
system/ubloxd/ubloxd
BIN
View File
Binary file not shown.