mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-06-19 16:52:06 +08:00
Compare commits
1 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 Jason Wen
|
1a7fa0c205 |
@@ -1,3 +1,30 @@
|
||||
sunnypilot - 0.9.2.3 (2023-06-18)
|
||||
========================
|
||||
* NEW❗: Auto Lane Change: Delay with Blind Spot
|
||||
* Toggle to enable a delay timer for seamless lane changes when blind spot monitoring (BSM) detects an obstructing vehicle, ensuring safe maneuvering
|
||||
* NEW❗: Driving Screen Off: Wake with Non-Critical Events
|
||||
* When Driving Screen Off Timer is not set to "Always On":
|
||||
* Enabled: Wake the brightness of the screen to display all events
|
||||
* Disabled: Wake the brightness of the screen to display critical events
|
||||
* Currently, all non-nudge modes are default to continue lane change after 1 seconds of blind spot detection
|
||||
* NEW❗: Fleet Manager PIN Requirement toggle
|
||||
* User can now enable or disable PIN requirement on the comma device before accessing Fleet Manager
|
||||
* NEW❗: Reset all sunnypilot settings toggle
|
||||
* NEW❗: Turn signals display on screen when blinker is used
|
||||
* Green: Blinker is on
|
||||
* Red: Blinker is on, car detected in the adjacent blind spot or road edge detected
|
||||
* IMPROVED: mapd: better exceptions handling when loading dependencies
|
||||
* UPDATED: Green Traffic Light Chime no longer displays an orange border when executed
|
||||
* FIXED: mapd: Road name flashing caused by desync with last GPS timestamp
|
||||
* FIXED: Ram HD (2500/3500): Ignore paramsd sanity check
|
||||
* Live parameters have trouble with self-tuning on this platform with upstream openpilot 0.9.2
|
||||
* Hyundai: Longitudinal support for CAN-based Camera SCC cars thanks to Zack1010OP's Patreon sponsor!
|
||||
|
||||
sunnypilot - 0.9.2.2 (2023-06-13)
|
||||
========================
|
||||
* NEW❗: Toyota: Allow M.A.D.S. toggling with LKAS Button (Beta)
|
||||
* IMPROVED: Ram: cruise button handling
|
||||
|
||||
sunnypilot - 0.9.2.1 (2023-06-10)
|
||||
========================
|
||||
* UPDATED: Synced with commaai's 0.9.2 release
|
||||
|
||||
@@ -37,7 +37,7 @@ Join the official sunnypilot Discord server to stay up to date with all the late
|
||||
|
||||
---
|
||||
|
||||
[sunnypilot](https://github.com/sunnyhaibin/sunnypilot) is a fork of comma.ai's openpilot, an open source driver assistance system. sunnypilot offers the user a unique driving experience for over 200 supported car makes and models with modified behaviors of driving assist engagements. sunnypilot complies with comma.ai's safety rules as accurately as possible.
|
||||
[sunnypilot](https://github.com/sunnyhaibin/sunnypilot) is a fork of comma.ai's openpilot, an open source driver assistance system. sunnypilot offers the user a unique driving experience for over 250+ supported car makes and models with modified behaviors of driving assist engagements. sunnypilot complies with comma.ai's safety rules as accurately as possible.
|
||||
|
||||
</details>
|
||||
|
||||
@@ -93,42 +93,48 @@ The following changes are a **VIOLATION** of this policy and **ARE NOT** include
|
||||
comma three
|
||||
------
|
||||
|
||||
Please refer to [Recommended Branches](#-recommended-branches) to find your preferred/supported branch, this guide will assume you want to install the latest release:
|
||||
Please refer to [Recommended Branches](#-recommended-branches) to find your preferred/supported branch. This guide will assume you want to install the latest `release-c3` branch.
|
||||
|
||||
1. [Factory reset/uninstall](https://github.com/commaai/openpilot/wiki/FAQ#how-can-i-reset-the-device) previous software if you already installed another software/fork.
|
||||
2. Select `Custom Software` when given the option upon reboot.
|
||||
3. Input the following URL: ```bit.ly/sp-dev-nicki-minaj```[^4] (note: `https://` is not requirement on the comma three)
|
||||
4. Complete the rest of the installation following the onscreen instructions.
|
||||
5. Pop into the `#installation-help` channel on Discord for further assistance.
|
||||
* sunnypilot not installed or you installed a version before 0.8.17?
|
||||
1. [Factory reset/uninstall](https://github.com/commaai/openpilot/wiki/FAQ#how-can-i-reset-the-device) the previous software if you have another software/fork installed.
|
||||
2. After factory reset/uninstall and upon reboot, select `Custom Software` when given the option.
|
||||
3. Input the installation URL per [Recommended Branches](#-recommended-branches). Example: ```bit.ly/sp-release-c3``` [^4] (note: `https://` is not requirement on the comma three)
|
||||
4. Complete the rest of the installation following the onscreen instructions.
|
||||
|
||||
* sunnypilot already installed and you installed a version after 0.8.17?
|
||||
1. On the comma three, go to `Settings` ▶️ `Software`.
|
||||
2. At the `Download` option, press `CHECK`. This will fetch the list of latest branches from sunnypilot.
|
||||
3. At the `Target Branch` option, press `SELECT` to open the Target Branch selector.
|
||||
4. Scroll to select the desired branch per [Recommended Branches](#-recommended-branches). Example: `release-c3`
|
||||
|
||||
Requires further assistance with software installation? Join the [sunnypilot Discord server](https://discord.sunnypilot.com) and message us in the `#installation-help` channel.
|
||||
|
||||
comma two
|
||||
------
|
||||
|
||||
Please refer to [Recommended Branches](#-recommended-branches) to find your preferred/supported branch, this will assume you want to install the latest release:
|
||||
|
||||
1. Factory reset/uninstall previous software if you already installed another software/fork.
|
||||
2. Select `Custom Software` when given the option.
|
||||
3. Input the following URL: ```https://smiskol.com/fork/sunnyhaibin/0.8.12-4-prod```
|
||||
1. [Factory reset/uninstall](https://github.com/commaai/openpilot/wiki/FAQ#how-can-i-reset-the-device) the previous software if you have another software/fork installed.
|
||||
2. After factory reset/uninstall and upon reboot, select `Custom Software` when given the option.
|
||||
3. Input the installation URL per [Recommended Branches](#-recommended-branches). Example: ```https://smiskol.com/fork/sunnyhaibin/0.8.12-4-prod```
|
||||
4. Complete the rest of the installation following the onscreen instructions.
|
||||
5. Pop into the `#installation-help` channel on Discord for further assistance.
|
||||
|
||||
Requires further assistance with software installation? Join the [sunnypilot Discord server](https://discord.sunnypilot.com) and message us in the `#installation-help` channel.
|
||||
|
||||
</details>
|
||||
|
||||
<details>
|
||||
<summary>SSH (More Versatile)</summary>
|
||||
</br>
|
||||
<br>
|
||||
|
||||
Prerequisites: [How to SSH](https://github.com/commaai/openpilot/wiki/SSH)
|
||||
|
||||
If you are looking to install sunnyhaibin's fork via SSH, run the following command in an SSH terminal after connecting to your device:
|
||||
|
||||
If you are looking to install sunnypilot via SSH, run the following command in an SSH terminal after connecting to your device:
|
||||
|
||||
comma three:
|
||||
------
|
||||
* [`dev-nicki-minaj`](https://github.com/sunnyhaibin/openpilot/tree/dev-nicki-minaj):
|
||||
* [`release-c3`](https://github.com/sunnyhaibin/openpilot/tree/release-c3):
|
||||
|
||||
```
|
||||
cd /data; rm -rf ./openpilot; git clone -b dev-nicki-minaj --depth 1 --single-branch --recurse-submodules --shallow-submodules https://github.com/sunnyhaibin/sunnypilot.git openpilot; cd openpilot; sudo reboot
|
||||
cd /data; rm -rf ./openpilot; git clone -b release-c3 --recurse-submodules https://github.com/sunnyhaibin/sunnypilot.git openpilot; cd openpilot; sudo reboot
|
||||
```
|
||||
|
||||
comma two:
|
||||
@@ -136,7 +142,7 @@ comma two:
|
||||
* [`0.8.12-prod-personal-hkg`](https://github.com/sunnyhaibin/openpilot/tree/0.8.12-prod-personal-hkg):
|
||||
|
||||
```
|
||||
cd /data; rm -rf ./openpilot; git clone -b 0.8.12-prod-personal-hkg --depth 1 --single-branch --recurse-submodules --shallow-submodules https://github.com/sunnyhaibin/sunnypilot.git openpilot; cd openpilot; reboot
|
||||
cd /data; rm -rf ./openpilot; git clone -b 0.8.12-prod-personal-hkg --recurse-submodules https://github.com/sunnyhaibin/sunnypilot.git openpilot; cd openpilot; sudo reboot
|
||||
```
|
||||
|
||||
After running the command to install the desired branch, your comma device should reboot.
|
||||
@@ -313,16 +319,17 @@ There are 4 modes to select on the steering wheel and/or the onroad camera scree
|
||||
|
||||
---
|
||||
|
||||
| Tag | Definition | Description |
|
||||
|:--------:|-----------------------------|---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
|
||||
| `prod` | Production/Release branches | Include features that are **verified** by trusted testers and the community. Ready to use. ✅ |
|
||||
| `test` | Test branches | Include new features that are **tested** by trusted testers and the community. Stability may vary. ⚠ |
|
||||
| `dev` | Development branches | All features are gathered in respective versions. Reviewed and merged features will be committed to `dev`. Stability may vary. ⚠ |
|
||||
| `master` | Main branch | Syncs with [commaai's openpilot `master`](https://github.com/commaai/openpilot) upstream branch. Accepts all pull requests. Does not include all sunnypilot features. Stability may vary. ⚠ |
|
||||
| Tag | Definition | Description |
|
||||
|:---------:|----------------------|---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
|
||||
| `release` | Release branches | Include features that are **verified** by trusted testers and the community. Ready to use. ✅ |
|
||||
| `staging` | Staging branches | Include new features that are **tested** by trusted testers and the community. Stability may vary. ⚠ |
|
||||
| `dev` | Development branches | All features are gathered in respective versions. Reviewed and merged features will be committed to `dev`. Stability may vary. ⚠ |
|
||||
| `master` | Main branch | Syncs with [commaai's openpilot `master`](https://github.com/commaai/openpilot) upstream branch. Accepts all pull requests. Does not include all sunnypilot features. Stability may vary. ⚠ |
|
||||
|
||||
Example:
|
||||
* [`test-c3`](https://github.com/sunnyhaibin/sunnypilot/tree/test-c3): Latest test branch that are tested by trusted testers and the community, verification required.
|
||||
* [`dev-nicki-minaj`](https://github.com/sunnyhaibin/sunnypilot/tree/dev-nicki-minaj): Latest development branch that include all sunnypilot features, testing required.
|
||||
* [`release-c3`](https://github.com/sunnyhaibin/sunnypilot/tree/release-c3): Latest release branch for comma three that are verified by trusted testers and the community. Ready to use.
|
||||
* [`staging-c3`](https://github.com/sunnyhaibin/sunnypilot/tree/staging-c3): Latest staging branch for comma three that are tested by trusted testers and the community. Verification required.
|
||||
* [`dev-c3`](https://github.com/sunnyhaibin/sunnypilot/tree/dev-c3): Latest development branch for comma three that include all sunnypilot features. Testing required.
|
||||
|
||||
</details>
|
||||
|
||||
@@ -330,12 +337,11 @@ Example:
|
||||
|
||||
---
|
||||
|
||||
| Branch | Definition | Compatible Device | Changelogs |
|
||||
|:----------------------------------------------------------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------|-------------------|---------------------------------------------------------------------------------------------------------|
|
||||
| [`0.8.12-prod-full-<car>`](https://github.com/sunnyhaibin/sunnypilot/branches/all?query=0.8.12-prod-full) | • Latest production/stable branch<br/>• Based on comma.ai's openpilot 0.8.12 | comma two/three | [`CHANGELOGS.md`](https://github.com/sunnyhaibin/sunnypilot/blob/0.8.12-prod-full-subaru/CHANGELOGS.md) |
|
||||
| [`0.8.14-prod-c3`](https://github.com/sunnyhaibin/sunnypilot/tree/0.8.14-prod-c3) | • Latest production/stable branch<br/>• Based on comma.ai's openpilot 0.8.14 | comma three | [`CHANGELOGS.md`](https://github.com/sunnyhaibin/sunnypilot/blob/0.8.14-prod-c3/CHANGELOGS.md) |
|
||||
| [`test-c3`](https://github.com/sunnyhaibin/sunnypilot/tree/test-c3) | • Latest test branch with experimental features<br/>• Based on comma.ai's openpilot commits up to December 16th, 2022 | comma three | [`CHANGELOGS.md`](https://github.com/sunnyhaibin/sunnypilot/blob/test-c3/CHANGELOGS.md) |
|
||||
| [`dev-nicki-minaj`](https://github.com/sunnyhaibin/sunnypilot/tree/dev-nicki-minaj) | • Latest development branch<br/>• Based on comma.ai's openpilot 0.9.2 release<br/>• Nicki Minaj driving model | comma three | [`CHANGELOGS.md`](https://github.com/sunnyhaibin/sunnypilot/blob/dev-nicki-minaj/CHANGELOGS.md) |
|
||||
| Branch | Definition | Compatible Device | Changelogs |
|
||||
|:------------------------------------------------------------------------------------|---------------------------------------------------------|-------------------|--------------------------------------------------------------------------------------------|
|
||||
| [`release-c3`](https://github.com/sunnyhaibin/sunnypilot/tree/release-c3) | • Latest release/stable branch | comma three | [`CHANGELOGS.md`](https://github.com/sunnyhaibin/sunnypilot/blob/release-c3/CHANGELOGS.md) |
|
||||
| [`staging-c3`](https://github.com/sunnyhaibin/sunnypilot/tree/staging-c3) | • Latest staging branch | comma three | [`CHANGELOGS.md`](https://github.com/sunnyhaibin/sunnypilot/blob/staging-c3/CHANGELOGS.md) |
|
||||
| [`dev-c3`](https://github.com/sunnyhaibin/sunnypilot/tree/dev-c3) | • Latest development branch with experimental features | comma three | [`CHANGELOGS.md`](https://github.com/sunnyhaibin/sunnypilot/blob/dev-c3/CHANGELOGS.md) |
|
||||
|
||||
</details>
|
||||
|
||||
@@ -370,7 +376,7 @@ Pull requests should be against the most current `master` branch.
|
||||
* [sshane](https://github.com/sshane/openpilot-installer-generator)
|
||||
* [jung](https://github.com/chanhojung/openpilot)
|
||||
* [dri94](https://github.com/dri94/openpilot)
|
||||
* [JamesKGithub](https://github.com/JamesKGithub/FrogPilot)
|
||||
* [FrogAi](https://github.com/frogAi/FrogPilot/)
|
||||
* [twilsonco](https://github.com/twilsonco/openpilot)
|
||||
* [martinl](https://github.com/martinl/openpilot)
|
||||
* [multikyd](https://github.com/openpilotkr)
|
||||
@@ -445,4 +451,4 @@ Your continuous love and support are greatly appreciated! Enjoy 🥰
|
||||
|
||||
[^1]:Requires data connection if not using Offline Maps data
|
||||
[^2]:At least 50 GB of storage space is required. If you have the 32 GB version of comma three, upgrading with a compatible 250 GB or 1 TB SSD is strongly recommended
|
||||
[^4]:Shortened URL for convenience. Full URL is ```smiskol.com/fork/sunnyhaibin/test-c3```
|
||||
[^4]:Shortened URL for convenience. Full URL is ```smiskol.com/fork/sunnyhaibin/release-c3```
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1 +1 @@
|
||||
const uint8_t gitversion[8] = "319650f4";
|
||||
const uint8_t gitversion[8] = "4c926292";
|
||||
|
||||
Binary file not shown.
Binary file not shown.
+1
-1
@@ -1 +1 @@
|
||||
#define COMMA_VERSION "0.9.2.1-release"
|
||||
#define COMMA_VERSION "0.9.2.3-release"
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -95,34 +95,44 @@ class CarController:
|
||||
if self.frame % 10 == 0 and self.CP.carFingerprint not in RAM_CARS:
|
||||
can_sends.append(create_lkas_heartbit(self.packer, CS.madsEnabled, CS.lkas_heartbit))
|
||||
|
||||
ram_cars = self.CP.carFingerprint in RAM_CARS
|
||||
|
||||
das_bus = 2 if self.CP.carFingerprint in RAM_CARS else 0
|
||||
# cruise buttons
|
||||
if CS.button_counter != self.last_button_frame:
|
||||
self.last_button_frame = CS.button_counter
|
||||
|
||||
if ram_cars:
|
||||
if CS.buttonStates["cancel"]:
|
||||
can_sends.append(create_cruise_buttons(self.packer, CS.button_counter, das_bus, self.CP, cancel=True))
|
||||
else:
|
||||
can_sends.append(create_cruise_buttons(self.packer, CS.button_counter, das_bus, self.CP,
|
||||
cruise_buttons_msg=CS.cruise_buttons,
|
||||
cancel=CC.cruiseControl.cancel, resume=CC.cruiseControl.resume))
|
||||
|
||||
# ACC cancellation
|
||||
if CC.cruiseControl.cancel:
|
||||
elif CC.cruiseControl.cancel:
|
||||
self.last_button_frame = self.frame
|
||||
can_sends.append(create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, cancel=True))
|
||||
can_sends.append(create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, self.CP, cancel=True))
|
||||
|
||||
# ACC resume from standstill
|
||||
elif CC.cruiseControl.resume:
|
||||
self.last_button_frame = self.frame
|
||||
can_sends.append(create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, resume=True))
|
||||
can_sends.append(create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, self.CP, resume=True))
|
||||
|
||||
elif not self.CP.pcmCruiseSpeed and CS.out.cruiseState.enabled:
|
||||
if not (CC.cruiseControl.cancel or CC.cruiseControl.resume) and not self.CP.pcmCruiseSpeed and CS.out.cruiseState.enabled:
|
||||
self.button_frame += 1
|
||||
button_counter_offset = [1, 1, 0, None][self.button_frame % 4]
|
||||
if self.CP.carFingerprint in RAM_CARS:
|
||||
if ram_cars:
|
||||
self.cruise_button = self.get_cruise_buttons(CS, CC.vCruise)
|
||||
elif button_counter_offset is not None:
|
||||
self.cruise_button = self.get_cruise_buttons(CS, CC.vCruise)
|
||||
|
||||
if self.cruise_button is not None:
|
||||
if self.CP.carFingerprint in RAM_CARS:
|
||||
can_sends.append(create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, buttons=self.cruise_button))
|
||||
if ram_cars:
|
||||
can_sends.append(create_cruise_buttons(self.packer, CS.button_counter, das_bus, self.CP, buttons=self.cruise_button))
|
||||
elif button_counter_offset is not None:
|
||||
can_sends.append(create_cruise_buttons(self.packer, CS.button_counter + button_counter_offset, das_bus, buttons=self.cruise_button))
|
||||
can_sends.append(create_cruise_buttons(self.packer, CS.button_counter + button_counter_offset, das_bus, self.CP, buttons=self.cruise_button))
|
||||
|
||||
# HUD alerts
|
||||
if self.frame % 25 == 0:
|
||||
@@ -175,9 +185,8 @@ class CarController:
|
||||
|
||||
# multikyd methods, sunnyhaibin logic
|
||||
def get_cruise_buttons_status(self, CS):
|
||||
if not CS.out.cruiseState.enabled:
|
||||
if any(CS.buttonStates[button_state] for button_state in BUTTONS_STATES):
|
||||
self.timer = 40
|
||||
if not CS.out.cruiseState.enabled or any(CS.buttonStates[button_state] for button_state in BUTTONS_STATES):
|
||||
self.timer = 40
|
||||
elif self.timer:
|
||||
self.timer -= 1
|
||||
else:
|
||||
|
||||
@@ -113,6 +113,7 @@ class CarState(CarStateBase):
|
||||
|
||||
self.lkas_car_model = cp_cam.vl["DAS_6"]["CAR_MODEL"]
|
||||
self.button_counter = cp.vl["CRUISE_BUTTONS"]["COUNTER"]
|
||||
self.cruise_buttons = cp.vl["CRUISE_BUTTONS"]
|
||||
|
||||
return ret
|
||||
|
||||
@@ -165,6 +166,10 @@ class CarState(CarStateBase):
|
||||
("ACC_Decel", "CRUISE_BUTTONS"),
|
||||
("ACC_Cancel", "CRUISE_BUTTONS"),
|
||||
("ACC_Resume", "CRUISE_BUTTONS"),
|
||||
("Cruise_OnOff", "CRUISE_BUTTONS"),
|
||||
("ACC_OnOff", "CRUISE_BUTTONS"),
|
||||
("ACC_Distance_Inc", "CRUISE_BUTTONS"),
|
||||
("ACC_Distance_Dec", "CRUISE_BUTTONS"),
|
||||
]
|
||||
|
||||
checks = [
|
||||
|
||||
@@ -63,7 +63,7 @@ def create_lkas_command(packer, CP, apply_steer, lkas_control_bit):
|
||||
return packer.make_can_msg("LKAS_COMMAND", 0, values)
|
||||
|
||||
|
||||
def create_cruise_buttons(packer, frame, bus, buttons=0, cancel=False, resume=False):
|
||||
def create_cruise_buttons(packer, frame, bus, CP, cruise_buttons_msg=None, buttons=0, cancel=False, resume=False):
|
||||
|
||||
acc_accel = 1 if buttons == 1 else 0
|
||||
acc_decel = 1 if buttons == 2 else 0
|
||||
@@ -75,6 +75,9 @@ def create_cruise_buttons(packer, frame, bus, buttons=0, cancel=False, resume=Fa
|
||||
"ACC_Decel": acc_decel,
|
||||
"COUNTER": frame % 0x10,
|
||||
}
|
||||
|
||||
if buttons == 0 and not (cancel or resume) and CP.carFingerprint in RAM_CARS:
|
||||
values = cruise_buttons_msg.copy()
|
||||
return packer.make_can_msg("CRUISE_BUTTONS", bus, values)
|
||||
|
||||
|
||||
|
||||
@@ -82,6 +82,7 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.mass = 3405. + STD_CARGO_KG
|
||||
ret.minSteerSpeed = 16
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, 1.0, False)
|
||||
ret.flags |= ChryslerFlags.SP_RAM_HD_PARAMSD_IGNORE.value
|
||||
|
||||
else:
|
||||
raise ValueError(f"Unsupported car: {candidate}")
|
||||
@@ -108,7 +109,7 @@ class CarInterface(CarInterfaceBase):
|
||||
be.pressed = self.CS.buttonStates[button]
|
||||
buttonEvents.append(be)
|
||||
|
||||
self.CS.mads_enabled = False if not (self.CS.control_initialized or self.mads_main_toggle) else ret.cruiseState.available
|
||||
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)
|
||||
|
||||
self.CS.accEnabled, buttonEvents = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
|
||||
buttonEvents, c.vCruise,
|
||||
|
||||
@@ -14,6 +14,7 @@ Ecu = car.CarParams.Ecu
|
||||
class ChryslerFlags(IntFlag):
|
||||
HIGHER_MIN_STEERING_SPEED = 1
|
||||
|
||||
SP_RAM_HD_PARAMSD_IGNORE = 2
|
||||
|
||||
class CAR:
|
||||
# Chrysler
|
||||
|
||||
@@ -262,7 +262,7 @@ class CarInterface(CarInterfaceBase):
|
||||
if self.CS.cruise_buttons != CruiseButtons.UNPRESS and self.CS.prev_cruise_buttons != CruiseButtons.UNPRESS:
|
||||
buttonEvents.append(create_button_event(CruiseButtons.UNPRESS, self.CS.prev_cruise_buttons, BUTTONS_DICT, CruiseButtons.UNPRESS))
|
||||
|
||||
self.CS.mads_enabled = False if not (self.CS.control_initialized or self.mads_main_toggle) else ret.cruiseState.available
|
||||
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)
|
||||
|
||||
if not self.CP.pcmCruise:
|
||||
if any(b.type == ButtonType.accelCruise and b.pressed for b in buttonEvents):
|
||||
|
||||
@@ -345,7 +345,7 @@ class CarInterface(CarInterfaceBase):
|
||||
if self.CS.cruise_setting != self.CS.prev_cruise_setting:
|
||||
buttonEvents.append(create_button_event(self.CS.cruise_setting, self.CS.prev_cruise_setting, {1: ButtonType.altButton1}))
|
||||
|
||||
self.CS.mads_enabled = False if not (self.CS.control_initialized or self.mads_main_toggle) else ret.cruiseState.available
|
||||
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)
|
||||
|
||||
self.CS.accEnabled, buttonEvents = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
|
||||
buttonEvents, c.vCruise)
|
||||
|
||||
@@ -8,7 +8,7 @@ from opendbc.can.packer import CANPacker
|
||||
from selfdrive.car import apply_driver_steer_torque_limits
|
||||
from selfdrive.car.hyundai import hyundaicanfd, hyundaican
|
||||
from selfdrive.car.hyundai.hyundaicanfd import CanBus
|
||||
from selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR
|
||||
from selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR, CAMERA_SCC_CAR
|
||||
from selfdrive.controls.lib.drive_helpers import HYUNDAI_V_CRUISE_MIN
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
@@ -167,7 +167,8 @@ class CarController:
|
||||
# *** common hyundai stuff ***
|
||||
|
||||
# tester present - w/ no response (keeps relevant ECU disabled)
|
||||
if self.frame % 100 == 0 and not ((self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) or escc) and self.CP.openpilotLongitudinalControl:
|
||||
if self.frame % 100 == 0 and not ((self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) or escc) and \
|
||||
self.CP.carFingerprint not in CAMERA_SCC_CAR and self.CP.openpilotLongitudinalControl:
|
||||
# for longitudinal control, either radar or ADAS driving ECU
|
||||
addr, bus = 0x7d0, 0
|
||||
if self.CP.flags & HyundaiFlags.CANFD_HDA2.value:
|
||||
@@ -288,7 +289,7 @@ class CarController:
|
||||
jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
|
||||
can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, accel, jerk, int(self.frame / 2),
|
||||
hud_control.leadVisible, set_speed_in_units, stopping, CC.cruiseControl.override, CS.mainEnabled,
|
||||
CS, escc))
|
||||
CS, escc, self.CP.carFingerprint))
|
||||
|
||||
# 20 Hz LFA MFA message
|
||||
if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value:
|
||||
@@ -296,7 +297,7 @@ class CarController:
|
||||
|
||||
# 5 Hz ACC options
|
||||
if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl:
|
||||
can_sends.extend(hyundaican.create_acc_opt(self.packer, escc))
|
||||
can_sends.extend(hyundaican.create_acc_opt(self.packer, escc, CS, self.CP.carFingerprint))
|
||||
|
||||
# 2 Hz front radar options
|
||||
if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl and not escc:
|
||||
|
||||
@@ -178,6 +178,10 @@ class CarState(CarStateBase):
|
||||
# save the entire LKAS11 and CLU11
|
||||
self.lkas11 = copy.copy(cp_cam.vl["LKAS11"])
|
||||
self.clu11 = copy.copy(cp.vl["CLU11"])
|
||||
# only forward FCA messages for FCW/AEB when using openpilot longitudinal on Camera SCC cars
|
||||
if self.CP.openpilotLongitudinalControl and self.CP.carFingerprint in CAMERA_SCC_CAR:
|
||||
self.fca11 = copy.copy(cp_cruise.vl["FCA11"])
|
||||
self.fca12 = copy.copy(cp_cruise.vl["FCA12"])
|
||||
self.steer_state = cp.vl["MDPS12"]["CF_Mdps_ToiActive"] # 0 NOT ACTIVE, 1 ACTIVE
|
||||
self.prev_cruise_buttons = self.cruise_buttons[-1]
|
||||
self.prev_main_buttons = self.main_buttons[-1]
|
||||
@@ -517,6 +521,34 @@ class CarState(CarStateBase):
|
||||
("CF_VSM_DecCmdAct", "SCC12"),
|
||||
]
|
||||
|
||||
if CP.openpilotLongitudinalControl and CP.carFingerprint in CAMERA_SCC_CAR:
|
||||
if CP.flags & HyundaiFlags.USE_FCA.value:
|
||||
signals += [
|
||||
("CF_VSM_Prefill", "FCA11"),
|
||||
("CF_VSM_HBACmd", "FCA11"),
|
||||
("CF_VSM_BeltCmd", "FCA11"),
|
||||
("CR_VSM_DecCmd", "FCA11"),
|
||||
("FCA_Status", "FCA11"),
|
||||
("FCA_StopReq", "FCA11"),
|
||||
("FCA_DrvSetStatus", "FCA11"),
|
||||
("FCA_Failinfo", "FCA11"),
|
||||
("CR_FCA_Alive", "FCA11"),
|
||||
("FCA_RelativeVelocity", "FCA11"),
|
||||
("FCA_TimetoCollision", "FCA11"),
|
||||
("CR_FCA_ChkSum", "FCA11"),
|
||||
("PAINT1_Status", "FCA11"),
|
||||
("FCA_CmdAct", "FCA11"),
|
||||
("CF_VSM_Warn", "FCA11"),
|
||||
("CF_VSM_DecCmdAct", "FCA11"),
|
||||
|
||||
("FCA_USM", "FCA12"),
|
||||
("FCA_DrvSetState", "FCA12"),
|
||||
]
|
||||
checks += [
|
||||
("FCA11", 50),
|
||||
("FCA12", 50),
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)
|
||||
|
||||
@staticmethod
|
||||
|
||||
@@ -100,7 +100,7 @@ def create_lfahda_mfc(packer, enabled, lat_active, lateral_paused, blinking_icon
|
||||
return packer.make_can_msg("LFAHDA_MFC", 0, values)
|
||||
|
||||
def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, long_override, main_enabled,
|
||||
CS, escc):
|
||||
CS, escc, car_fingerprint):
|
||||
commands = []
|
||||
|
||||
scc11_values = {
|
||||
@@ -145,24 +145,30 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, s
|
||||
|
||||
# note that some vehicles most likely have an alternate checksum/counter definition
|
||||
# https://github.com/commaai/opendbc/commit/9ddcdb22c4929baf310295e832668e6e7fcfa602
|
||||
fca11_values = {
|
||||
"CR_FCA_Alive": idx % 0xF,
|
||||
"PAINT1_Status": 0 if escc else 1,
|
||||
"FCA_DrvSetStatus": 0 if escc else 1,
|
||||
"FCA_Status": 0 if escc else 1, # AEB disabled
|
||||
if car_fingerprint in CAMERA_SCC_CAR:
|
||||
fca11_values = CS.fca11
|
||||
fca11_values["PAINT1_Status"] = 1
|
||||
fca11_values["FCA_DrvSetStatus"] = 1
|
||||
fca11_values["FCA_Status"] = 1 # AEB disabled, until a route with AEB or FCW trigger is verified
|
||||
else:
|
||||
fca11_values = {
|
||||
"CR_FCA_Alive": idx % 0xF,
|
||||
"PAINT1_Status": 0 if escc else 1,
|
||||
"FCA_DrvSetStatus": 0 if escc else 1,
|
||||
"FCA_Status": 0 if escc else 1, # AEB disabled
|
||||
|
||||
"FCA_CmdAct": CS.escc_cmd_act,
|
||||
"CF_VSM_Warn": CS.escc_aeb_warning,
|
||||
"CF_VSM_DecCmdAct": CS.escc_aeb_dec_cmd_act,
|
||||
"CR_VSM_DecCmd": CS.escc_aeb_dec_cmd,
|
||||
}
|
||||
"FCA_CmdAct": CS.escc_cmd_act,
|
||||
"CF_VSM_Warn": CS.escc_aeb_warning,
|
||||
"CF_VSM_DecCmdAct": CS.escc_aeb_dec_cmd_act,
|
||||
"CR_VSM_DecCmd": CS.escc_aeb_dec_cmd,
|
||||
}
|
||||
fca11_dat = packer.make_can_msg("FCA11", 0, fca11_values)[2]
|
||||
fca11_values["CR_FCA_ChkSum"] = hyundai_checksum(fca11_dat[:7])
|
||||
commands.append(packer.make_can_msg("FCA11", 0, fca11_values))
|
||||
|
||||
return commands
|
||||
|
||||
def create_acc_opt(packer, escc):
|
||||
def create_acc_opt(packer, escc, CS, car_fingerprint):
|
||||
commands = []
|
||||
|
||||
scc13_values = {
|
||||
@@ -172,10 +178,15 @@ def create_acc_opt(packer, escc):
|
||||
}
|
||||
commands.append(packer.make_can_msg("SCC13", 0, scc13_values))
|
||||
|
||||
fca12_values = {
|
||||
"FCA_DrvSetState": 0 if escc else 2,
|
||||
"FCA_USM": 0 if escc else 1, # AEB disabled
|
||||
}
|
||||
if car_fingerprint in CAMERA_SCC_CAR:
|
||||
fca12_values = CS.fca12
|
||||
fca12_values["FCA_DrvSetState"] = 2
|
||||
fca12_values["FCA_USM"] = 1 # AEB disabled, until a route with AEB or FCW trigger is verified
|
||||
else:
|
||||
fca12_values = {
|
||||
"FCA_DrvSetState": 0 if escc else 2,
|
||||
"FCA_USM": 0 if escc else 1, # AEB disabled
|
||||
}
|
||||
commands.append(packer.make_can_msg("FCA12", 0, fca12_values))
|
||||
|
||||
return commands
|
||||
|
||||
@@ -248,7 +248,7 @@ class CarInterface(CarInterfaceBase):
|
||||
else:
|
||||
ret.longitudinalTuning.kpV = [0.5]
|
||||
ret.longitudinalTuning.kiV = [0.0]
|
||||
ret.experimentalLongitudinalAvailable = candidate not in (LEGACY_SAFETY_MODE_CAR | CAMERA_SCC_CAR)
|
||||
ret.experimentalLongitudinalAvailable = candidate not in LEGACY_SAFETY_MODE_CAR
|
||||
ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable
|
||||
ret.pcmCruise = not ret.openpilotLongitudinalControl
|
||||
|
||||
@@ -330,7 +330,8 @@ class CarInterface(CarInterfaceBase):
|
||||
|
||||
@staticmethod
|
||||
def init(CP, logcan, sendcan):
|
||||
if CP.openpilotLongitudinalControl and not ((CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) or (CP.flags & HyundaiFlags.SP_ENHANCED_SCC)):
|
||||
if CP.openpilotLongitudinalControl and not ((CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) or (CP.flags & HyundaiFlags.SP_ENHANCED_SCC)) and \
|
||||
CP.carFingerprint not in CAMERA_SCC_CAR:
|
||||
addr, bus = 0x7d0, 0
|
||||
if CP.flags & HyundaiFlags.CANFD_HDA2.value:
|
||||
addr, bus = 0x730, CanBus(CP).ECAN
|
||||
|
||||
@@ -113,6 +113,7 @@ class CarInterfaceBase(ABC):
|
||||
self.gac_max = -1
|
||||
self.reverse_dm_cam = self.param_s.get_bool("ReverseDmCam")
|
||||
self.mads_main_toggle = self.param_s.get_bool("MadsCruiseMain")
|
||||
self.lkas_toggle = self.param_s.get_bool("LkasToggle")
|
||||
|
||||
@staticmethod
|
||||
def get_pid_accel_limits(CP, current_speed, cruise_speed):
|
||||
@@ -398,6 +399,16 @@ class CarInterfaceBase(ABC):
|
||||
regen = cs_out.regenBraking and (not self.CS.out.regenBraking or not cs_out.standstill)
|
||||
return brake or regen
|
||||
|
||||
def get_sp_cruise_main_state(self, cs_out, CS):
|
||||
if not CS.control_initialized:
|
||||
mads_enabled = False
|
||||
elif not self.mads_main_toggle:
|
||||
mads_enabled = False
|
||||
else:
|
||||
mads_enabled = cs_out.cruiseState.available
|
||||
|
||||
return mads_enabled
|
||||
|
||||
def get_sp_common_state(self, cs_out, CS, gear_allowed=True, gap_button=False):
|
||||
cs_out.cruiseState.enabled = CS.accEnabled if not self.CP.pcmCruise or not self.CP.pcmCruiseSpeed else cs_out.cruiseState.enabled
|
||||
if not self.enable_mads:
|
||||
|
||||
@@ -72,7 +72,7 @@ class CarInterface(CarInterfaceBase):
|
||||
be.pressed = self.CS.buttonStates[button]
|
||||
buttonEvents.append(be)
|
||||
|
||||
self.CS.mads_enabled = False if not (self.CS.control_initialized or self.mads_main_toggle) else ret.cruiseState.available
|
||||
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)
|
||||
|
||||
self.CS.accEnabled, buttonEvents = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
|
||||
buttonEvents, c.vCruise)
|
||||
|
||||
@@ -51,7 +51,7 @@ class CarInterface(CarInterfaceBase):
|
||||
#be.type = car.CarState.ButtonEvent.Type.accelCruise
|
||||
#buttonEvents.append(be)
|
||||
|
||||
self.CS.mads_enabled = False if not (self.CS.control_initialized or self.mads_main_toggle) else ret.cruiseState.available
|
||||
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)
|
||||
|
||||
if ret.cruiseState.available:
|
||||
if self.enable_mads:
|
||||
|
||||
@@ -125,7 +125,7 @@ class CarInterface(CarInterfaceBase):
|
||||
|
||||
buttonEvents = []
|
||||
|
||||
self.CS.mads_enabled = False if not (self.CS.control_initialized or self.mads_main_toggle) else ret.cruiseState.available
|
||||
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)
|
||||
|
||||
if ret.cruiseState.available:
|
||||
if self.enable_mads:
|
||||
|
||||
@@ -263,20 +263,21 @@ class CarInterface(CarInterfaceBase):
|
||||
if self.CS.gap_dist_button != self.CS.prev_gap_dist_button:
|
||||
buttonEvents.append(create_button_event(self.CS.gap_dist_button, self.CS.prev_gap_dist_button, {1: ButtonType.gapAdjustCruise}))
|
||||
|
||||
self.CS.mads_enabled = False if not (self.CS.control_initialized or self.mads_main_toggle) else ret.cruiseState.available
|
||||
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)
|
||||
|
||||
if ret.cruiseState.available:
|
||||
if self.enable_mads:
|
||||
if not self.CS.prev_mads_enabled and self.CS.mads_enabled:
|
||||
self.CS.madsEnabled = True
|
||||
if self.CS.lta_status_active:
|
||||
if (self.CS.prev_lkas_enabled == 16 and self.CS.lkas_enabled == 0) or \
|
||||
(self.CS.prev_lkas_enabled == 0 and self.CS.lkas_enabled == 16):
|
||||
self.CS.madsEnabled = not self.CS.madsEnabled
|
||||
else:
|
||||
if (not self.CS.prev_lkas_enabled and self.CS.lkas_enabled) or \
|
||||
(self.CS.prev_lkas_enabled == 1 and not self.CS.lkas_enabled):
|
||||
self.CS.madsEnabled = not self.CS.madsEnabled
|
||||
if self.lkas_toggle:
|
||||
if self.CS.lta_status_active:
|
||||
if (self.CS.prev_lkas_enabled == 16 and self.CS.lkas_enabled == 0) or \
|
||||
(self.CS.prev_lkas_enabled == 0 and self.CS.lkas_enabled == 16):
|
||||
self.CS.madsEnabled = not self.CS.madsEnabled
|
||||
else:
|
||||
if (not self.CS.prev_lkas_enabled and self.CS.lkas_enabled) or \
|
||||
(self.CS.prev_lkas_enabled == 1 and not self.CS.lkas_enabled):
|
||||
self.CS.madsEnabled = not self.CS.madsEnabled
|
||||
self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
|
||||
if not (self.CP.openpilotLongitudinalControl or self.gac):
|
||||
ret.gapAdjustCruiseTr = 3
|
||||
|
||||
@@ -239,7 +239,7 @@ class CarInterface(CarInterfaceBase):
|
||||
be.pressed = self.CS.buttonStates[button]
|
||||
buttonEvents.append(be)
|
||||
|
||||
self.CS.mads_enabled = False if not (self.CS.control_initialized or self.mads_main_toggle) else ret.cruiseState.available
|
||||
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)
|
||||
|
||||
self.CS.accEnabled, buttonEvents = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
|
||||
buttonEvents, c.vCruise,
|
||||
|
||||
@@ -48,6 +48,7 @@ class DesireHelper:
|
||||
self.road_edge = False
|
||||
self.count = 0
|
||||
self.edge_toggle = self.param_s.get("RoadEdge")
|
||||
self.lane_change_bsm_delay = self.param_s.get_bool("AutoLaneChangeBsmDelay")
|
||||
|
||||
def update(self, carstate, lateral_active, lane_change_prob, model_data):
|
||||
lane_change_set_timer = int(self.param_s.get("AutoLaneChangeTimer", encoding="utf8"))
|
||||
@@ -114,6 +115,13 @@ class DesireHelper:
|
||||
(carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
|
||||
|
||||
self.lane_change_wait_timer += DT_MDL
|
||||
|
||||
if self.lane_change_bsm_delay and blindspot_detected and lane_change_auto_timer:
|
||||
if lane_change_auto_timer == 0.1:
|
||||
self.lane_change_wait_timer = -1
|
||||
else:
|
||||
self.lane_change_wait_timer = lane_change_auto_timer - 1
|
||||
|
||||
if not one_blinker or below_lane_change_speed:
|
||||
self.lane_change_state = LaneChangeState.off
|
||||
self.prev_lane_change = False
|
||||
|
||||
@@ -124,9 +124,14 @@ class VCruiseHelper:
|
||||
if button_type is None:
|
||||
return
|
||||
|
||||
resume_button = ButtonType.accelCruise
|
||||
if not self.CP.pcmCruiseSpeed:
|
||||
if self.CP.carName == "chrysler":
|
||||
resume_button = ButtonType.resumeCruise
|
||||
|
||||
# Don't adjust speed when pressing resume to exit standstill
|
||||
cruise_standstill = self.button_change_states[button_type]["standstill"] or CS.cruiseState.standstill
|
||||
if button_type == ButtonType.accelCruise and cruise_standstill:
|
||||
if button_type == resume_button and cruise_standstill:
|
||||
return
|
||||
|
||||
# Don't adjust speed if we've enabled since the button was depressed (some ports enable on rising edge)
|
||||
|
||||
@@ -660,7 +660,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
|
||||
ET.PERMANENT: Alert(
|
||||
"",
|
||||
"",
|
||||
AlertStatus.userPrompt, AlertSize.none,
|
||||
AlertStatus.normal, AlertSize.none,
|
||||
Priority.MID, VisualAlert.none, AudibleAlert.promptStarting, 1.5),
|
||||
},
|
||||
|
||||
|
||||
@@ -1,608 +0,0 @@
|
||||
{
|
||||
"acados_include_path": "/data/openpilot-special/third_party/acados/include/acados/include",
|
||||
"acados_lib_path": "/data/openpilot-special/third_party/acados/include/acados/lib",
|
||||
"code_export_directory": "/data/openpilot-special/selfdrive/controls/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
|
||||
],
|
||||
[
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0
|
||||
]
|
||||
],
|
||||
"Vu_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.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
|
||||
],
|
||||
[
|
||||
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
|
||||
],
|
||||
[
|
||||
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,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
]
|
||||
],
|
||||
"W_0": [
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.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,
|
||||
0.0,
|
||||
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,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"yref_0": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"yref_e": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"zl": [],
|
||||
"zl_e": [],
|
||||
"zu": [],
|
||||
"zu_e": []
|
||||
},
|
||||
"cython_include_dirs": "/usr/local/pyenv/versions/3.8.10/lib/python3.8/site-packages/numpy/core/include",
|
||||
"dims": {
|
||||
"N": 32,
|
||||
"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": 5,
|
||||
"ny_0": 5,
|
||||
"ny_e": 3,
|
||||
"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,
|
||||
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,
|
||||
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,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1
|
||||
],
|
||||
"tf": 10.0,
|
||||
"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,
|
||||
0.322265625,
|
||||
0.341796875,
|
||||
0.361328125,
|
||||
0.380859375,
|
||||
0.400390625,
|
||||
0.419921875,
|
||||
0.439453125,
|
||||
0.458984375,
|
||||
0.478515625,
|
||||
0.498046875,
|
||||
0.517578125,
|
||||
0.537109375,
|
||||
0.556640625,
|
||||
0.576171875,
|
||||
0.595703125,
|
||||
0.615234375
|
||||
]
|
||||
}
|
||||
}
|
||||
@@ -1,278 +0,0 @@
|
||||
/*
|
||||
* 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;
|
||||
};
|
||||
|
||||
@@ -1,103 +0,0 @@
|
||||
/*
|
||||
* 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_
|
||||
-264
@@ -1,264 +0,0 @@
|
||||
/*
|
||||
* 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, (32+1) * 2);
|
||||
// y_ref_0
|
||||
ssSetInputPortVectorDimension(S, 3, 5);
|
||||
// y_ref
|
||||
ssSetInputPortVectorDimension(S, 4, 155);
|
||||
// y_ref_e
|
||||
ssSetInputPortVectorDimension(S, 5, 3);
|
||||
// lbx
|
||||
ssSetInputPortVectorDimension(S, 6, 62);
|
||||
// ubx
|
||||
ssSetInputPortVectorDimension(S, 7, 62);/* 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[5];
|
||||
|
||||
/* 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 <= 32; 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 < 5; 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 < 32; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < 5; jj++)
|
||||
buffer[jj] = (double)(*in_sign[(ii-1)*5+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 < 3; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 32, "yref", (void *) buffer);
|
||||
// lbx
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 6);
|
||||
for (int ii = 1; ii < 32; 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 < 32; 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
|
||||
@@ -1,128 +0,0 @@
|
||||
/*
|
||||
* 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;
|
||||
}
|
||||
@@ -1,125 +0,0 @@
|
||||
%
|
||||
% 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-special/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-special/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 [66]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [5]\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 [155]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [3]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') lbx for shooting nodes 1 to N-1, size [62]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') ubx for shooting nodes 1 to N-1, size [62]\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)
|
||||
@@ -1,627 +0,0 @@
|
||||
{
|
||||
"acados_include_path": "/data/openpilot-special/third_party/acados/include/acados/include",
|
||||
"acados_lib_path": "/data/openpilot-special/third_party/acados/include/acados/lib",
|
||||
"code_export_directory": "/data/openpilot-special/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code",
|
||||
"constraints": {
|
||||
"C": [],
|
||||
"C_e": [],
|
||||
"D": [],
|
||||
"constr_type": "BGH",
|
||||
"constr_type_e": "BGH",
|
||||
"idxbu": [],
|
||||
"idxbx": [],
|
||||
"idxbx_0": [
|
||||
0,
|
||||
1,
|
||||
2
|
||||
],
|
||||
"idxbx_e": [],
|
||||
"idxbxe_0": [
|
||||
0,
|
||||
1,
|
||||
2
|
||||
],
|
||||
"idxsbu": [],
|
||||
"idxsbx": [],
|
||||
"idxsbx_e": [],
|
||||
"idxsg": [],
|
||||
"idxsg_e": [],
|
||||
"idxsh": [
|
||||
0,
|
||||
1,
|
||||
2,
|
||||
3
|
||||
],
|
||||
"idxsh_e": [],
|
||||
"idxsphi": [],
|
||||
"idxsphi_e": [],
|
||||
"lbu": [],
|
||||
"lbx": [],
|
||||
"lbx_0": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"lbx_e": [],
|
||||
"lg": [],
|
||||
"lg_e": [],
|
||||
"lh": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"lh_e": [],
|
||||
"lphi": [],
|
||||
"lphi_e": [],
|
||||
"lsbu": [],
|
||||
"lsbx": [],
|
||||
"lsbx_e": [],
|
||||
"lsg": [],
|
||||
"lsg_e": [],
|
||||
"lsh": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"lsh_e": [],
|
||||
"lsphi": [],
|
||||
"lsphi_e": [],
|
||||
"ubu": [],
|
||||
"ubx": [],
|
||||
"ubx_0": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"ubx_e": [],
|
||||
"ug": [],
|
||||
"ug_e": [],
|
||||
"uh": [
|
||||
10000.0,
|
||||
10000.0,
|
||||
10000.0,
|
||||
10000.0
|
||||
],
|
||||
"uh_e": [],
|
||||
"uphi": [],
|
||||
"uphi_e": [],
|
||||
"usbu": [],
|
||||
"usbx": [],
|
||||
"usbx_e": [],
|
||||
"usg": [],
|
||||
"usg_e": [],
|
||||
"ush": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"ush_e": [],
|
||||
"usphi": [],
|
||||
"usphi_e": []
|
||||
},
|
||||
"cost": {
|
||||
"Vu": [
|
||||
[
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0
|
||||
]
|
||||
],
|
||||
"Vu_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.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
|
||||
],
|
||||
[
|
||||
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,
|
||||
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,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
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,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.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,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
]
|
||||
],
|
||||
"Zl": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"Zl_e": [],
|
||||
"Zu": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"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,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"yref_0": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"yref_e": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"zl": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"zl_e": [],
|
||||
"zu": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"zu_e": []
|
||||
},
|
||||
"cython_include_dirs": "/usr/local/pyenv/versions/3.8.10/lib/python3.8/site-packages/numpy/core/include",
|
||||
"dims": {
|
||||
"N": 12,
|
||||
"nbu": 0,
|
||||
"nbx": 0,
|
||||
"nbx_0": 3,
|
||||
"nbx_e": 0,
|
||||
"nbxe_0": 3,
|
||||
"ng": 0,
|
||||
"ng_e": 0,
|
||||
"nh": 4,
|
||||
"nh_e": 0,
|
||||
"np": 6,
|
||||
"nphi": 0,
|
||||
"nphi_e": 0,
|
||||
"nr": 0,
|
||||
"nr_e": 0,
|
||||
"ns": 4,
|
||||
"ns_e": 0,
|
||||
"nsbu": 0,
|
||||
"nsbx": 0,
|
||||
"nsbx_e": 0,
|
||||
"nsg": 0,
|
||||
"nsg_e": 0,
|
||||
"nsh": 4,
|
||||
"nsh_e": 0,
|
||||
"nsphi": 0,
|
||||
"nsphi_e": 0,
|
||||
"nu": 1,
|
||||
"nx": 3,
|
||||
"ny": 6,
|
||||
"ny_0": 6,
|
||||
"ny_e": 5,
|
||||
"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": "long"
|
||||
},
|
||||
"parameter_values": [
|
||||
-1.2,
|
||||
1.2,
|
||||
0.0,
|
||||
0.0,
|
||||
1.45,
|
||||
0.75
|
||||
],
|
||||
"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.06944444444444445,
|
||||
"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": 10,
|
||||
"qp_solver_tol_comp": 0.001,
|
||||
"qp_solver_tol_eq": 0.001,
|
||||
"qp_solver_tol_ineq": 0.001,
|
||||
"qp_solver_tol_stat": 0.001,
|
||||
"qp_solver_warm_start": 0,
|
||||
"regularize_method": null,
|
||||
"sim_method_jac_reuse": [
|
||||
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
|
||||
],
|
||||
"sim_method_num_steps": [
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1
|
||||
],
|
||||
"tf": 10.0,
|
||||
"time_steps": [
|
||||
0.06944444444444445,
|
||||
0.20833333333333334,
|
||||
0.3472222222222222,
|
||||
0.48611111111111116,
|
||||
0.6250000000000002,
|
||||
0.7638888888888886,
|
||||
0.9027777777777786,
|
||||
1.041666666666666,
|
||||
1.1805555555555554,
|
||||
1.3194444444444455,
|
||||
1.4583333333333313,
|
||||
1.5972222222222232
|
||||
]
|
||||
}
|
||||
}
|
||||
Binary file not shown.
-282
@@ -1,282 +0,0 @@
|
||||
/*
|
||||
* 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 "long_model/long_model.h"
|
||||
#include "acados_sim_solver_long.h"
|
||||
|
||||
|
||||
// ** solver data **
|
||||
|
||||
sim_solver_capsule * long_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 long_acados_sim_solver_free_capsule(sim_solver_capsule * capsule)
|
||||
{
|
||||
free(capsule);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int long_acados_sim_create(sim_solver_capsule * capsule)
|
||||
{
|
||||
// initialize
|
||||
const int nx = LONG_NX;
|
||||
const int nu = LONG_NU;
|
||||
const int nz = LONG_NZ;
|
||||
const int np = LONG_NP;
|
||||
bool tmp_bool;
|
||||
|
||||
|
||||
double Tsim = 0.06944444444444445;
|
||||
|
||||
|
||||
// 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 = &long_expl_vde_forw;
|
||||
capsule->sim_forw_vde_casadi->casadi_n_in = &long_expl_vde_forw_n_in;
|
||||
capsule->sim_forw_vde_casadi->casadi_n_out = &long_expl_vde_forw_n_out;
|
||||
capsule->sim_forw_vde_casadi->casadi_sparsity_in = &long_expl_vde_forw_sparsity_in;
|
||||
capsule->sim_forw_vde_casadi->casadi_sparsity_out = &long_expl_vde_forw_sparsity_out;
|
||||
capsule->sim_forw_vde_casadi->casadi_work = &long_expl_vde_forw_work;
|
||||
external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np);
|
||||
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_fun = &long_expl_ode_fun;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_n_in = &long_expl_ode_fun_n_in;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_n_out = &long_expl_ode_fun_n_out;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &long_expl_ode_fun_sparsity_in;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &long_expl_ode_fun_sparsity_out;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_work = &long_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 * long_sim_config = sim_config_create(plan);
|
||||
capsule->acados_sim_config = long_sim_config;
|
||||
|
||||
// sim dims
|
||||
void *long_sim_dims = sim_dims_create(long_sim_config);
|
||||
capsule->acados_sim_dims = long_sim_dims;
|
||||
sim_dims_set(long_sim_config, long_sim_dims, "nx", &nx);
|
||||
sim_dims_set(long_sim_config, long_sim_dims, "nu", &nu);
|
||||
sim_dims_set(long_sim_config, long_sim_dims, "nz", &nz);
|
||||
|
||||
|
||||
// sim opts
|
||||
sim_opts *long_sim_opts = sim_opts_create(long_sim_config, long_sim_dims);
|
||||
capsule->acados_sim_opts = long_sim_opts;
|
||||
int tmp_int = 3;
|
||||
sim_opts_set(long_sim_config, long_sim_opts, "newton_iter", &tmp_int);
|
||||
sim_collocation_type collocation_type = GAUSS_LEGENDRE;
|
||||
sim_opts_set(long_sim_config, long_sim_opts, "collocation_type", &collocation_type);
|
||||
|
||||
|
||||
tmp_int = 4;
|
||||
sim_opts_set(long_sim_config, long_sim_opts, "num_stages", &tmp_int);
|
||||
tmp_int = 1;
|
||||
sim_opts_set(long_sim_config, long_sim_opts, "num_steps", &tmp_int);
|
||||
tmp_bool = 0;
|
||||
sim_opts_set(long_sim_config, long_sim_opts, "jac_reuse", &tmp_bool);
|
||||
|
||||
|
||||
// sim in / out
|
||||
sim_in *long_sim_in = sim_in_create(long_sim_config, long_sim_dims);
|
||||
capsule->acados_sim_in = long_sim_in;
|
||||
sim_out *long_sim_out = sim_out_create(long_sim_config, long_sim_dims);
|
||||
capsule->acados_sim_out = long_sim_out;
|
||||
|
||||
sim_in_set(long_sim_config, long_sim_dims,
|
||||
long_sim_in, "T", &Tsim);
|
||||
|
||||
// model functions
|
||||
long_sim_config->model_set(long_sim_in->model,
|
||||
"expl_vde_for", capsule->sim_forw_vde_casadi);
|
||||
long_sim_config->model_set(long_sim_in->model,
|
||||
"expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
|
||||
|
||||
// sim solver
|
||||
sim_solver *long_sim_solver = sim_solver_create(long_sim_config,
|
||||
long_sim_dims, long_sim_opts);
|
||||
capsule->acados_sim_solver = long_sim_solver;
|
||||
|
||||
|
||||
/* initialize parameter values */
|
||||
double* p = calloc(np, sizeof(double));
|
||||
|
||||
p[0] = -1.2;
|
||||
p[1] = 1.2;
|
||||
p[4] = 1.45;
|
||||
p[5] = 0.75;
|
||||
|
||||
long_acados_sim_update_params(capsule, p, np);
|
||||
free(p);
|
||||
|
||||
|
||||
/* initialize input */
|
||||
// x
|
||||
double x0[3];
|
||||
for (int ii = 0; ii < 3; ii++)
|
||||
x0[ii] = 0.0;
|
||||
|
||||
sim_in_set(long_sim_config, long_sim_dims,
|
||||
long_sim_in, "x", x0);
|
||||
|
||||
|
||||
// u
|
||||
double u0[1];
|
||||
for (int ii = 0; ii < 1; ii++)
|
||||
u0[ii] = 0.0;
|
||||
|
||||
sim_in_set(long_sim_config, long_sim_dims,
|
||||
long_sim_in, "u", u0);
|
||||
|
||||
// S_forw
|
||||
double S_forw[12];
|
||||
for (int ii = 0; ii < 12; ii++)
|
||||
S_forw[ii] = 0.0;
|
||||
for (int ii = 0; ii < 3; ii++)
|
||||
S_forw[ii + ii * 3 ] = 1.0;
|
||||
|
||||
|
||||
sim_in_set(long_sim_config, long_sim_dims,
|
||||
long_sim_in, "S_forw", S_forw);
|
||||
|
||||
int status = sim_precompute(long_sim_solver, long_sim_in, long_sim_out);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
int long_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 long_acados_sim_solve()! Exiting.\n");
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
int long_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 long_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np)
|
||||
{
|
||||
int status = 0;
|
||||
int casadi_np = LONG_NP;
|
||||
|
||||
if (casadi_np != np) {
|
||||
printf("long_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 * long_acados_get_sim_config(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_config;
|
||||
};
|
||||
|
||||
sim_in * long_acados_get_sim_in(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_in;
|
||||
};
|
||||
|
||||
sim_out * long_acados_get_sim_out(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_out;
|
||||
};
|
||||
|
||||
void * long_acados_get_sim_dims(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_dims;
|
||||
};
|
||||
|
||||
sim_opts * long_acados_get_sim_opts(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_opts;
|
||||
};
|
||||
|
||||
sim_solver * long_acados_get_sim_solver(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_solver;
|
||||
};
|
||||
|
||||
-103
@@ -1,103 +0,0 @@
|
||||
/*
|
||||
* 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_long_H_
|
||||
#define ACADOS_SIM_long_H_
|
||||
|
||||
#include "acados_c/sim_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
|
||||
#define LONG_NX 3
|
||||
#define LONG_NZ 0
|
||||
#define LONG_NU 1
|
||||
#define LONG_NP 6
|
||||
|
||||
#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 long_acados_sim_create(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_sim_solve(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_sim_free(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np);
|
||||
|
||||
ACADOS_SYMBOL_EXPORT sim_config * long_acados_get_sim_config(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT sim_in * long_acados_get_sim_in(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT sim_out * long_acados_get_sim_out(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT void * long_acados_get_sim_dims(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT sim_opts * long_acados_get_sim_opts(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT sim_solver * long_acados_get_sim_solver(sim_solver_capsule *capsule);
|
||||
|
||||
|
||||
ACADOS_SYMBOL_EXPORT sim_solver_capsule * long_acados_sim_solver_create_capsule(void);
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_sim_solver_free_capsule(sim_solver_capsule *capsule);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_SIM_long_H_
|
||||
-264
@@ -1,264 +0,0 @@
|
||||
/*
|
||||
* 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_long
|
||||
#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 "long_model/long_model.h"
|
||||
#include "acados_solver_long.h"
|
||||
|
||||
#include "simstruc.h"
|
||||
|
||||
#define SAMPLINGTIME 0.06944444444444445
|
||||
|
||||
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, 3);
|
||||
// ubx_0
|
||||
ssSetInputPortVectorDimension(S, 1, 3);
|
||||
// parameters
|
||||
ssSetInputPortVectorDimension(S, 2, (12+1) * 6);
|
||||
// y_ref_0
|
||||
ssSetInputPortVectorDimension(S, 3, 6);
|
||||
// y_ref
|
||||
ssSetInputPortVectorDimension(S, 4, 66);
|
||||
// y_ref_e
|
||||
ssSetInputPortVectorDimension(S, 5, 5);
|
||||
// lh
|
||||
ssSetInputPortVectorDimension(S, 6, 4);
|
||||
// uh
|
||||
ssSetInputPortVectorDimension(S, 7, 4);/* specify dimension information for the OUTPUT ports */
|
||||
ssSetOutputPortVectorDimension(S, 0, 1 );
|
||||
ssSetOutputPortVectorDimension(S, 1, 1 );
|
||||
ssSetOutputPortVectorDimension(S, 2, 1 );
|
||||
ssSetOutputPortVectorDimension(S, 3, 3 ); // 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)
|
||||
{
|
||||
long_solver_capsule *capsule = long_acados_create_capsule();
|
||||
long_acados_create(capsule);
|
||||
|
||||
ssSetUserData(S, (void*)capsule);
|
||||
}
|
||||
|
||||
|
||||
static void mdlOutputs(SimStruct *S, int_T tid)
|
||||
{
|
||||
long_solver_capsule *capsule = ssGetUserData(S);
|
||||
ocp_nlp_config *nlp_config = long_acados_get_nlp_config(capsule);
|
||||
ocp_nlp_dims *nlp_dims = long_acados_get_nlp_dims(capsule);
|
||||
ocp_nlp_in *nlp_in = long_acados_get_nlp_in(capsule);
|
||||
ocp_nlp_out *nlp_out = long_acados_get_nlp_out(capsule);
|
||||
|
||||
InputRealPtrsType in_sign;
|
||||
|
||||
// local buffer
|
||||
real_t buffer[6];
|
||||
|
||||
/* go through inputs */
|
||||
// lbx_0
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 0);
|
||||
for (int i = 0; i < 3; 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 < 3; 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 <= 12; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < 6; jj++)
|
||||
buffer[jj] = (double)(*in_sign[ii*6+jj]);
|
||||
long_acados_update_params(capsule, ii, buffer, 6);
|
||||
}
|
||||
|
||||
|
||||
// y_ref_0
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 3);
|
||||
|
||||
for (int i = 0; i < 6; 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 < 12; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < 6; jj++)
|
||||
buffer[jj] = (double)(*in_sign[(ii-1)*6+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 < 5; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 12, "yref", (void *) buffer);
|
||||
// lh
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 6);
|
||||
|
||||
for (int i = 0; i < 4; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
for (int ii = 0; ii < 12; ii++)
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lh", buffer);
|
||||
// uh
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 7);
|
||||
|
||||
for (int i = 0; i < 4; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
for (int ii = 0; ii < 12; ii++)
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "uh", buffer);
|
||||
|
||||
/* call solver */
|
||||
int rti_phase = 0;
|
||||
ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase);
|
||||
int acados_status = long_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)
|
||||
{
|
||||
long_solver_capsule *capsule = ssGetUserData(S);
|
||||
|
||||
long_acados_free(capsule);
|
||||
long_acados_free_capsule(capsule);
|
||||
}
|
||||
|
||||
|
||||
#ifdef MATLAB_MEX_FILE
|
||||
#include "simulink.c"
|
||||
#else
|
||||
#include "cg_sfun.h"
|
||||
#endif
|
||||
@@ -1,130 +0,0 @@
|
||||
/*
|
||||
* 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_long.h"
|
||||
|
||||
#define NX LONG_NX
|
||||
#define NZ LONG_NZ
|
||||
#define NU LONG_NU
|
||||
#define NP LONG_NP
|
||||
|
||||
|
||||
int main()
|
||||
{
|
||||
int status = 0;
|
||||
sim_solver_capsule *capsule = long_acados_sim_solver_create_capsule();
|
||||
status = long_acados_sim_create(capsule);
|
||||
|
||||
if (status)
|
||||
{
|
||||
printf("acados_create() returned status %d. Exiting.\n", status);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
sim_config *acados_sim_config = long_acados_get_sim_config(capsule);
|
||||
sim_in *acados_sim_in = long_acados_get_sim_in(capsule);
|
||||
sim_out *acados_sim_out = long_acados_get_sim_out(capsule);
|
||||
void *acados_sim_dims = long_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[0] = 0;
|
||||
x_current[1] = 0;
|
||||
x_current[2] = 0;
|
||||
|
||||
|
||||
|
||||
|
||||
// initial value for control input
|
||||
double u0[NU];
|
||||
u0[0] = 0.0;
|
||||
// set parameters
|
||||
double p[NP];
|
||||
p[0] = -1.2;
|
||||
p[1] = 1.2;
|
||||
p[2] = 0;
|
||||
p[3] = 0;
|
||||
p[4] = 1.45;
|
||||
p[5] = 0.75;
|
||||
|
||||
long_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 = long_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 = long_acados_sim_free(capsule);
|
||||
if (status) {
|
||||
printf("long_acados_sim_free() returned status %d. \n", status);
|
||||
}
|
||||
|
||||
long_acados_sim_solver_free_capsule(capsule);
|
||||
|
||||
return status;
|
||||
}
|
||||
@@ -1,128 +0,0 @@
|
||||
%
|
||||
% 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 = { ...
|
||||
'long_model/long_expl_ode_fun.c', ...
|
||||
'long_model/long_expl_vde_forw.c',...
|
||||
'long_cost/long_cost_y_0_fun.c',...
|
||||
'long_cost/long_cost_y_0_fun_jac_ut_xt.c',...
|
||||
'long_cost/long_cost_y_0_hess.c',...
|
||||
'long_cost/long_cost_y_fun.c',...
|
||||
'long_cost/long_cost_y_fun_jac_ut_xt.c',...
|
||||
'long_cost/long_cost_y_hess.c',...
|
||||
'long_cost/long_cost_y_e_fun.c',...
|
||||
'long_cost/long_cost_y_e_fun_jac_ut_xt.c',...
|
||||
'long_cost/long_cost_y_e_hess.c',...
|
||||
'long_constraints/long_constr_h_fun.c', ...
|
||||
'long_constraints/long_constr_h_fun_jac_uxt_zt_hess.c', ...
|
||||
'long_constraints/long_constr_h_fun_jac_uxt_zt.c', ...
|
||||
'acados_solver_sfunction_long.c', ...
|
||||
'acados_solver_long.c'
|
||||
};
|
||||
|
||||
INC_PATH = '/data/openpilot-special/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-special/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_long' );
|
||||
|
||||
fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_long', '.', ...
|
||||
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 [3]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
|
||||
' size [3]\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 [78]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [6]\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 [66]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [5]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') lh, size [4]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') uh, size [4]\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)
|
||||
Binary file not shown.
@@ -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_3273042440096814304) {
|
||||
out_3273042440096814304[0] = delta_x[0] + nom_x[0];
|
||||
out_3273042440096814304[1] = delta_x[1] + nom_x[1];
|
||||
out_3273042440096814304[2] = delta_x[2] + nom_x[2];
|
||||
out_3273042440096814304[3] = delta_x[3] + nom_x[3];
|
||||
out_3273042440096814304[4] = delta_x[4] + nom_x[4];
|
||||
out_3273042440096814304[5] = delta_x[5] + nom_x[5];
|
||||
out_3273042440096814304[6] = delta_x[6] + nom_x[6];
|
||||
out_3273042440096814304[7] = delta_x[7] + nom_x[7];
|
||||
out_3273042440096814304[8] = delta_x[8] + nom_x[8];
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_2634337573502312531) {
|
||||
out_2634337573502312531[0] = delta_x[0] + nom_x[0];
|
||||
out_2634337573502312531[1] = delta_x[1] + nom_x[1];
|
||||
out_2634337573502312531[2] = delta_x[2] + nom_x[2];
|
||||
out_2634337573502312531[3] = delta_x[3] + nom_x[3];
|
||||
out_2634337573502312531[4] = delta_x[4] + nom_x[4];
|
||||
out_2634337573502312531[5] = delta_x[5] + nom_x[5];
|
||||
out_2634337573502312531[6] = delta_x[6] + nom_x[6];
|
||||
out_2634337573502312531[7] = delta_x[7] + nom_x[7];
|
||||
out_2634337573502312531[8] = delta_x[8] + nom_x[8];
|
||||
}
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_5732845985013317398) {
|
||||
out_5732845985013317398[0] = -nom_x[0] + true_x[0];
|
||||
out_5732845985013317398[1] = -nom_x[1] + true_x[1];
|
||||
out_5732845985013317398[2] = -nom_x[2] + true_x[2];
|
||||
out_5732845985013317398[3] = -nom_x[3] + true_x[3];
|
||||
out_5732845985013317398[4] = -nom_x[4] + true_x[4];
|
||||
out_5732845985013317398[5] = -nom_x[5] + true_x[5];
|
||||
out_5732845985013317398[6] = -nom_x[6] + true_x[6];
|
||||
out_5732845985013317398[7] = -nom_x[7] + true_x[7];
|
||||
out_5732845985013317398[8] = -nom_x[8] + true_x[8];
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_364624501276122878) {
|
||||
out_364624501276122878[0] = -nom_x[0] + true_x[0];
|
||||
out_364624501276122878[1] = -nom_x[1] + true_x[1];
|
||||
out_364624501276122878[2] = -nom_x[2] + true_x[2];
|
||||
out_364624501276122878[3] = -nom_x[3] + true_x[3];
|
||||
out_364624501276122878[4] = -nom_x[4] + true_x[4];
|
||||
out_364624501276122878[5] = -nom_x[5] + true_x[5];
|
||||
out_364624501276122878[6] = -nom_x[6] + true_x[6];
|
||||
out_364624501276122878[7] = -nom_x[7] + true_x[7];
|
||||
out_364624501276122878[8] = -nom_x[8] + true_x[8];
|
||||
}
|
||||
void H_mod_fun(double *state, double *out_421472006259524810) {
|
||||
out_421472006259524810[0] = 1.0;
|
||||
out_421472006259524810[1] = 0;
|
||||
out_421472006259524810[2] = 0;
|
||||
out_421472006259524810[3] = 0;
|
||||
out_421472006259524810[4] = 0;
|
||||
out_421472006259524810[5] = 0;
|
||||
out_421472006259524810[6] = 0;
|
||||
out_421472006259524810[7] = 0;
|
||||
out_421472006259524810[8] = 0;
|
||||
out_421472006259524810[9] = 0;
|
||||
out_421472006259524810[10] = 1.0;
|
||||
out_421472006259524810[11] = 0;
|
||||
out_421472006259524810[12] = 0;
|
||||
out_421472006259524810[13] = 0;
|
||||
out_421472006259524810[14] = 0;
|
||||
out_421472006259524810[15] = 0;
|
||||
out_421472006259524810[16] = 0;
|
||||
out_421472006259524810[17] = 0;
|
||||
out_421472006259524810[18] = 0;
|
||||
out_421472006259524810[19] = 0;
|
||||
out_421472006259524810[20] = 1.0;
|
||||
out_421472006259524810[21] = 0;
|
||||
out_421472006259524810[22] = 0;
|
||||
out_421472006259524810[23] = 0;
|
||||
out_421472006259524810[24] = 0;
|
||||
out_421472006259524810[25] = 0;
|
||||
out_421472006259524810[26] = 0;
|
||||
out_421472006259524810[27] = 0;
|
||||
out_421472006259524810[28] = 0;
|
||||
out_421472006259524810[29] = 0;
|
||||
out_421472006259524810[30] = 1.0;
|
||||
out_421472006259524810[31] = 0;
|
||||
out_421472006259524810[32] = 0;
|
||||
out_421472006259524810[33] = 0;
|
||||
out_421472006259524810[34] = 0;
|
||||
out_421472006259524810[35] = 0;
|
||||
out_421472006259524810[36] = 0;
|
||||
out_421472006259524810[37] = 0;
|
||||
out_421472006259524810[38] = 0;
|
||||
out_421472006259524810[39] = 0;
|
||||
out_421472006259524810[40] = 1.0;
|
||||
out_421472006259524810[41] = 0;
|
||||
out_421472006259524810[42] = 0;
|
||||
out_421472006259524810[43] = 0;
|
||||
out_421472006259524810[44] = 0;
|
||||
out_421472006259524810[45] = 0;
|
||||
out_421472006259524810[46] = 0;
|
||||
out_421472006259524810[47] = 0;
|
||||
out_421472006259524810[48] = 0;
|
||||
out_421472006259524810[49] = 0;
|
||||
out_421472006259524810[50] = 1.0;
|
||||
out_421472006259524810[51] = 0;
|
||||
out_421472006259524810[52] = 0;
|
||||
out_421472006259524810[53] = 0;
|
||||
out_421472006259524810[54] = 0;
|
||||
out_421472006259524810[55] = 0;
|
||||
out_421472006259524810[56] = 0;
|
||||
out_421472006259524810[57] = 0;
|
||||
out_421472006259524810[58] = 0;
|
||||
out_421472006259524810[59] = 0;
|
||||
out_421472006259524810[60] = 1.0;
|
||||
out_421472006259524810[61] = 0;
|
||||
out_421472006259524810[62] = 0;
|
||||
out_421472006259524810[63] = 0;
|
||||
out_421472006259524810[64] = 0;
|
||||
out_421472006259524810[65] = 0;
|
||||
out_421472006259524810[66] = 0;
|
||||
out_421472006259524810[67] = 0;
|
||||
out_421472006259524810[68] = 0;
|
||||
out_421472006259524810[69] = 0;
|
||||
out_421472006259524810[70] = 1.0;
|
||||
out_421472006259524810[71] = 0;
|
||||
out_421472006259524810[72] = 0;
|
||||
out_421472006259524810[73] = 0;
|
||||
out_421472006259524810[74] = 0;
|
||||
out_421472006259524810[75] = 0;
|
||||
out_421472006259524810[76] = 0;
|
||||
out_421472006259524810[77] = 0;
|
||||
out_421472006259524810[78] = 0;
|
||||
out_421472006259524810[79] = 0;
|
||||
out_421472006259524810[80] = 1.0;
|
||||
void H_mod_fun(double *state, double *out_8283385258124076174) {
|
||||
out_8283385258124076174[0] = 1.0;
|
||||
out_8283385258124076174[1] = 0;
|
||||
out_8283385258124076174[2] = 0;
|
||||
out_8283385258124076174[3] = 0;
|
||||
out_8283385258124076174[4] = 0;
|
||||
out_8283385258124076174[5] = 0;
|
||||
out_8283385258124076174[6] = 0;
|
||||
out_8283385258124076174[7] = 0;
|
||||
out_8283385258124076174[8] = 0;
|
||||
out_8283385258124076174[9] = 0;
|
||||
out_8283385258124076174[10] = 1.0;
|
||||
out_8283385258124076174[11] = 0;
|
||||
out_8283385258124076174[12] = 0;
|
||||
out_8283385258124076174[13] = 0;
|
||||
out_8283385258124076174[14] = 0;
|
||||
out_8283385258124076174[15] = 0;
|
||||
out_8283385258124076174[16] = 0;
|
||||
out_8283385258124076174[17] = 0;
|
||||
out_8283385258124076174[18] = 0;
|
||||
out_8283385258124076174[19] = 0;
|
||||
out_8283385258124076174[20] = 1.0;
|
||||
out_8283385258124076174[21] = 0;
|
||||
out_8283385258124076174[22] = 0;
|
||||
out_8283385258124076174[23] = 0;
|
||||
out_8283385258124076174[24] = 0;
|
||||
out_8283385258124076174[25] = 0;
|
||||
out_8283385258124076174[26] = 0;
|
||||
out_8283385258124076174[27] = 0;
|
||||
out_8283385258124076174[28] = 0;
|
||||
out_8283385258124076174[29] = 0;
|
||||
out_8283385258124076174[30] = 1.0;
|
||||
out_8283385258124076174[31] = 0;
|
||||
out_8283385258124076174[32] = 0;
|
||||
out_8283385258124076174[33] = 0;
|
||||
out_8283385258124076174[34] = 0;
|
||||
out_8283385258124076174[35] = 0;
|
||||
out_8283385258124076174[36] = 0;
|
||||
out_8283385258124076174[37] = 0;
|
||||
out_8283385258124076174[38] = 0;
|
||||
out_8283385258124076174[39] = 0;
|
||||
out_8283385258124076174[40] = 1.0;
|
||||
out_8283385258124076174[41] = 0;
|
||||
out_8283385258124076174[42] = 0;
|
||||
out_8283385258124076174[43] = 0;
|
||||
out_8283385258124076174[44] = 0;
|
||||
out_8283385258124076174[45] = 0;
|
||||
out_8283385258124076174[46] = 0;
|
||||
out_8283385258124076174[47] = 0;
|
||||
out_8283385258124076174[48] = 0;
|
||||
out_8283385258124076174[49] = 0;
|
||||
out_8283385258124076174[50] = 1.0;
|
||||
out_8283385258124076174[51] = 0;
|
||||
out_8283385258124076174[52] = 0;
|
||||
out_8283385258124076174[53] = 0;
|
||||
out_8283385258124076174[54] = 0;
|
||||
out_8283385258124076174[55] = 0;
|
||||
out_8283385258124076174[56] = 0;
|
||||
out_8283385258124076174[57] = 0;
|
||||
out_8283385258124076174[58] = 0;
|
||||
out_8283385258124076174[59] = 0;
|
||||
out_8283385258124076174[60] = 1.0;
|
||||
out_8283385258124076174[61] = 0;
|
||||
out_8283385258124076174[62] = 0;
|
||||
out_8283385258124076174[63] = 0;
|
||||
out_8283385258124076174[64] = 0;
|
||||
out_8283385258124076174[65] = 0;
|
||||
out_8283385258124076174[66] = 0;
|
||||
out_8283385258124076174[67] = 0;
|
||||
out_8283385258124076174[68] = 0;
|
||||
out_8283385258124076174[69] = 0;
|
||||
out_8283385258124076174[70] = 1.0;
|
||||
out_8283385258124076174[71] = 0;
|
||||
out_8283385258124076174[72] = 0;
|
||||
out_8283385258124076174[73] = 0;
|
||||
out_8283385258124076174[74] = 0;
|
||||
out_8283385258124076174[75] = 0;
|
||||
out_8283385258124076174[76] = 0;
|
||||
out_8283385258124076174[77] = 0;
|
||||
out_8283385258124076174[78] = 0;
|
||||
out_8283385258124076174[79] = 0;
|
||||
out_8283385258124076174[80] = 1.0;
|
||||
}
|
||||
void f_fun(double *state, double dt, double *out_3776724096449913501) {
|
||||
out_3776724096449913501[0] = state[0];
|
||||
out_3776724096449913501[1] = state[1];
|
||||
out_3776724096449913501[2] = state[2];
|
||||
out_3776724096449913501[3] = state[3];
|
||||
out_3776724096449913501[4] = state[4];
|
||||
out_3776724096449913501[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_3776724096449913501[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_3776724096449913501[7] = state[7];
|
||||
out_3776724096449913501[8] = state[8];
|
||||
void f_fun(double *state, double dt, double *out_1842212169269794394) {
|
||||
out_1842212169269794394[0] = state[0];
|
||||
out_1842212169269794394[1] = state[1];
|
||||
out_1842212169269794394[2] = state[2];
|
||||
out_1842212169269794394[3] = state[3];
|
||||
out_1842212169269794394[4] = state[4];
|
||||
out_1842212169269794394[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_1842212169269794394[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_1842212169269794394[7] = state[7];
|
||||
out_1842212169269794394[8] = state[8];
|
||||
}
|
||||
void F_fun(double *state, double dt, double *out_1353218831850316658) {
|
||||
out_1353218831850316658[0] = 1;
|
||||
out_1353218831850316658[1] = 0;
|
||||
out_1353218831850316658[2] = 0;
|
||||
out_1353218831850316658[3] = 0;
|
||||
out_1353218831850316658[4] = 0;
|
||||
out_1353218831850316658[5] = 0;
|
||||
out_1353218831850316658[6] = 0;
|
||||
out_1353218831850316658[7] = 0;
|
||||
out_1353218831850316658[8] = 0;
|
||||
out_1353218831850316658[9] = 0;
|
||||
out_1353218831850316658[10] = 1;
|
||||
out_1353218831850316658[11] = 0;
|
||||
out_1353218831850316658[12] = 0;
|
||||
out_1353218831850316658[13] = 0;
|
||||
out_1353218831850316658[14] = 0;
|
||||
out_1353218831850316658[15] = 0;
|
||||
out_1353218831850316658[16] = 0;
|
||||
out_1353218831850316658[17] = 0;
|
||||
out_1353218831850316658[18] = 0;
|
||||
out_1353218831850316658[19] = 0;
|
||||
out_1353218831850316658[20] = 1;
|
||||
out_1353218831850316658[21] = 0;
|
||||
out_1353218831850316658[22] = 0;
|
||||
out_1353218831850316658[23] = 0;
|
||||
out_1353218831850316658[24] = 0;
|
||||
out_1353218831850316658[25] = 0;
|
||||
out_1353218831850316658[26] = 0;
|
||||
out_1353218831850316658[27] = 0;
|
||||
out_1353218831850316658[28] = 0;
|
||||
out_1353218831850316658[29] = 0;
|
||||
out_1353218831850316658[30] = 1;
|
||||
out_1353218831850316658[31] = 0;
|
||||
out_1353218831850316658[32] = 0;
|
||||
out_1353218831850316658[33] = 0;
|
||||
out_1353218831850316658[34] = 0;
|
||||
out_1353218831850316658[35] = 0;
|
||||
out_1353218831850316658[36] = 0;
|
||||
out_1353218831850316658[37] = 0;
|
||||
out_1353218831850316658[38] = 0;
|
||||
out_1353218831850316658[39] = 0;
|
||||
out_1353218831850316658[40] = 1;
|
||||
out_1353218831850316658[41] = 0;
|
||||
out_1353218831850316658[42] = 0;
|
||||
out_1353218831850316658[43] = 0;
|
||||
out_1353218831850316658[44] = 0;
|
||||
out_1353218831850316658[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_1353218831850316658[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_1353218831850316658[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_1353218831850316658[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_1353218831850316658[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_1353218831850316658[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_1353218831850316658[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_1353218831850316658[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_1353218831850316658[53] = -9.8000000000000007*dt;
|
||||
out_1353218831850316658[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_1353218831850316658[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_1353218831850316658[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_1353218831850316658[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_1353218831850316658[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_1353218831850316658[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_1353218831850316658[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_1353218831850316658[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_1353218831850316658[62] = 0;
|
||||
out_1353218831850316658[63] = 0;
|
||||
out_1353218831850316658[64] = 0;
|
||||
out_1353218831850316658[65] = 0;
|
||||
out_1353218831850316658[66] = 0;
|
||||
out_1353218831850316658[67] = 0;
|
||||
out_1353218831850316658[68] = 0;
|
||||
out_1353218831850316658[69] = 0;
|
||||
out_1353218831850316658[70] = 1;
|
||||
out_1353218831850316658[71] = 0;
|
||||
out_1353218831850316658[72] = 0;
|
||||
out_1353218831850316658[73] = 0;
|
||||
out_1353218831850316658[74] = 0;
|
||||
out_1353218831850316658[75] = 0;
|
||||
out_1353218831850316658[76] = 0;
|
||||
out_1353218831850316658[77] = 0;
|
||||
out_1353218831850316658[78] = 0;
|
||||
out_1353218831850316658[79] = 0;
|
||||
out_1353218831850316658[80] = 1;
|
||||
void F_fun(double *state, double dt, double *out_844160246145237490) {
|
||||
out_844160246145237490[0] = 1;
|
||||
out_844160246145237490[1] = 0;
|
||||
out_844160246145237490[2] = 0;
|
||||
out_844160246145237490[3] = 0;
|
||||
out_844160246145237490[4] = 0;
|
||||
out_844160246145237490[5] = 0;
|
||||
out_844160246145237490[6] = 0;
|
||||
out_844160246145237490[7] = 0;
|
||||
out_844160246145237490[8] = 0;
|
||||
out_844160246145237490[9] = 0;
|
||||
out_844160246145237490[10] = 1;
|
||||
out_844160246145237490[11] = 0;
|
||||
out_844160246145237490[12] = 0;
|
||||
out_844160246145237490[13] = 0;
|
||||
out_844160246145237490[14] = 0;
|
||||
out_844160246145237490[15] = 0;
|
||||
out_844160246145237490[16] = 0;
|
||||
out_844160246145237490[17] = 0;
|
||||
out_844160246145237490[18] = 0;
|
||||
out_844160246145237490[19] = 0;
|
||||
out_844160246145237490[20] = 1;
|
||||
out_844160246145237490[21] = 0;
|
||||
out_844160246145237490[22] = 0;
|
||||
out_844160246145237490[23] = 0;
|
||||
out_844160246145237490[24] = 0;
|
||||
out_844160246145237490[25] = 0;
|
||||
out_844160246145237490[26] = 0;
|
||||
out_844160246145237490[27] = 0;
|
||||
out_844160246145237490[28] = 0;
|
||||
out_844160246145237490[29] = 0;
|
||||
out_844160246145237490[30] = 1;
|
||||
out_844160246145237490[31] = 0;
|
||||
out_844160246145237490[32] = 0;
|
||||
out_844160246145237490[33] = 0;
|
||||
out_844160246145237490[34] = 0;
|
||||
out_844160246145237490[35] = 0;
|
||||
out_844160246145237490[36] = 0;
|
||||
out_844160246145237490[37] = 0;
|
||||
out_844160246145237490[38] = 0;
|
||||
out_844160246145237490[39] = 0;
|
||||
out_844160246145237490[40] = 1;
|
||||
out_844160246145237490[41] = 0;
|
||||
out_844160246145237490[42] = 0;
|
||||
out_844160246145237490[43] = 0;
|
||||
out_844160246145237490[44] = 0;
|
||||
out_844160246145237490[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_844160246145237490[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_844160246145237490[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_844160246145237490[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_844160246145237490[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_844160246145237490[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_844160246145237490[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_844160246145237490[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_844160246145237490[53] = -9.8000000000000007*dt;
|
||||
out_844160246145237490[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_844160246145237490[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_844160246145237490[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_844160246145237490[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_844160246145237490[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_844160246145237490[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_844160246145237490[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_844160246145237490[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_844160246145237490[62] = 0;
|
||||
out_844160246145237490[63] = 0;
|
||||
out_844160246145237490[64] = 0;
|
||||
out_844160246145237490[65] = 0;
|
||||
out_844160246145237490[66] = 0;
|
||||
out_844160246145237490[67] = 0;
|
||||
out_844160246145237490[68] = 0;
|
||||
out_844160246145237490[69] = 0;
|
||||
out_844160246145237490[70] = 1;
|
||||
out_844160246145237490[71] = 0;
|
||||
out_844160246145237490[72] = 0;
|
||||
out_844160246145237490[73] = 0;
|
||||
out_844160246145237490[74] = 0;
|
||||
out_844160246145237490[75] = 0;
|
||||
out_844160246145237490[76] = 0;
|
||||
out_844160246145237490[77] = 0;
|
||||
out_844160246145237490[78] = 0;
|
||||
out_844160246145237490[79] = 0;
|
||||
out_844160246145237490[80] = 1;
|
||||
}
|
||||
void h_25(double *state, double *unused, double *out_1344834859822615607) {
|
||||
out_1344834859822615607[0] = state[6];
|
||||
void h_25(double *state, double *unused, double *out_1303483449461821305) {
|
||||
out_1303483449461821305[0] = state[6];
|
||||
}
|
||||
void H_25(double *state, double *unused, double *out_6507594322557232788) {
|
||||
out_6507594322557232788[0] = 0;
|
||||
out_6507594322557232788[1] = 0;
|
||||
out_6507594322557232788[2] = 0;
|
||||
out_6507594322557232788[3] = 0;
|
||||
out_6507594322557232788[4] = 0;
|
||||
out_6507594322557232788[5] = 0;
|
||||
out_6507594322557232788[6] = 1;
|
||||
out_6507594322557232788[7] = 0;
|
||||
out_6507594322557232788[8] = 0;
|
||||
void H_25(double *state, double *unused, double *out_8922345036839792704) {
|
||||
out_8922345036839792704[0] = 0;
|
||||
out_8922345036839792704[1] = 0;
|
||||
out_8922345036839792704[2] = 0;
|
||||
out_8922345036839792704[3] = 0;
|
||||
out_8922345036839792704[4] = 0;
|
||||
out_8922345036839792704[5] = 0;
|
||||
out_8922345036839792704[6] = 1;
|
||||
out_8922345036839792704[7] = 0;
|
||||
out_8922345036839792704[8] = 0;
|
||||
}
|
||||
void h_24(double *state, double *unused, double *out_5433380832945564861) {
|
||||
out_5433380832945564861[0] = state[4];
|
||||
out_5433380832945564861[1] = state[5];
|
||||
void h_24(double *state, double *unused, double *out_1550963005683787860) {
|
||||
out_1550963005683787860[0] = state[4];
|
||||
out_1550963005683787860[1] = state[5];
|
||||
}
|
||||
void H_24(double *state, double *unused, double *out_8680243921562732354) {
|
||||
out_8680243921562732354[0] = 0;
|
||||
out_8680243921562732354[1] = 0;
|
||||
out_8680243921562732354[2] = 0;
|
||||
out_8680243921562732354[3] = 0;
|
||||
out_8680243921562732354[4] = 1;
|
||||
out_8680243921562732354[5] = 0;
|
||||
out_8680243921562732354[6] = 0;
|
||||
out_8680243921562732354[7] = 0;
|
||||
out_8680243921562732354[8] = 0;
|
||||
out_8680243921562732354[9] = 0;
|
||||
out_8680243921562732354[10] = 0;
|
||||
out_8680243921562732354[11] = 0;
|
||||
out_8680243921562732354[12] = 0;
|
||||
out_8680243921562732354[13] = 0;
|
||||
out_8680243921562732354[14] = 1;
|
||||
out_8680243921562732354[15] = 0;
|
||||
out_8680243921562732354[16] = 0;
|
||||
out_8680243921562732354[17] = 0;
|
||||
void H_24(double *state, double *unused, double *out_6745130613232642731) {
|
||||
out_6745130613232642731[0] = 0;
|
||||
out_6745130613232642731[1] = 0;
|
||||
out_6745130613232642731[2] = 0;
|
||||
out_6745130613232642731[3] = 0;
|
||||
out_6745130613232642731[4] = 1;
|
||||
out_6745130613232642731[5] = 0;
|
||||
out_6745130613232642731[6] = 0;
|
||||
out_6745130613232642731[7] = 0;
|
||||
out_6745130613232642731[8] = 0;
|
||||
out_6745130613232642731[9] = 0;
|
||||
out_6745130613232642731[10] = 0;
|
||||
out_6745130613232642731[11] = 0;
|
||||
out_6745130613232642731[12] = 0;
|
||||
out_6745130613232642731[13] = 0;
|
||||
out_6745130613232642731[14] = 1;
|
||||
out_6745130613232642731[15] = 0;
|
||||
out_6745130613232642731[16] = 0;
|
||||
out_6745130613232642731[17] = 0;
|
||||
}
|
||||
void h_30(double *state, double *unused, double *out_7964191929203084827) {
|
||||
out_7964191929203084827[0] = state[4];
|
||||
void h_30(double *state, double *unused, double *out_7891311375938116468) {
|
||||
out_7891311375938116468[0] = state[4];
|
||||
}
|
||||
void H_30(double *state, double *unused, double *out_6636933269700472858) {
|
||||
out_6636933269700472858[0] = 0;
|
||||
out_6636933269700472858[1] = 0;
|
||||
out_6636933269700472858[2] = 0;
|
||||
out_6636933269700472858[3] = 0;
|
||||
out_6636933269700472858[4] = 1;
|
||||
out_6636933269700472858[5] = 0;
|
||||
out_6636933269700472858[6] = 0;
|
||||
out_6636933269700472858[7] = 0;
|
||||
out_6636933269700472858[8] = 0;
|
||||
void H_30(double *state, double *unused, double *out_6404012078332544077) {
|
||||
out_6404012078332544077[0] = 0;
|
||||
out_6404012078332544077[1] = 0;
|
||||
out_6404012078332544077[2] = 0;
|
||||
out_6404012078332544077[3] = 0;
|
||||
out_6404012078332544077[4] = 1;
|
||||
out_6404012078332544077[5] = 0;
|
||||
out_6404012078332544077[6] = 0;
|
||||
out_6404012078332544077[7] = 0;
|
||||
out_6404012078332544077[8] = 0;
|
||||
}
|
||||
void h_26(double *state, double *unused, double *out_7445766093062134104) {
|
||||
out_7445766093062134104[0] = state[7];
|
||||
void h_26(double *state, double *unused, double *out_8834821228839778903) {
|
||||
out_8834821228839778903[0] = state[7];
|
||||
}
|
||||
void H_26(double *state, double *unused, double *out_8197646432278262604) {
|
||||
out_8197646432278262604[0] = 0;
|
||||
out_8197646432278262604[1] = 0;
|
||||
out_8197646432278262604[2] = 0;
|
||||
out_8197646432278262604[3] = 0;
|
||||
out_8197646432278262604[4] = 0;
|
||||
out_8197646432278262604[5] = 0;
|
||||
out_8197646432278262604[6] = 0;
|
||||
out_8197646432278262604[7] = 1;
|
||||
out_8197646432278262604[8] = 0;
|
||||
void H_26(double *state, double *unused, double *out_5782895717995702688) {
|
||||
out_5782895717995702688[0] = 0;
|
||||
out_5782895717995702688[1] = 0;
|
||||
out_5782895717995702688[2] = 0;
|
||||
out_5782895717995702688[3] = 0;
|
||||
out_5782895717995702688[4] = 0;
|
||||
out_5782895717995702688[5] = 0;
|
||||
out_5782895717995702688[6] = 0;
|
||||
out_5782895717995702688[7] = 1;
|
||||
out_5782895717995702688[8] = 0;
|
||||
}
|
||||
void h_27(double *state, double *unused, double *out_7327758699128757360) {
|
||||
out_7327758699128757360[0] = state[3];
|
||||
void h_27(double *state, double *unused, double *out_5701435530743450317) {
|
||||
out_5701435530743450317[0] = state[3];
|
||||
}
|
||||
void H_27(double *state, double *unused, double *out_8811696581500897769) {
|
||||
out_8811696581500897769[0] = 0;
|
||||
out_8811696581500897769[1] = 0;
|
||||
out_8811696581500897769[2] = 0;
|
||||
out_8811696581500897769[3] = 1;
|
||||
out_8811696581500897769[4] = 0;
|
||||
out_8811696581500897769[5] = 0;
|
||||
out_8811696581500897769[6] = 0;
|
||||
out_8811696581500897769[7] = 0;
|
||||
out_8811696581500897769[8] = 0;
|
||||
void H_27(double *state, double *unused, double *out_8578775390132968988) {
|
||||
out_8578775390132968988[0] = 0;
|
||||
out_8578775390132968988[1] = 0;
|
||||
out_8578775390132968988[2] = 0;
|
||||
out_8578775390132968988[3] = 1;
|
||||
out_8578775390132968988[4] = 0;
|
||||
out_8578775390132968988[5] = 0;
|
||||
out_8578775390132968988[6] = 0;
|
||||
out_8578775390132968988[7] = 0;
|
||||
out_8578775390132968988[8] = 0;
|
||||
}
|
||||
void h_29(double *state, double *unused, double *out_7020682076301422392) {
|
||||
out_7020682076301422392[0] = state[1];
|
||||
void h_29(double *state, double *unused, double *out_4854525324493783783) {
|
||||
out_4854525324493783783[0] = state[1];
|
||||
}
|
||||
void H_29(double *state, double *unused, double *out_7921684765339102814) {
|
||||
out_7921684765339102814[0] = 0;
|
||||
out_7921684765339102814[1] = 1;
|
||||
out_7921684765339102814[2] = 0;
|
||||
out_7921684765339102814[3] = 0;
|
||||
out_7921684765339102814[4] = 0;
|
||||
out_7921684765339102814[5] = 0;
|
||||
out_7921684765339102814[6] = 0;
|
||||
out_7921684765339102814[7] = 0;
|
||||
out_7921684765339102814[8] = 0;
|
||||
void H_29(double *state, double *unused, double *out_5893780734018151893) {
|
||||
out_5893780734018151893[0] = 0;
|
||||
out_5893780734018151893[1] = 1;
|
||||
out_5893780734018151893[2] = 0;
|
||||
out_5893780734018151893[3] = 0;
|
||||
out_5893780734018151893[4] = 0;
|
||||
out_5893780734018151893[5] = 0;
|
||||
out_5893780734018151893[6] = 0;
|
||||
out_5893780734018151893[7] = 0;
|
||||
out_5893780734018151893[8] = 0;
|
||||
}
|
||||
void h_28(double *state, double *unused, double *out_4051910886381300771) {
|
||||
out_4051910886381300771[0] = state[0];
|
||||
void h_28(double *state, double *unused, double *out_697605986670398829) {
|
||||
out_697605986670398829[0] = state[0];
|
||||
}
|
||||
void H_28(double *state, double *unused, double *out_8561429036805122551) {
|
||||
out_8561429036805122551[0] = 1;
|
||||
out_8561429036805122551[1] = 0;
|
||||
out_8561429036805122551[2] = 0;
|
||||
out_8561429036805122551[3] = 0;
|
||||
out_8561429036805122551[4] = 0;
|
||||
out_8561429036805122551[5] = 0;
|
||||
out_8561429036805122551[6] = 0;
|
||||
out_8561429036805122551[7] = 0;
|
||||
out_8561429036805122551[8] = 0;
|
||||
void H_28(double *state, double *unused, double *out_7470564322621869149) {
|
||||
out_7470564322621869149[0] = 1;
|
||||
out_7470564322621869149[1] = 0;
|
||||
out_7470564322621869149[2] = 0;
|
||||
out_7470564322621869149[3] = 0;
|
||||
out_7470564322621869149[4] = 0;
|
||||
out_7470564322621869149[5] = 0;
|
||||
out_7470564322621869149[6] = 0;
|
||||
out_7470564322621869149[7] = 0;
|
||||
out_7470564322621869149[8] = 0;
|
||||
}
|
||||
void h_31(double *state, double *unused, double *out_5842118044635442722) {
|
||||
out_5842118044635442722[0] = state[8];
|
||||
void h_31(double *state, double *unused, double *out_1460670117812950450) {
|
||||
out_1460670117812950450[0] = state[8];
|
||||
}
|
||||
void H_31(double *state, double *unused, double *out_6476948360680272360) {
|
||||
out_6476948360680272360[0] = 0;
|
||||
out_6476948360680272360[1] = 0;
|
||||
out_6476948360680272360[2] = 0;
|
||||
out_6476948360680272360[3] = 0;
|
||||
out_6476948360680272360[4] = 0;
|
||||
out_6476948360680272360[5] = 0;
|
||||
out_6476948360680272360[6] = 0;
|
||||
out_6476948360680272360[7] = 0;
|
||||
out_6476948360680272360[8] = 1;
|
||||
void H_31(double *state, double *unused, double *out_5156687615762351212) {
|
||||
out_5156687615762351212[0] = 0;
|
||||
out_5156687615762351212[1] = 0;
|
||||
out_5156687615762351212[2] = 0;
|
||||
out_5156687615762351212[3] = 0;
|
||||
out_5156687615762351212[4] = 0;
|
||||
out_5156687615762351212[5] = 0;
|
||||
out_5156687615762351212[6] = 0;
|
||||
out_5156687615762351212[7] = 0;
|
||||
out_5156687615762351212[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_3273042440096814304) {
|
||||
err_fun(nom_x, delta_x, out_3273042440096814304);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_2634337573502312531) {
|
||||
err_fun(nom_x, delta_x, out_2634337573502312531);
|
||||
}
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_5732845985013317398) {
|
||||
inv_err_fun(nom_x, true_x, out_5732845985013317398);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_364624501276122878) {
|
||||
inv_err_fun(nom_x, true_x, out_364624501276122878);
|
||||
}
|
||||
void car_H_mod_fun(double *state, double *out_421472006259524810) {
|
||||
H_mod_fun(state, out_421472006259524810);
|
||||
void car_H_mod_fun(double *state, double *out_8283385258124076174) {
|
||||
H_mod_fun(state, out_8283385258124076174);
|
||||
}
|
||||
void car_f_fun(double *state, double dt, double *out_3776724096449913501) {
|
||||
f_fun(state, dt, out_3776724096449913501);
|
||||
void car_f_fun(double *state, double dt, double *out_1842212169269794394) {
|
||||
f_fun(state, dt, out_1842212169269794394);
|
||||
}
|
||||
void car_F_fun(double *state, double dt, double *out_1353218831850316658) {
|
||||
F_fun(state, dt, out_1353218831850316658);
|
||||
void car_F_fun(double *state, double dt, double *out_844160246145237490) {
|
||||
F_fun(state, dt, out_844160246145237490);
|
||||
}
|
||||
void car_h_25(double *state, double *unused, double *out_1344834859822615607) {
|
||||
h_25(state, unused, out_1344834859822615607);
|
||||
void car_h_25(double *state, double *unused, double *out_1303483449461821305) {
|
||||
h_25(state, unused, out_1303483449461821305);
|
||||
}
|
||||
void car_H_25(double *state, double *unused, double *out_6507594322557232788) {
|
||||
H_25(state, unused, out_6507594322557232788);
|
||||
void car_H_25(double *state, double *unused, double *out_8922345036839792704) {
|
||||
H_25(state, unused, out_8922345036839792704);
|
||||
}
|
||||
void car_h_24(double *state, double *unused, double *out_5433380832945564861) {
|
||||
h_24(state, unused, out_5433380832945564861);
|
||||
void car_h_24(double *state, double *unused, double *out_1550963005683787860) {
|
||||
h_24(state, unused, out_1550963005683787860);
|
||||
}
|
||||
void car_H_24(double *state, double *unused, double *out_8680243921562732354) {
|
||||
H_24(state, unused, out_8680243921562732354);
|
||||
void car_H_24(double *state, double *unused, double *out_6745130613232642731) {
|
||||
H_24(state, unused, out_6745130613232642731);
|
||||
}
|
||||
void car_h_30(double *state, double *unused, double *out_7964191929203084827) {
|
||||
h_30(state, unused, out_7964191929203084827);
|
||||
void car_h_30(double *state, double *unused, double *out_7891311375938116468) {
|
||||
h_30(state, unused, out_7891311375938116468);
|
||||
}
|
||||
void car_H_30(double *state, double *unused, double *out_6636933269700472858) {
|
||||
H_30(state, unused, out_6636933269700472858);
|
||||
void car_H_30(double *state, double *unused, double *out_6404012078332544077) {
|
||||
H_30(state, unused, out_6404012078332544077);
|
||||
}
|
||||
void car_h_26(double *state, double *unused, double *out_7445766093062134104) {
|
||||
h_26(state, unused, out_7445766093062134104);
|
||||
void car_h_26(double *state, double *unused, double *out_8834821228839778903) {
|
||||
h_26(state, unused, out_8834821228839778903);
|
||||
}
|
||||
void car_H_26(double *state, double *unused, double *out_8197646432278262604) {
|
||||
H_26(state, unused, out_8197646432278262604);
|
||||
void car_H_26(double *state, double *unused, double *out_5782895717995702688) {
|
||||
H_26(state, unused, out_5782895717995702688);
|
||||
}
|
||||
void car_h_27(double *state, double *unused, double *out_7327758699128757360) {
|
||||
h_27(state, unused, out_7327758699128757360);
|
||||
void car_h_27(double *state, double *unused, double *out_5701435530743450317) {
|
||||
h_27(state, unused, out_5701435530743450317);
|
||||
}
|
||||
void car_H_27(double *state, double *unused, double *out_8811696581500897769) {
|
||||
H_27(state, unused, out_8811696581500897769);
|
||||
void car_H_27(double *state, double *unused, double *out_8578775390132968988) {
|
||||
H_27(state, unused, out_8578775390132968988);
|
||||
}
|
||||
void car_h_29(double *state, double *unused, double *out_7020682076301422392) {
|
||||
h_29(state, unused, out_7020682076301422392);
|
||||
void car_h_29(double *state, double *unused, double *out_4854525324493783783) {
|
||||
h_29(state, unused, out_4854525324493783783);
|
||||
}
|
||||
void car_H_29(double *state, double *unused, double *out_7921684765339102814) {
|
||||
H_29(state, unused, out_7921684765339102814);
|
||||
void car_H_29(double *state, double *unused, double *out_5893780734018151893) {
|
||||
H_29(state, unused, out_5893780734018151893);
|
||||
}
|
||||
void car_h_28(double *state, double *unused, double *out_4051910886381300771) {
|
||||
h_28(state, unused, out_4051910886381300771);
|
||||
void car_h_28(double *state, double *unused, double *out_697605986670398829) {
|
||||
h_28(state, unused, out_697605986670398829);
|
||||
}
|
||||
void car_H_28(double *state, double *unused, double *out_8561429036805122551) {
|
||||
H_28(state, unused, out_8561429036805122551);
|
||||
void car_H_28(double *state, double *unused, double *out_7470564322621869149) {
|
||||
H_28(state, unused, out_7470564322621869149);
|
||||
}
|
||||
void car_h_31(double *state, double *unused, double *out_5842118044635442722) {
|
||||
h_31(state, unused, out_5842118044635442722);
|
||||
void car_h_31(double *state, double *unused, double *out_1460670117812950450) {
|
||||
h_31(state, unused, out_1460670117812950450);
|
||||
}
|
||||
void car_H_31(double *state, double *unused, double *out_6476948360680272360) {
|
||||
H_31(state, unused, out_6476948360680272360);
|
||||
void car_H_31(double *state, double *unused, double *out_5156687615762351212) {
|
||||
H_31(state, unused, out_5156687615762351212);
|
||||
}
|
||||
void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
|
||||
predict(in_x, in_P, in_Q, dt);
|
||||
|
||||
@@ -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_3273042440096814304);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_5732845985013317398);
|
||||
void car_H_mod_fun(double *state, double *out_421472006259524810);
|
||||
void car_f_fun(double *state, double dt, double *out_3776724096449913501);
|
||||
void car_F_fun(double *state, double dt, double *out_1353218831850316658);
|
||||
void car_h_25(double *state, double *unused, double *out_1344834859822615607);
|
||||
void car_H_25(double *state, double *unused, double *out_6507594322557232788);
|
||||
void car_h_24(double *state, double *unused, double *out_5433380832945564861);
|
||||
void car_H_24(double *state, double *unused, double *out_8680243921562732354);
|
||||
void car_h_30(double *state, double *unused, double *out_7964191929203084827);
|
||||
void car_H_30(double *state, double *unused, double *out_6636933269700472858);
|
||||
void car_h_26(double *state, double *unused, double *out_7445766093062134104);
|
||||
void car_H_26(double *state, double *unused, double *out_8197646432278262604);
|
||||
void car_h_27(double *state, double *unused, double *out_7327758699128757360);
|
||||
void car_H_27(double *state, double *unused, double *out_8811696581500897769);
|
||||
void car_h_29(double *state, double *unused, double *out_7020682076301422392);
|
||||
void car_H_29(double *state, double *unused, double *out_7921684765339102814);
|
||||
void car_h_28(double *state, double *unused, double *out_4051910886381300771);
|
||||
void car_H_28(double *state, double *unused, double *out_8561429036805122551);
|
||||
void car_h_31(double *state, double *unused, double *out_5842118044635442722);
|
||||
void car_H_31(double *state, double *unused, double *out_6476948360680272360);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_2634337573502312531);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_364624501276122878);
|
||||
void car_H_mod_fun(double *state, double *out_8283385258124076174);
|
||||
void car_f_fun(double *state, double dt, double *out_1842212169269794394);
|
||||
void car_F_fun(double *state, double dt, double *out_844160246145237490);
|
||||
void car_h_25(double *state, double *unused, double *out_1303483449461821305);
|
||||
void car_H_25(double *state, double *unused, double *out_8922345036839792704);
|
||||
void car_h_24(double *state, double *unused, double *out_1550963005683787860);
|
||||
void car_H_24(double *state, double *unused, double *out_6745130613232642731);
|
||||
void car_h_30(double *state, double *unused, double *out_7891311375938116468);
|
||||
void car_H_30(double *state, double *unused, double *out_6404012078332544077);
|
||||
void car_h_26(double *state, double *unused, double *out_8834821228839778903);
|
||||
void car_H_26(double *state, double *unused, double *out_5782895717995702688);
|
||||
void car_h_27(double *state, double *unused, double *out_5701435530743450317);
|
||||
void car_H_27(double *state, double *unused, double *out_8578775390132968988);
|
||||
void car_h_29(double *state, double *unused, double *out_4854525324493783783);
|
||||
void car_H_29(double *state, double *unused, double *out_5893780734018151893);
|
||||
void car_h_28(double *state, double *unused, double *out_697605986670398829);
|
||||
void car_H_28(double *state, double *unused, double *out_7470564322621869149);
|
||||
void car_h_31(double *state, double *unused, double *out_1460670117812950450);
|
||||
void car_H_31(double *state, double *unused, double *out_5156687615762351212);
|
||||
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);
|
||||
|
||||
@@ -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_4588913850449385760) {
|
||||
out_4588913850449385760[0] = delta_x[0] + nom_x[0];
|
||||
out_4588913850449385760[1] = delta_x[1] + nom_x[1];
|
||||
out_4588913850449385760[2] = delta_x[2] + nom_x[2];
|
||||
out_4588913850449385760[3] = delta_x[3] + nom_x[3];
|
||||
out_4588913850449385760[4] = delta_x[4] + nom_x[4];
|
||||
out_4588913850449385760[5] = delta_x[5] + nom_x[5];
|
||||
out_4588913850449385760[6] = delta_x[6] + nom_x[6];
|
||||
out_4588913850449385760[7] = delta_x[7] + nom_x[7];
|
||||
out_4588913850449385760[8] = delta_x[8] + nom_x[8];
|
||||
out_4588913850449385760[9] = delta_x[9] + nom_x[9];
|
||||
out_4588913850449385760[10] = delta_x[10] + nom_x[10];
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_6208051589904680687) {
|
||||
out_6208051589904680687[0] = delta_x[0] + nom_x[0];
|
||||
out_6208051589904680687[1] = delta_x[1] + nom_x[1];
|
||||
out_6208051589904680687[2] = delta_x[2] + nom_x[2];
|
||||
out_6208051589904680687[3] = delta_x[3] + nom_x[3];
|
||||
out_6208051589904680687[4] = delta_x[4] + nom_x[4];
|
||||
out_6208051589904680687[5] = delta_x[5] + nom_x[5];
|
||||
out_6208051589904680687[6] = delta_x[6] + nom_x[6];
|
||||
out_6208051589904680687[7] = delta_x[7] + nom_x[7];
|
||||
out_6208051589904680687[8] = delta_x[8] + nom_x[8];
|
||||
out_6208051589904680687[9] = delta_x[9] + nom_x[9];
|
||||
out_6208051589904680687[10] = delta_x[10] + nom_x[10];
|
||||
}
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_361280867974834565) {
|
||||
out_361280867974834565[0] = -nom_x[0] + true_x[0];
|
||||
out_361280867974834565[1] = -nom_x[1] + true_x[1];
|
||||
out_361280867974834565[2] = -nom_x[2] + true_x[2];
|
||||
out_361280867974834565[3] = -nom_x[3] + true_x[3];
|
||||
out_361280867974834565[4] = -nom_x[4] + true_x[4];
|
||||
out_361280867974834565[5] = -nom_x[5] + true_x[5];
|
||||
out_361280867974834565[6] = -nom_x[6] + true_x[6];
|
||||
out_361280867974834565[7] = -nom_x[7] + true_x[7];
|
||||
out_361280867974834565[8] = -nom_x[8] + true_x[8];
|
||||
out_361280867974834565[9] = -nom_x[9] + true_x[9];
|
||||
out_361280867974834565[10] = -nom_x[10] + true_x[10];
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_980651649795713821) {
|
||||
out_980651649795713821[0] = -nom_x[0] + true_x[0];
|
||||
out_980651649795713821[1] = -nom_x[1] + true_x[1];
|
||||
out_980651649795713821[2] = -nom_x[2] + true_x[2];
|
||||
out_980651649795713821[3] = -nom_x[3] + true_x[3];
|
||||
out_980651649795713821[4] = -nom_x[4] + true_x[4];
|
||||
out_980651649795713821[5] = -nom_x[5] + true_x[5];
|
||||
out_980651649795713821[6] = -nom_x[6] + true_x[6];
|
||||
out_980651649795713821[7] = -nom_x[7] + true_x[7];
|
||||
out_980651649795713821[8] = -nom_x[8] + true_x[8];
|
||||
out_980651649795713821[9] = -nom_x[9] + true_x[9];
|
||||
out_980651649795713821[10] = -nom_x[10] + true_x[10];
|
||||
}
|
||||
void H_mod_fun(double *state, double *out_3692418476170695800) {
|
||||
out_3692418476170695800[0] = 1.0;
|
||||
out_3692418476170695800[1] = 0;
|
||||
out_3692418476170695800[2] = 0;
|
||||
out_3692418476170695800[3] = 0;
|
||||
out_3692418476170695800[4] = 0;
|
||||
out_3692418476170695800[5] = 0;
|
||||
out_3692418476170695800[6] = 0;
|
||||
out_3692418476170695800[7] = 0;
|
||||
out_3692418476170695800[8] = 0;
|
||||
out_3692418476170695800[9] = 0;
|
||||
out_3692418476170695800[10] = 0;
|
||||
out_3692418476170695800[11] = 0;
|
||||
out_3692418476170695800[12] = 1.0;
|
||||
out_3692418476170695800[13] = 0;
|
||||
out_3692418476170695800[14] = 0;
|
||||
out_3692418476170695800[15] = 0;
|
||||
out_3692418476170695800[16] = 0;
|
||||
out_3692418476170695800[17] = 0;
|
||||
out_3692418476170695800[18] = 0;
|
||||
out_3692418476170695800[19] = 0;
|
||||
out_3692418476170695800[20] = 0;
|
||||
out_3692418476170695800[21] = 0;
|
||||
out_3692418476170695800[22] = 0;
|
||||
out_3692418476170695800[23] = 0;
|
||||
out_3692418476170695800[24] = 1.0;
|
||||
out_3692418476170695800[25] = 0;
|
||||
out_3692418476170695800[26] = 0;
|
||||
out_3692418476170695800[27] = 0;
|
||||
out_3692418476170695800[28] = 0;
|
||||
out_3692418476170695800[29] = 0;
|
||||
out_3692418476170695800[30] = 0;
|
||||
out_3692418476170695800[31] = 0;
|
||||
out_3692418476170695800[32] = 0;
|
||||
out_3692418476170695800[33] = 0;
|
||||
out_3692418476170695800[34] = 0;
|
||||
out_3692418476170695800[35] = 0;
|
||||
out_3692418476170695800[36] = 1.0;
|
||||
out_3692418476170695800[37] = 0;
|
||||
out_3692418476170695800[38] = 0;
|
||||
out_3692418476170695800[39] = 0;
|
||||
out_3692418476170695800[40] = 0;
|
||||
out_3692418476170695800[41] = 0;
|
||||
out_3692418476170695800[42] = 0;
|
||||
out_3692418476170695800[43] = 0;
|
||||
out_3692418476170695800[44] = 0;
|
||||
out_3692418476170695800[45] = 0;
|
||||
out_3692418476170695800[46] = 0;
|
||||
out_3692418476170695800[47] = 0;
|
||||
out_3692418476170695800[48] = 1.0;
|
||||
out_3692418476170695800[49] = 0;
|
||||
out_3692418476170695800[50] = 0;
|
||||
out_3692418476170695800[51] = 0;
|
||||
out_3692418476170695800[52] = 0;
|
||||
out_3692418476170695800[53] = 0;
|
||||
out_3692418476170695800[54] = 0;
|
||||
out_3692418476170695800[55] = 0;
|
||||
out_3692418476170695800[56] = 0;
|
||||
out_3692418476170695800[57] = 0;
|
||||
out_3692418476170695800[58] = 0;
|
||||
out_3692418476170695800[59] = 0;
|
||||
out_3692418476170695800[60] = 1.0;
|
||||
out_3692418476170695800[61] = 0;
|
||||
out_3692418476170695800[62] = 0;
|
||||
out_3692418476170695800[63] = 0;
|
||||
out_3692418476170695800[64] = 0;
|
||||
out_3692418476170695800[65] = 0;
|
||||
out_3692418476170695800[66] = 0;
|
||||
out_3692418476170695800[67] = 0;
|
||||
out_3692418476170695800[68] = 0;
|
||||
out_3692418476170695800[69] = 0;
|
||||
out_3692418476170695800[70] = 0;
|
||||
out_3692418476170695800[71] = 0;
|
||||
out_3692418476170695800[72] = 1.0;
|
||||
out_3692418476170695800[73] = 0;
|
||||
out_3692418476170695800[74] = 0;
|
||||
out_3692418476170695800[75] = 0;
|
||||
out_3692418476170695800[76] = 0;
|
||||
out_3692418476170695800[77] = 0;
|
||||
out_3692418476170695800[78] = 0;
|
||||
out_3692418476170695800[79] = 0;
|
||||
out_3692418476170695800[80] = 0;
|
||||
out_3692418476170695800[81] = 0;
|
||||
out_3692418476170695800[82] = 0;
|
||||
out_3692418476170695800[83] = 0;
|
||||
out_3692418476170695800[84] = 1.0;
|
||||
out_3692418476170695800[85] = 0;
|
||||
out_3692418476170695800[86] = 0;
|
||||
out_3692418476170695800[87] = 0;
|
||||
out_3692418476170695800[88] = 0;
|
||||
out_3692418476170695800[89] = 0;
|
||||
out_3692418476170695800[90] = 0;
|
||||
out_3692418476170695800[91] = 0;
|
||||
out_3692418476170695800[92] = 0;
|
||||
out_3692418476170695800[93] = 0;
|
||||
out_3692418476170695800[94] = 0;
|
||||
out_3692418476170695800[95] = 0;
|
||||
out_3692418476170695800[96] = 1.0;
|
||||
out_3692418476170695800[97] = 0;
|
||||
out_3692418476170695800[98] = 0;
|
||||
out_3692418476170695800[99] = 0;
|
||||
out_3692418476170695800[100] = 0;
|
||||
out_3692418476170695800[101] = 0;
|
||||
out_3692418476170695800[102] = 0;
|
||||
out_3692418476170695800[103] = 0;
|
||||
out_3692418476170695800[104] = 0;
|
||||
out_3692418476170695800[105] = 0;
|
||||
out_3692418476170695800[106] = 0;
|
||||
out_3692418476170695800[107] = 0;
|
||||
out_3692418476170695800[108] = 1.0;
|
||||
out_3692418476170695800[109] = 0;
|
||||
out_3692418476170695800[110] = 0;
|
||||
out_3692418476170695800[111] = 0;
|
||||
out_3692418476170695800[112] = 0;
|
||||
out_3692418476170695800[113] = 0;
|
||||
out_3692418476170695800[114] = 0;
|
||||
out_3692418476170695800[115] = 0;
|
||||
out_3692418476170695800[116] = 0;
|
||||
out_3692418476170695800[117] = 0;
|
||||
out_3692418476170695800[118] = 0;
|
||||
out_3692418476170695800[119] = 0;
|
||||
out_3692418476170695800[120] = 1.0;
|
||||
void H_mod_fun(double *state, double *out_8761108313881456199) {
|
||||
out_8761108313881456199[0] = 1.0;
|
||||
out_8761108313881456199[1] = 0;
|
||||
out_8761108313881456199[2] = 0;
|
||||
out_8761108313881456199[3] = 0;
|
||||
out_8761108313881456199[4] = 0;
|
||||
out_8761108313881456199[5] = 0;
|
||||
out_8761108313881456199[6] = 0;
|
||||
out_8761108313881456199[7] = 0;
|
||||
out_8761108313881456199[8] = 0;
|
||||
out_8761108313881456199[9] = 0;
|
||||
out_8761108313881456199[10] = 0;
|
||||
out_8761108313881456199[11] = 0;
|
||||
out_8761108313881456199[12] = 1.0;
|
||||
out_8761108313881456199[13] = 0;
|
||||
out_8761108313881456199[14] = 0;
|
||||
out_8761108313881456199[15] = 0;
|
||||
out_8761108313881456199[16] = 0;
|
||||
out_8761108313881456199[17] = 0;
|
||||
out_8761108313881456199[18] = 0;
|
||||
out_8761108313881456199[19] = 0;
|
||||
out_8761108313881456199[20] = 0;
|
||||
out_8761108313881456199[21] = 0;
|
||||
out_8761108313881456199[22] = 0;
|
||||
out_8761108313881456199[23] = 0;
|
||||
out_8761108313881456199[24] = 1.0;
|
||||
out_8761108313881456199[25] = 0;
|
||||
out_8761108313881456199[26] = 0;
|
||||
out_8761108313881456199[27] = 0;
|
||||
out_8761108313881456199[28] = 0;
|
||||
out_8761108313881456199[29] = 0;
|
||||
out_8761108313881456199[30] = 0;
|
||||
out_8761108313881456199[31] = 0;
|
||||
out_8761108313881456199[32] = 0;
|
||||
out_8761108313881456199[33] = 0;
|
||||
out_8761108313881456199[34] = 0;
|
||||
out_8761108313881456199[35] = 0;
|
||||
out_8761108313881456199[36] = 1.0;
|
||||
out_8761108313881456199[37] = 0;
|
||||
out_8761108313881456199[38] = 0;
|
||||
out_8761108313881456199[39] = 0;
|
||||
out_8761108313881456199[40] = 0;
|
||||
out_8761108313881456199[41] = 0;
|
||||
out_8761108313881456199[42] = 0;
|
||||
out_8761108313881456199[43] = 0;
|
||||
out_8761108313881456199[44] = 0;
|
||||
out_8761108313881456199[45] = 0;
|
||||
out_8761108313881456199[46] = 0;
|
||||
out_8761108313881456199[47] = 0;
|
||||
out_8761108313881456199[48] = 1.0;
|
||||
out_8761108313881456199[49] = 0;
|
||||
out_8761108313881456199[50] = 0;
|
||||
out_8761108313881456199[51] = 0;
|
||||
out_8761108313881456199[52] = 0;
|
||||
out_8761108313881456199[53] = 0;
|
||||
out_8761108313881456199[54] = 0;
|
||||
out_8761108313881456199[55] = 0;
|
||||
out_8761108313881456199[56] = 0;
|
||||
out_8761108313881456199[57] = 0;
|
||||
out_8761108313881456199[58] = 0;
|
||||
out_8761108313881456199[59] = 0;
|
||||
out_8761108313881456199[60] = 1.0;
|
||||
out_8761108313881456199[61] = 0;
|
||||
out_8761108313881456199[62] = 0;
|
||||
out_8761108313881456199[63] = 0;
|
||||
out_8761108313881456199[64] = 0;
|
||||
out_8761108313881456199[65] = 0;
|
||||
out_8761108313881456199[66] = 0;
|
||||
out_8761108313881456199[67] = 0;
|
||||
out_8761108313881456199[68] = 0;
|
||||
out_8761108313881456199[69] = 0;
|
||||
out_8761108313881456199[70] = 0;
|
||||
out_8761108313881456199[71] = 0;
|
||||
out_8761108313881456199[72] = 1.0;
|
||||
out_8761108313881456199[73] = 0;
|
||||
out_8761108313881456199[74] = 0;
|
||||
out_8761108313881456199[75] = 0;
|
||||
out_8761108313881456199[76] = 0;
|
||||
out_8761108313881456199[77] = 0;
|
||||
out_8761108313881456199[78] = 0;
|
||||
out_8761108313881456199[79] = 0;
|
||||
out_8761108313881456199[80] = 0;
|
||||
out_8761108313881456199[81] = 0;
|
||||
out_8761108313881456199[82] = 0;
|
||||
out_8761108313881456199[83] = 0;
|
||||
out_8761108313881456199[84] = 1.0;
|
||||
out_8761108313881456199[85] = 0;
|
||||
out_8761108313881456199[86] = 0;
|
||||
out_8761108313881456199[87] = 0;
|
||||
out_8761108313881456199[88] = 0;
|
||||
out_8761108313881456199[89] = 0;
|
||||
out_8761108313881456199[90] = 0;
|
||||
out_8761108313881456199[91] = 0;
|
||||
out_8761108313881456199[92] = 0;
|
||||
out_8761108313881456199[93] = 0;
|
||||
out_8761108313881456199[94] = 0;
|
||||
out_8761108313881456199[95] = 0;
|
||||
out_8761108313881456199[96] = 1.0;
|
||||
out_8761108313881456199[97] = 0;
|
||||
out_8761108313881456199[98] = 0;
|
||||
out_8761108313881456199[99] = 0;
|
||||
out_8761108313881456199[100] = 0;
|
||||
out_8761108313881456199[101] = 0;
|
||||
out_8761108313881456199[102] = 0;
|
||||
out_8761108313881456199[103] = 0;
|
||||
out_8761108313881456199[104] = 0;
|
||||
out_8761108313881456199[105] = 0;
|
||||
out_8761108313881456199[106] = 0;
|
||||
out_8761108313881456199[107] = 0;
|
||||
out_8761108313881456199[108] = 1.0;
|
||||
out_8761108313881456199[109] = 0;
|
||||
out_8761108313881456199[110] = 0;
|
||||
out_8761108313881456199[111] = 0;
|
||||
out_8761108313881456199[112] = 0;
|
||||
out_8761108313881456199[113] = 0;
|
||||
out_8761108313881456199[114] = 0;
|
||||
out_8761108313881456199[115] = 0;
|
||||
out_8761108313881456199[116] = 0;
|
||||
out_8761108313881456199[117] = 0;
|
||||
out_8761108313881456199[118] = 0;
|
||||
out_8761108313881456199[119] = 0;
|
||||
out_8761108313881456199[120] = 1.0;
|
||||
}
|
||||
void f_fun(double *state, double dt, double *out_3599858202863344339) {
|
||||
out_3599858202863344339[0] = dt*state[3] + state[0];
|
||||
out_3599858202863344339[1] = dt*state[4] + state[1];
|
||||
out_3599858202863344339[2] = dt*state[5] + state[2];
|
||||
out_3599858202863344339[3] = state[3];
|
||||
out_3599858202863344339[4] = state[4];
|
||||
out_3599858202863344339[5] = state[5];
|
||||
out_3599858202863344339[6] = dt*state[7] + state[6];
|
||||
out_3599858202863344339[7] = dt*state[8] + state[7];
|
||||
out_3599858202863344339[8] = state[8];
|
||||
out_3599858202863344339[9] = state[9];
|
||||
out_3599858202863344339[10] = state[10];
|
||||
void f_fun(double *state, double dt, double *out_6985979927194349446) {
|
||||
out_6985979927194349446[0] = dt*state[3] + state[0];
|
||||
out_6985979927194349446[1] = dt*state[4] + state[1];
|
||||
out_6985979927194349446[2] = dt*state[5] + state[2];
|
||||
out_6985979927194349446[3] = state[3];
|
||||
out_6985979927194349446[4] = state[4];
|
||||
out_6985979927194349446[5] = state[5];
|
||||
out_6985979927194349446[6] = dt*state[7] + state[6];
|
||||
out_6985979927194349446[7] = dt*state[8] + state[7];
|
||||
out_6985979927194349446[8] = state[8];
|
||||
out_6985979927194349446[9] = state[9];
|
||||
out_6985979927194349446[10] = state[10];
|
||||
}
|
||||
void F_fun(double *state, double dt, double *out_6614403358289748728) {
|
||||
out_6614403358289748728[0] = 1;
|
||||
out_6614403358289748728[1] = 0;
|
||||
out_6614403358289748728[2] = 0;
|
||||
out_6614403358289748728[3] = dt;
|
||||
out_6614403358289748728[4] = 0;
|
||||
out_6614403358289748728[5] = 0;
|
||||
out_6614403358289748728[6] = 0;
|
||||
out_6614403358289748728[7] = 0;
|
||||
out_6614403358289748728[8] = 0;
|
||||
out_6614403358289748728[9] = 0;
|
||||
out_6614403358289748728[10] = 0;
|
||||
out_6614403358289748728[11] = 0;
|
||||
out_6614403358289748728[12] = 1;
|
||||
out_6614403358289748728[13] = 0;
|
||||
out_6614403358289748728[14] = 0;
|
||||
out_6614403358289748728[15] = dt;
|
||||
out_6614403358289748728[16] = 0;
|
||||
out_6614403358289748728[17] = 0;
|
||||
out_6614403358289748728[18] = 0;
|
||||
out_6614403358289748728[19] = 0;
|
||||
out_6614403358289748728[20] = 0;
|
||||
out_6614403358289748728[21] = 0;
|
||||
out_6614403358289748728[22] = 0;
|
||||
out_6614403358289748728[23] = 0;
|
||||
out_6614403358289748728[24] = 1;
|
||||
out_6614403358289748728[25] = 0;
|
||||
out_6614403358289748728[26] = 0;
|
||||
out_6614403358289748728[27] = dt;
|
||||
out_6614403358289748728[28] = 0;
|
||||
out_6614403358289748728[29] = 0;
|
||||
out_6614403358289748728[30] = 0;
|
||||
out_6614403358289748728[31] = 0;
|
||||
out_6614403358289748728[32] = 0;
|
||||
out_6614403358289748728[33] = 0;
|
||||
out_6614403358289748728[34] = 0;
|
||||
out_6614403358289748728[35] = 0;
|
||||
out_6614403358289748728[36] = 1;
|
||||
out_6614403358289748728[37] = 0;
|
||||
out_6614403358289748728[38] = 0;
|
||||
out_6614403358289748728[39] = 0;
|
||||
out_6614403358289748728[40] = 0;
|
||||
out_6614403358289748728[41] = 0;
|
||||
out_6614403358289748728[42] = 0;
|
||||
out_6614403358289748728[43] = 0;
|
||||
out_6614403358289748728[44] = 0;
|
||||
out_6614403358289748728[45] = 0;
|
||||
out_6614403358289748728[46] = 0;
|
||||
out_6614403358289748728[47] = 0;
|
||||
out_6614403358289748728[48] = 1;
|
||||
out_6614403358289748728[49] = 0;
|
||||
out_6614403358289748728[50] = 0;
|
||||
out_6614403358289748728[51] = 0;
|
||||
out_6614403358289748728[52] = 0;
|
||||
out_6614403358289748728[53] = 0;
|
||||
out_6614403358289748728[54] = 0;
|
||||
out_6614403358289748728[55] = 0;
|
||||
out_6614403358289748728[56] = 0;
|
||||
out_6614403358289748728[57] = 0;
|
||||
out_6614403358289748728[58] = 0;
|
||||
out_6614403358289748728[59] = 0;
|
||||
out_6614403358289748728[60] = 1;
|
||||
out_6614403358289748728[61] = 0;
|
||||
out_6614403358289748728[62] = 0;
|
||||
out_6614403358289748728[63] = 0;
|
||||
out_6614403358289748728[64] = 0;
|
||||
out_6614403358289748728[65] = 0;
|
||||
out_6614403358289748728[66] = 0;
|
||||
out_6614403358289748728[67] = 0;
|
||||
out_6614403358289748728[68] = 0;
|
||||
out_6614403358289748728[69] = 0;
|
||||
out_6614403358289748728[70] = 0;
|
||||
out_6614403358289748728[71] = 0;
|
||||
out_6614403358289748728[72] = 1;
|
||||
out_6614403358289748728[73] = dt;
|
||||
out_6614403358289748728[74] = 0;
|
||||
out_6614403358289748728[75] = 0;
|
||||
out_6614403358289748728[76] = 0;
|
||||
out_6614403358289748728[77] = 0;
|
||||
out_6614403358289748728[78] = 0;
|
||||
out_6614403358289748728[79] = 0;
|
||||
out_6614403358289748728[80] = 0;
|
||||
out_6614403358289748728[81] = 0;
|
||||
out_6614403358289748728[82] = 0;
|
||||
out_6614403358289748728[83] = 0;
|
||||
out_6614403358289748728[84] = 1;
|
||||
out_6614403358289748728[85] = dt;
|
||||
out_6614403358289748728[86] = 0;
|
||||
out_6614403358289748728[87] = 0;
|
||||
out_6614403358289748728[88] = 0;
|
||||
out_6614403358289748728[89] = 0;
|
||||
out_6614403358289748728[90] = 0;
|
||||
out_6614403358289748728[91] = 0;
|
||||
out_6614403358289748728[92] = 0;
|
||||
out_6614403358289748728[93] = 0;
|
||||
out_6614403358289748728[94] = 0;
|
||||
out_6614403358289748728[95] = 0;
|
||||
out_6614403358289748728[96] = 1;
|
||||
out_6614403358289748728[97] = 0;
|
||||
out_6614403358289748728[98] = 0;
|
||||
out_6614403358289748728[99] = 0;
|
||||
out_6614403358289748728[100] = 0;
|
||||
out_6614403358289748728[101] = 0;
|
||||
out_6614403358289748728[102] = 0;
|
||||
out_6614403358289748728[103] = 0;
|
||||
out_6614403358289748728[104] = 0;
|
||||
out_6614403358289748728[105] = 0;
|
||||
out_6614403358289748728[106] = 0;
|
||||
out_6614403358289748728[107] = 0;
|
||||
out_6614403358289748728[108] = 1;
|
||||
out_6614403358289748728[109] = 0;
|
||||
out_6614403358289748728[110] = 0;
|
||||
out_6614403358289748728[111] = 0;
|
||||
out_6614403358289748728[112] = 0;
|
||||
out_6614403358289748728[113] = 0;
|
||||
out_6614403358289748728[114] = 0;
|
||||
out_6614403358289748728[115] = 0;
|
||||
out_6614403358289748728[116] = 0;
|
||||
out_6614403358289748728[117] = 0;
|
||||
out_6614403358289748728[118] = 0;
|
||||
out_6614403358289748728[119] = 0;
|
||||
out_6614403358289748728[120] = 1;
|
||||
void F_fun(double *state, double dt, double *out_5398596809828011208) {
|
||||
out_5398596809828011208[0] = 1;
|
||||
out_5398596809828011208[1] = 0;
|
||||
out_5398596809828011208[2] = 0;
|
||||
out_5398596809828011208[3] = dt;
|
||||
out_5398596809828011208[4] = 0;
|
||||
out_5398596809828011208[5] = 0;
|
||||
out_5398596809828011208[6] = 0;
|
||||
out_5398596809828011208[7] = 0;
|
||||
out_5398596809828011208[8] = 0;
|
||||
out_5398596809828011208[9] = 0;
|
||||
out_5398596809828011208[10] = 0;
|
||||
out_5398596809828011208[11] = 0;
|
||||
out_5398596809828011208[12] = 1;
|
||||
out_5398596809828011208[13] = 0;
|
||||
out_5398596809828011208[14] = 0;
|
||||
out_5398596809828011208[15] = dt;
|
||||
out_5398596809828011208[16] = 0;
|
||||
out_5398596809828011208[17] = 0;
|
||||
out_5398596809828011208[18] = 0;
|
||||
out_5398596809828011208[19] = 0;
|
||||
out_5398596809828011208[20] = 0;
|
||||
out_5398596809828011208[21] = 0;
|
||||
out_5398596809828011208[22] = 0;
|
||||
out_5398596809828011208[23] = 0;
|
||||
out_5398596809828011208[24] = 1;
|
||||
out_5398596809828011208[25] = 0;
|
||||
out_5398596809828011208[26] = 0;
|
||||
out_5398596809828011208[27] = dt;
|
||||
out_5398596809828011208[28] = 0;
|
||||
out_5398596809828011208[29] = 0;
|
||||
out_5398596809828011208[30] = 0;
|
||||
out_5398596809828011208[31] = 0;
|
||||
out_5398596809828011208[32] = 0;
|
||||
out_5398596809828011208[33] = 0;
|
||||
out_5398596809828011208[34] = 0;
|
||||
out_5398596809828011208[35] = 0;
|
||||
out_5398596809828011208[36] = 1;
|
||||
out_5398596809828011208[37] = 0;
|
||||
out_5398596809828011208[38] = 0;
|
||||
out_5398596809828011208[39] = 0;
|
||||
out_5398596809828011208[40] = 0;
|
||||
out_5398596809828011208[41] = 0;
|
||||
out_5398596809828011208[42] = 0;
|
||||
out_5398596809828011208[43] = 0;
|
||||
out_5398596809828011208[44] = 0;
|
||||
out_5398596809828011208[45] = 0;
|
||||
out_5398596809828011208[46] = 0;
|
||||
out_5398596809828011208[47] = 0;
|
||||
out_5398596809828011208[48] = 1;
|
||||
out_5398596809828011208[49] = 0;
|
||||
out_5398596809828011208[50] = 0;
|
||||
out_5398596809828011208[51] = 0;
|
||||
out_5398596809828011208[52] = 0;
|
||||
out_5398596809828011208[53] = 0;
|
||||
out_5398596809828011208[54] = 0;
|
||||
out_5398596809828011208[55] = 0;
|
||||
out_5398596809828011208[56] = 0;
|
||||
out_5398596809828011208[57] = 0;
|
||||
out_5398596809828011208[58] = 0;
|
||||
out_5398596809828011208[59] = 0;
|
||||
out_5398596809828011208[60] = 1;
|
||||
out_5398596809828011208[61] = 0;
|
||||
out_5398596809828011208[62] = 0;
|
||||
out_5398596809828011208[63] = 0;
|
||||
out_5398596809828011208[64] = 0;
|
||||
out_5398596809828011208[65] = 0;
|
||||
out_5398596809828011208[66] = 0;
|
||||
out_5398596809828011208[67] = 0;
|
||||
out_5398596809828011208[68] = 0;
|
||||
out_5398596809828011208[69] = 0;
|
||||
out_5398596809828011208[70] = 0;
|
||||
out_5398596809828011208[71] = 0;
|
||||
out_5398596809828011208[72] = 1;
|
||||
out_5398596809828011208[73] = dt;
|
||||
out_5398596809828011208[74] = 0;
|
||||
out_5398596809828011208[75] = 0;
|
||||
out_5398596809828011208[76] = 0;
|
||||
out_5398596809828011208[77] = 0;
|
||||
out_5398596809828011208[78] = 0;
|
||||
out_5398596809828011208[79] = 0;
|
||||
out_5398596809828011208[80] = 0;
|
||||
out_5398596809828011208[81] = 0;
|
||||
out_5398596809828011208[82] = 0;
|
||||
out_5398596809828011208[83] = 0;
|
||||
out_5398596809828011208[84] = 1;
|
||||
out_5398596809828011208[85] = dt;
|
||||
out_5398596809828011208[86] = 0;
|
||||
out_5398596809828011208[87] = 0;
|
||||
out_5398596809828011208[88] = 0;
|
||||
out_5398596809828011208[89] = 0;
|
||||
out_5398596809828011208[90] = 0;
|
||||
out_5398596809828011208[91] = 0;
|
||||
out_5398596809828011208[92] = 0;
|
||||
out_5398596809828011208[93] = 0;
|
||||
out_5398596809828011208[94] = 0;
|
||||
out_5398596809828011208[95] = 0;
|
||||
out_5398596809828011208[96] = 1;
|
||||
out_5398596809828011208[97] = 0;
|
||||
out_5398596809828011208[98] = 0;
|
||||
out_5398596809828011208[99] = 0;
|
||||
out_5398596809828011208[100] = 0;
|
||||
out_5398596809828011208[101] = 0;
|
||||
out_5398596809828011208[102] = 0;
|
||||
out_5398596809828011208[103] = 0;
|
||||
out_5398596809828011208[104] = 0;
|
||||
out_5398596809828011208[105] = 0;
|
||||
out_5398596809828011208[106] = 0;
|
||||
out_5398596809828011208[107] = 0;
|
||||
out_5398596809828011208[108] = 1;
|
||||
out_5398596809828011208[109] = 0;
|
||||
out_5398596809828011208[110] = 0;
|
||||
out_5398596809828011208[111] = 0;
|
||||
out_5398596809828011208[112] = 0;
|
||||
out_5398596809828011208[113] = 0;
|
||||
out_5398596809828011208[114] = 0;
|
||||
out_5398596809828011208[115] = 0;
|
||||
out_5398596809828011208[116] = 0;
|
||||
out_5398596809828011208[117] = 0;
|
||||
out_5398596809828011208[118] = 0;
|
||||
out_5398596809828011208[119] = 0;
|
||||
out_5398596809828011208[120] = 1;
|
||||
}
|
||||
void h_6(double *state, double *sat_pos, double *out_3965377368938519904) {
|
||||
out_3965377368938519904[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_854109254235318073) {
|
||||
out_854109254235318073[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_4010469744061131744) {
|
||||
out_4010469744061131744[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_4010469744061131744[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_4010469744061131744[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_4010469744061131744[3] = 0;
|
||||
out_4010469744061131744[4] = 0;
|
||||
out_4010469744061131744[5] = 0;
|
||||
out_4010469744061131744[6] = 1;
|
||||
out_4010469744061131744[7] = 0;
|
||||
out_4010469744061131744[8] = 0;
|
||||
out_4010469744061131744[9] = 0;
|
||||
out_4010469744061131744[10] = 0;
|
||||
void H_6(double *state, double *sat_pos, double *out_4878360041893525515) {
|
||||
out_4878360041893525515[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_4878360041893525515[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_4878360041893525515[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_4878360041893525515[3] = 0;
|
||||
out_4878360041893525515[4] = 0;
|
||||
out_4878360041893525515[5] = 0;
|
||||
out_4878360041893525515[6] = 1;
|
||||
out_4878360041893525515[7] = 0;
|
||||
out_4878360041893525515[8] = 0;
|
||||
out_4878360041893525515[9] = 0;
|
||||
out_4878360041893525515[10] = 0;
|
||||
}
|
||||
void h_20(double *state, double *sat_pos, double *out_4397465078196892446) {
|
||||
out_4397465078196892446[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_3910060068815622044) {
|
||||
out_3910060068815622044[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_7230021967014098202) {
|
||||
out_7230021967014098202[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_7230021967014098202[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_7230021967014098202[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_7230021967014098202[3] = 0;
|
||||
out_7230021967014098202[4] = 0;
|
||||
out_7230021967014098202[5] = 0;
|
||||
out_7230021967014098202[6] = 1;
|
||||
out_7230021967014098202[7] = 0;
|
||||
out_7230021967014098202[8] = 0;
|
||||
out_7230021967014098202[9] = 1;
|
||||
out_7230021967014098202[10] = sat_pos[3];
|
||||
void H_20(double *state, double *sat_pos, double *out_7601181949165199926) {
|
||||
out_7601181949165199926[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_7601181949165199926[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_7601181949165199926[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_7601181949165199926[3] = 0;
|
||||
out_7601181949165199926[4] = 0;
|
||||
out_7601181949165199926[5] = 0;
|
||||
out_7601181949165199926[6] = 1;
|
||||
out_7601181949165199926[7] = 0;
|
||||
out_7601181949165199926[8] = 0;
|
||||
out_7601181949165199926[9] = 1;
|
||||
out_7601181949165199926[10] = sat_pos[3];
|
||||
}
|
||||
void h_7(double *state, double *sat_pos_vel, double *out_8859539764878579118) {
|
||||
out_8859539764878579118[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_7752585033610334316) {
|
||||
out_7752585033610334316[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_6351382189608033375) {
|
||||
out_6351382189608033375[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_6351382189608033375[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_6351382189608033375[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_6351382189608033375[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_6351382189608033375[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_6351382189608033375[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_6351382189608033375[6] = 0;
|
||||
out_6351382189608033375[7] = 1;
|
||||
out_6351382189608033375[8] = 0;
|
||||
out_6351382189608033375[9] = 0;
|
||||
out_6351382189608033375[10] = 0;
|
||||
void H_7(double *state, double *sat_pos_vel, double *out_8389742934130346329) {
|
||||
out_8389742934130346329[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_8389742934130346329[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_8389742934130346329[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_8389742934130346329[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_8389742934130346329[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_8389742934130346329[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_8389742934130346329[6] = 0;
|
||||
out_8389742934130346329[7] = 1;
|
||||
out_8389742934130346329[8] = 0;
|
||||
out_8389742934130346329[9] = 0;
|
||||
out_8389742934130346329[10] = 0;
|
||||
}
|
||||
void h_21(double *state, double *sat_pos_vel, double *out_8859539764878579118) {
|
||||
out_8859539764878579118[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_7752585033610334316) {
|
||||
out_7752585033610334316[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_6351382189608033375) {
|
||||
out_6351382189608033375[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_6351382189608033375[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_6351382189608033375[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_6351382189608033375[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_6351382189608033375[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_6351382189608033375[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_6351382189608033375[6] = 0;
|
||||
out_6351382189608033375[7] = 1;
|
||||
out_6351382189608033375[8] = 0;
|
||||
out_6351382189608033375[9] = 0;
|
||||
out_6351382189608033375[10] = 0;
|
||||
void H_21(double *state, double *sat_pos_vel, double *out_8389742934130346329) {
|
||||
out_8389742934130346329[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_8389742934130346329[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_8389742934130346329[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_8389742934130346329[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_8389742934130346329[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_8389742934130346329[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_8389742934130346329[6] = 0;
|
||||
out_8389742934130346329[7] = 1;
|
||||
out_8389742934130346329[8] = 0;
|
||||
out_8389742934130346329[9] = 0;
|
||||
out_8389742934130346329[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_4588913850449385760) {
|
||||
err_fun(nom_x, delta_x, out_4588913850449385760);
|
||||
void gnss_err_fun(double *nom_x, double *delta_x, double *out_6208051589904680687) {
|
||||
err_fun(nom_x, delta_x, out_6208051589904680687);
|
||||
}
|
||||
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_361280867974834565) {
|
||||
inv_err_fun(nom_x, true_x, out_361280867974834565);
|
||||
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_980651649795713821) {
|
||||
inv_err_fun(nom_x, true_x, out_980651649795713821);
|
||||
}
|
||||
void gnss_H_mod_fun(double *state, double *out_3692418476170695800) {
|
||||
H_mod_fun(state, out_3692418476170695800);
|
||||
void gnss_H_mod_fun(double *state, double *out_8761108313881456199) {
|
||||
H_mod_fun(state, out_8761108313881456199);
|
||||
}
|
||||
void gnss_f_fun(double *state, double dt, double *out_3599858202863344339) {
|
||||
f_fun(state, dt, out_3599858202863344339);
|
||||
void gnss_f_fun(double *state, double dt, double *out_6985979927194349446) {
|
||||
f_fun(state, dt, out_6985979927194349446);
|
||||
}
|
||||
void gnss_F_fun(double *state, double dt, double *out_6614403358289748728) {
|
||||
F_fun(state, dt, out_6614403358289748728);
|
||||
void gnss_F_fun(double *state, double dt, double *out_5398596809828011208) {
|
||||
F_fun(state, dt, out_5398596809828011208);
|
||||
}
|
||||
void gnss_h_6(double *state, double *sat_pos, double *out_3965377368938519904) {
|
||||
h_6(state, sat_pos, out_3965377368938519904);
|
||||
void gnss_h_6(double *state, double *sat_pos, double *out_854109254235318073) {
|
||||
h_6(state, sat_pos, out_854109254235318073);
|
||||
}
|
||||
void gnss_H_6(double *state, double *sat_pos, double *out_4010469744061131744) {
|
||||
H_6(state, sat_pos, out_4010469744061131744);
|
||||
void gnss_H_6(double *state, double *sat_pos, double *out_4878360041893525515) {
|
||||
H_6(state, sat_pos, out_4878360041893525515);
|
||||
}
|
||||
void gnss_h_20(double *state, double *sat_pos, double *out_4397465078196892446) {
|
||||
h_20(state, sat_pos, out_4397465078196892446);
|
||||
void gnss_h_20(double *state, double *sat_pos, double *out_3910060068815622044) {
|
||||
h_20(state, sat_pos, out_3910060068815622044);
|
||||
}
|
||||
void gnss_H_20(double *state, double *sat_pos, double *out_7230021967014098202) {
|
||||
H_20(state, sat_pos, out_7230021967014098202);
|
||||
void gnss_H_20(double *state, double *sat_pos, double *out_7601181949165199926) {
|
||||
H_20(state, sat_pos, out_7601181949165199926);
|
||||
}
|
||||
void gnss_h_7(double *state, double *sat_pos_vel, double *out_8859539764878579118) {
|
||||
h_7(state, sat_pos_vel, out_8859539764878579118);
|
||||
void gnss_h_7(double *state, double *sat_pos_vel, double *out_7752585033610334316) {
|
||||
h_7(state, sat_pos_vel, out_7752585033610334316);
|
||||
}
|
||||
void gnss_H_7(double *state, double *sat_pos_vel, double *out_6351382189608033375) {
|
||||
H_7(state, sat_pos_vel, out_6351382189608033375);
|
||||
void gnss_H_7(double *state, double *sat_pos_vel, double *out_8389742934130346329) {
|
||||
H_7(state, sat_pos_vel, out_8389742934130346329);
|
||||
}
|
||||
void gnss_h_21(double *state, double *sat_pos_vel, double *out_8859539764878579118) {
|
||||
h_21(state, sat_pos_vel, out_8859539764878579118);
|
||||
void gnss_h_21(double *state, double *sat_pos_vel, double *out_7752585033610334316) {
|
||||
h_21(state, sat_pos_vel, out_7752585033610334316);
|
||||
}
|
||||
void gnss_H_21(double *state, double *sat_pos_vel, double *out_6351382189608033375) {
|
||||
H_21(state, sat_pos_vel, out_6351382189608033375);
|
||||
void gnss_H_21(double *state, double *sat_pos_vel, double *out_8389742934130346329) {
|
||||
H_21(state, sat_pos_vel, out_8389742934130346329);
|
||||
}
|
||||
void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt) {
|
||||
predict(in_x, in_P, in_Q, dt);
|
||||
|
||||
@@ -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_4588913850449385760);
|
||||
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_361280867974834565);
|
||||
void gnss_H_mod_fun(double *state, double *out_3692418476170695800);
|
||||
void gnss_f_fun(double *state, double dt, double *out_3599858202863344339);
|
||||
void gnss_F_fun(double *state, double dt, double *out_6614403358289748728);
|
||||
void gnss_h_6(double *state, double *sat_pos, double *out_3965377368938519904);
|
||||
void gnss_H_6(double *state, double *sat_pos, double *out_4010469744061131744);
|
||||
void gnss_h_20(double *state, double *sat_pos, double *out_4397465078196892446);
|
||||
void gnss_H_20(double *state, double *sat_pos, double *out_7230021967014098202);
|
||||
void gnss_h_7(double *state, double *sat_pos_vel, double *out_8859539764878579118);
|
||||
void gnss_H_7(double *state, double *sat_pos_vel, double *out_6351382189608033375);
|
||||
void gnss_h_21(double *state, double *sat_pos_vel, double *out_8859539764878579118);
|
||||
void gnss_H_21(double *state, double *sat_pos_vel, double *out_6351382189608033375);
|
||||
void gnss_err_fun(double *nom_x, double *delta_x, double *out_6208051589904680687);
|
||||
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_980651649795713821);
|
||||
void gnss_H_mod_fun(double *state, double *out_8761108313881456199);
|
||||
void gnss_f_fun(double *state, double dt, double *out_6985979927194349446);
|
||||
void gnss_F_fun(double *state, double dt, double *out_5398596809828011208);
|
||||
void gnss_h_6(double *state, double *sat_pos, double *out_854109254235318073);
|
||||
void gnss_H_6(double *state, double *sat_pos, double *out_4878360041893525515);
|
||||
void gnss_h_20(double *state, double *sat_pos, double *out_3910060068815622044);
|
||||
void gnss_H_20(double *state, double *sat_pos, double *out_7601181949165199926);
|
||||
void gnss_h_7(double *state, double *sat_pos_vel, double *out_7752585033610334316);
|
||||
void gnss_H_7(double *state, double *sat_pos_vel, double *out_8389742934130346329);
|
||||
void gnss_h_21(double *state, double *sat_pos_vel, double *out_7752585033610334316);
|
||||
void gnss_H_21(double *state, double *sat_pos_vel, double *out_8389742934130346329);
|
||||
void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
}
|
||||
Binary file not shown.
File diff suppressed because it is too large
Load Diff
@@ -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_6409019039917668284);
|
||||
void live_err_fun(double *nom_x, double *delta_x, double *out_4061617048242378524);
|
||||
void live_inv_err_fun(double *nom_x, double *true_x, double *out_1032655403399547817);
|
||||
void live_H_mod_fun(double *state, double *out_1457537824329529482);
|
||||
void live_f_fun(double *state, double dt, double *out_7387809313477893541);
|
||||
void live_F_fun(double *state, double dt, double *out_9180543782983665576);
|
||||
void live_h_4(double *state, double *unused, double *out_4492390463949384143);
|
||||
void live_H_4(double *state, double *unused, double *out_6840823934948797475);
|
||||
void live_h_9(double *state, double *unused, double *out_7683409762000862661);
|
||||
void live_H_9(double *state, double *unused, double *out_6599634288319206830);
|
||||
void live_h_10(double *state, double *unused, double *out_9216865912583274047);
|
||||
void live_H_10(double *state, double *unused, double *out_773562674632846954);
|
||||
void live_h_12(double *state, double *unused, double *out_7476619097990127394);
|
||||
void live_H_12(double *state, double *unused, double *out_1821367526916835680);
|
||||
void live_h_35(double *state, double *unused, double *out_162724865929704519);
|
||||
void live_H_35(double *state, double *unused, double *out_924195505408178029);
|
||||
void live_h_32(double *state, double *unused, double *out_4377549943098853278);
|
||||
void live_H_32(double *state, double *unused, double *out_3416809270324928859);
|
||||
void live_h_13(double *state, double *unused, double *out_1977043243384356195);
|
||||
void live_H_13(double *state, double *unused, double *out_7445239302620530630);
|
||||
void live_h_14(double *state, double *unused, double *out_7683409762000862661);
|
||||
void live_H_14(double *state, double *unused, double *out_6599634288319206830);
|
||||
void live_h_33(double *state, double *unused, double *out_605021574862289458);
|
||||
void live_H_33(double *state, double *unused, double *out_323604872937332495);
|
||||
void live_H(double *in_vec, double *out_5173492457777781966);
|
||||
void live_err_fun(double *nom_x, double *delta_x, double *out_5922210577939604492);
|
||||
void live_inv_err_fun(double *nom_x, double *true_x, double *out_6180260714953073839);
|
||||
void live_H_mod_fun(double *state, double *out_4517018211893934989);
|
||||
void live_f_fun(double *state, double dt, double *out_8888895791769188636);
|
||||
void live_F_fun(double *state, double dt, double *out_466548289212658788);
|
||||
void live_h_4(double *state, double *unused, double *out_7903893439571717825);
|
||||
void live_H_4(double *state, double *unused, double *out_8120204986329223125);
|
||||
void live_h_9(double *state, double *unused, double *out_1448285436355291799);
|
||||
void live_H_9(double *state, double *unused, double *out_7879015339699632480);
|
||||
void live_h_10(double *state, double *unused, double *out_7833176734581826377);
|
||||
void live_H_10(double *state, double *unused, double *out_2945925451724885313);
|
||||
void live_h_12(double *state, double *unused, double *out_7236527996738028961);
|
||||
void live_H_12(double *state, double *unused, double *out_3100748578297261330);
|
||||
void live_h_35(double *state, double *unused, double *out_6306229462798517433);
|
||||
void live_H_35(double *state, double *unused, double *out_355185545972247621);
|
||||
void live_h_32(double *state, double *unused, double *out_8578926984007907638);
|
||||
void live_H_32(double *state, double *unused, double *out_5446118826235364937);
|
||||
void live_h_13(double *state, double *unused, double *out_8977427483885865133);
|
||||
void live_H_13(double *state, double *unused, double *out_7231128031793255484);
|
||||
void live_h_14(double *state, double *unused, double *out_1448285436355291799);
|
||||
void live_H_14(double *state, double *unused, double *out_7879015339699632480);
|
||||
void live_h_33(double *state, double *unused, double *out_3309457895536849751);
|
||||
void live_H_33(double *state, double *unused, double *out_2795371458666609983);
|
||||
void live_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
}
|
||||
@@ -8,6 +8,7 @@ from cereal import car
|
||||
from common.params import Params, put_nonblocking
|
||||
from common.realtime import config_realtime_process, DT_MDL
|
||||
from common.numpy_fast import clip
|
||||
from selfdrive.car.chrysler.values import ChryslerFlags
|
||||
from selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States
|
||||
from selfdrive.locationd.models.constants import GENERATED_DIR
|
||||
from system.swaglog import cloudlog
|
||||
@@ -199,6 +200,8 @@ def main(sm=None, pm=None):
|
||||
0.2 <= liveParameters.stiffnessFactor <= 5.0,
|
||||
min_sr <= liveParameters.steerRatio <= max_sr,
|
||||
))
|
||||
if CP.carName == "chrysler" and CP.flags & ChryslerFlags.SP_RAM_HD_PARAMSD_IGNORE:
|
||||
liveParameters.valid = True
|
||||
liveParameters.steerRatioStd = float(P[States.STEER_RATIO])
|
||||
liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS])
|
||||
liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET])
|
||||
|
||||
@@ -8,7 +8,6 @@ import time
|
||||
import traceback
|
||||
from common.basedir import BASEDIR
|
||||
from common.text_window import TextWindow
|
||||
import selfdrive.sentry as sentry
|
||||
from urllib.request import urlopen
|
||||
from glob import glob
|
||||
import subprocess
|
||||
@@ -106,18 +105,36 @@ if __name__ == "__main__" and (OPSPLINE_SPEC is None or OVERPY_SPEC is None):
|
||||
print(f"SP_LOG: Preloaded dependencies extracted to {THIRD_PARTY_DIR}")
|
||||
except Exception as e:
|
||||
preload_fault = True
|
||||
command = f'rm -rf {THIRD_PARTY_DIR}'
|
||||
process = subprocess.Popen(command, stdout=subprocess.PIPE, shell=True)
|
||||
print(f"SP_LOG: An error occurred while extracting preloaded dependencies: {e}")
|
||||
print(f"SP_LOG: Cleanup directory {e}")
|
||||
if not os.path.exists(PRELOADED_DEP_FILE) or preload_fault:
|
||||
if os.path.exists(THIRD_PARTY_DIR_SP):
|
||||
spinner.update("Loading dependencies")
|
||||
command = f'rm -rf {THIRD_PARTY_DIR}; cp -rf {THIRD_PARTY_DIR_SP} {THIRD_PARTY_DIR}'
|
||||
process = subprocess.Popen(command, stdout=subprocess.PIPE, shell=True)
|
||||
print(f"SP_LOG: Removed directory {THIRD_PARTY_DIR}")
|
||||
print(f"SP_LOG: Copied {THIRD_PARTY_DIR_SP} to {THIRD_PARTY_DIR}")
|
||||
try:
|
||||
spinner.update("Loading cached dependencies")
|
||||
command = f'rm -rf {THIRD_PARTY_DIR}; cp -rf {THIRD_PARTY_DIR_SP} {THIRD_PARTY_DIR}'
|
||||
process = subprocess.Popen(command, stdout=subprocess.PIPE, shell=True)
|
||||
print(f"SP_LOG: Removed directory {THIRD_PARTY_DIR}")
|
||||
print(f"SP_LOG: Copied {THIRD_PARTY_DIR_SP} to {THIRD_PARTY_DIR}")
|
||||
except Exception as e:
|
||||
command = f'rm -rf {THIRD_PARTY_DIR}'
|
||||
process = subprocess.Popen(command, stdout=subprocess.PIPE, shell=True)
|
||||
print(f"SP_LOG: An error occurred while loading cached dependencies: {e}")
|
||||
print(f"SP_LOG: Cleanup directory {e}")
|
||||
else:
|
||||
spinner.update("Waiting for internet")
|
||||
install_dep(spinner)
|
||||
try:
|
||||
install_dep(spinner)
|
||||
except Exception as e:
|
||||
command = f'rm -rf {THIRD_PARTY_DIR}'
|
||||
process = subprocess.Popen(command, stdout=subprocess.PIPE, shell=True)
|
||||
print(f"SP_LOG: An error occurred while downloading dependencies: {e}")
|
||||
print(f"SP_LOG: Cleanup directory {e}")
|
||||
except Exception:
|
||||
command = f'rm -rf {THIRD_PARTY_DIR}'
|
||||
process = subprocess.Popen(command, stdout=subprocess.PIPE, shell=True)
|
||||
import selfdrive.sentry as sentry
|
||||
sentry.init(sentry.SentryProject.SELFDRIVE)
|
||||
traceback.print_exc()
|
||||
sentry.capture_exception()
|
||||
|
||||
@@ -47,6 +47,7 @@ def manager_init() -> None:
|
||||
|
||||
("AccMadsCombo", "1"),
|
||||
("AutoLaneChangeTimer", "0"),
|
||||
("AutoLaneChangeBsmDelay", "1"),
|
||||
("BelowSpeedPause", "0"),
|
||||
("BrakeLights", "0"),
|
||||
("BrightnessControl", "0"),
|
||||
@@ -78,10 +79,12 @@ def manager_init() -> None:
|
||||
("HandsOnWheelMonitoring", "0"),
|
||||
("HideVEgoUi", "0"),
|
||||
("LastSpeedLimitSignTap", "0"),
|
||||
("LkasToggle", "0"),
|
||||
("MadsIconToggle", "1"),
|
||||
("MaxTimeOffroad", "9"),
|
||||
("OnroadScreenOff", "0"),
|
||||
("OnroadScreenOff", "-2"),
|
||||
("OnroadScreenOffBrightness", "50"),
|
||||
("OnroadScreenOffEvent", "1"),
|
||||
("PathOffset", "0"),
|
||||
("ReverseAccChange", "0"),
|
||||
("ShowDebugUI", "1"),
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,5 +1,10 @@
|
||||
#pragma once
|
||||
|
||||
// gate this here
|
||||
#define TEMPORAL
|
||||
#define DESIRE
|
||||
#define TRAFFIC_CONVENTION
|
||||
|
||||
#include <array>
|
||||
#include <memory>
|
||||
|
||||
@@ -12,12 +17,6 @@
|
||||
#include "selfdrive/modeld/models/nav.h"
|
||||
#include "selfdrive/modeld/runners/run.h"
|
||||
|
||||
// gate this here
|
||||
#define TEMPORAL
|
||||
#define DESIRE
|
||||
#define TRAFFIC_CONVENTION
|
||||
#define NAV
|
||||
|
||||
constexpr int FEATURE_LEN = 128;
|
||||
constexpr int HISTORY_BUFFER_LEN = 99;
|
||||
constexpr int DESIRE_LEN = 8;
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
+2
-2
@@ -60,7 +60,7 @@ def report_tombstone(fn: str, message: str, contents: str) -> None:
|
||||
def capture_exception(*args, **kwargs) -> None:
|
||||
save_exception(traceback.format_exc())
|
||||
cloudlog.error("crash", exc_info=kwargs.get('exc_info', 1))
|
||||
bind_user(id=dongle_id, ip_address=ip, name=gitname)
|
||||
bind_user(id=dongle_id, ip_address=ip, username=gitname)
|
||||
|
||||
try:
|
||||
sentry_sdk.capture_exception(*args, **kwargs)
|
||||
@@ -135,7 +135,7 @@ def init(project: SentryProject) -> None:
|
||||
|
||||
sentry_sdk.set_user({"id": dongle_id})
|
||||
sentry_sdk.set_user({"ip_address": ip})
|
||||
sentry_sdk.set_user({"name": gitname})
|
||||
sentry_sdk.set_user({"username": gitname})
|
||||
sentry_sdk.set_tag("dirty", is_dirty())
|
||||
sentry_sdk.set_tag("origin", get_origin())
|
||||
sentry_sdk.set_tag("branch", get_branch())
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -354,7 +354,19 @@ Please use caution when using this feature. Only use the blinker when traffic an
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Fleet Manager PIN</source>
|
||||
<source>TOGGLE</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable or disable PIN requirement for Fleet Manager access.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Are you sure you want to turn off PIN requirement?</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Turn Off</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
@@ -365,6 +377,22 @@ Please use caution when using this feature. Only use the blinker when traffic an
|
||||
<source>Display error from the tmux session when an error has occurred from a system process.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Reset Mapbox Access Token</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Are you sure you want to reset the Mapbox access token?</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Reset sunnypilot Settings</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Are you sure you want to reset all sunnypilot settings?</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>DriveStats</name>
|
||||
@@ -1003,7 +1031,19 @@ This may take up to a minute.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>You must restart your car or your device to apply these changes.</source>
|
||||
<source>Block Lane Change: Road Edge Detection</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable this toggle to block lane change when road edge is detected on the stalk actuated side.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Auto Lane Change: Delay with Blind Spot</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Toggle to enable a delay timer for seamless lane changes when blind spot monitoring (BSM) detects a obstructing vehicle, ensuring safe maneuvering.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
@@ -1011,11 +1051,7 @@ This may take up to a minute.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Block Lane Change: Road Edge Detection</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable this toggle to block lane change when road edge is detected on the stalk actuated side.</source>
|
||||
<source>You must restart your car or your device to apply these changes.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
@@ -1077,6 +1113,22 @@ This may take up to a minute.</source>
|
||||
<source>Enabling this toggle will retain the hotspot/tethering toggle state across reboots.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Driving Screen Off: Non-Critical Events</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>When <b>Driving Screen Off Timer</b> is not set to <b>"Always On"</b>:</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enabled: Wake the brightness of the screen to display all events.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Disabled: Wake the brightness of the screen to display critical events.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>SPVehiclesTogglesPanel</name>
|
||||
@@ -1084,6 +1136,18 @@ This may take up to a minute.</source>
|
||||
<source>Toyota/Lexus</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Allow M.A.D.S. toggling w/ LKAS Button (Beta)</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Allows M.A.D.S. engagement/disengagement with "LKAS" button from the steering wheel.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Note: Enabling this toggle may have unexpected behavior with steering control. It is the driver's responsibility to observe their environment and make decisions accordingly.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>SPVisualsPanel</name>
|
||||
@@ -1466,6 +1530,10 @@ Reboot now?</source>
|
||||
<source>Uninstall</source>
|
||||
<translation>Deinstallieren</translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Current Driving Model</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Database updates can be downloaded while the car is off.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
@@ -1495,10 +1563,6 @@ Estimated time: 30-90 minutes</source>
|
||||
<source>Are you sure you want to reboot to start downloading the selected database? Estimated time: 30-90 minutes</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Current Driving Model</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>SpeedLimitOffsetType</name>
|
||||
|
||||
@@ -354,7 +354,19 @@ Please use caution when using this feature. Only use the blinker when traffic an
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Fleet Manager PIN</source>
|
||||
<source>TOGGLE</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable or disable PIN requirement for Fleet Manager access.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Are you sure you want to turn off PIN requirement?</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Turn Off</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
@@ -365,6 +377,22 @@ Please use caution when using this feature. Only use the blinker when traffic an
|
||||
<source>Display error from the tmux session when an error has occurred from a system process.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Reset Mapbox Access Token</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Are you sure you want to reset the Mapbox access token?</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Reset sunnypilot Settings</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Are you sure you want to reset all sunnypilot settings?</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>DriveStats</name>
|
||||
@@ -1001,7 +1029,19 @@ This may take up to a minute.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>You must restart your car or your device to apply these changes.</source>
|
||||
<source>Block Lane Change: Road Edge Detection</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable this toggle to block lane change when road edge is detected on the stalk actuated side.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Auto Lane Change: Delay with Blind Spot</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Toggle to enable a delay timer for seamless lane changes when blind spot monitoring (BSM) detects a obstructing vehicle, ensuring safe maneuvering.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
@@ -1009,11 +1049,7 @@ This may take up to a minute.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Block Lane Change: Road Edge Detection</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable this toggle to block lane change when road edge is detected on the stalk actuated side.</source>
|
||||
<source>You must restart your car or your device to apply these changes.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
@@ -1075,6 +1111,22 @@ This may take up to a minute.</source>
|
||||
<source>Enabling this toggle will retain the hotspot/tethering toggle state across reboots.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Driving Screen Off: Non-Critical Events</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>When <b>Driving Screen Off Timer</b> is not set to <b>"Always On"</b>:</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enabled: Wake the brightness of the screen to display all events.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Disabled: Wake the brightness of the screen to display critical events.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>SPVehiclesTogglesPanel</name>
|
||||
@@ -1082,6 +1134,18 @@ This may take up to a minute.</source>
|
||||
<source>Toyota/Lexus</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Allow M.A.D.S. toggling w/ LKAS Button (Beta)</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Allows M.A.D.S. engagement/disengagement with "LKAS" button from the steering wheel.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Note: Enabling this toggle may have unexpected behavior with steering control. It is the driver's responsibility to observe their environment and make decisions accordingly.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>SPVisualsPanel</name>
|
||||
@@ -1462,6 +1526,10 @@ Reboot now?</source>
|
||||
<source>Uninstall</source>
|
||||
<translation>アンインストール</translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Current Driving Model</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Database updates can be downloaded while the car is off.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
@@ -1491,10 +1559,6 @@ Estimated time: 30-90 minutes</source>
|
||||
<source>Are you sure you want to reboot to start downloading the selected database? Estimated time: 30-90 minutes</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Current Driving Model</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>SpeedLimitOffsetType</name>
|
||||
|
||||
@@ -354,7 +354,19 @@ Please use caution when using this feature. Only use the blinker when traffic an
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Fleet Manager PIN</source>
|
||||
<source>TOGGLE</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable or disable PIN requirement for Fleet Manager access.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Are you sure you want to turn off PIN requirement?</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Turn Off</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
@@ -365,6 +377,22 @@ Please use caution when using this feature. Only use the blinker when traffic an
|
||||
<source>Display error from the tmux session when an error has occurred from a system process.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Reset Mapbox Access Token</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Are you sure you want to reset the Mapbox access token?</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Reset sunnypilot Settings</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Are you sure you want to reset all sunnypilot settings?</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>DriveStats</name>
|
||||
@@ -1002,7 +1030,19 @@ This may take up to a minute.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>You must restart your car or your device to apply these changes.</source>
|
||||
<source>Block Lane Change: Road Edge Detection</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable this toggle to block lane change when road edge is detected on the stalk actuated side.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Auto Lane Change: Delay with Blind Spot</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Toggle to enable a delay timer for seamless lane changes when blind spot monitoring (BSM) detects a obstructing vehicle, ensuring safe maneuvering.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
@@ -1010,11 +1050,7 @@ This may take up to a minute.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Block Lane Change: Road Edge Detection</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable this toggle to block lane change when road edge is detected on the stalk actuated side.</source>
|
||||
<source>You must restart your car or your device to apply these changes.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
@@ -1076,6 +1112,22 @@ This may take up to a minute.</source>
|
||||
<source>Enabling this toggle will retain the hotspot/tethering toggle state across reboots.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Driving Screen Off: Non-Critical Events</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>When <b>Driving Screen Off Timer</b> is not set to <b>"Always On"</b>:</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enabled: Wake the brightness of the screen to display all events.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Disabled: Wake the brightness of the screen to display critical events.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>SPVehiclesTogglesPanel</name>
|
||||
@@ -1083,6 +1135,18 @@ This may take up to a minute.</source>
|
||||
<source>Toyota/Lexus</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Allow M.A.D.S. toggling w/ LKAS Button (Beta)</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Allows M.A.D.S. engagement/disengagement with "LKAS" button from the steering wheel.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Note: Enabling this toggle may have unexpected behavior with steering control. It is the driver's responsibility to observe their environment and make decisions accordingly.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>SPVisualsPanel</name>
|
||||
@@ -1463,6 +1527,10 @@ Reboot now?</source>
|
||||
<source>Uninstall</source>
|
||||
<translation>제거</translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Current Driving Model</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Database updates can be downloaded while the car is off.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
@@ -1492,10 +1560,6 @@ Estimated time: 30-90 minutes</source>
|
||||
<source>Are you sure you want to reboot to start downloading the selected database? Estimated time: 30-90 minutes</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Current Driving Model</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>SpeedLimitOffsetType</name>
|
||||
|
||||
@@ -354,15 +354,19 @@ Please use caution when using this feature. Only use the blinker when traffic an
|
||||
<translation>Revisar</translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Review the rules, features, and limitations of sunnypilot</source>
|
||||
<source>TOGGLE</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>sunnypilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. sunnypilot is continuously calibrating, resetting is rarely required.</source>
|
||||
<source>Enable or disable PIN requirement for Fleet Manager access.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Fleet Manager PIN</source>
|
||||
<source>Are you sure you want to turn off PIN requirement?</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Turn Off</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
@@ -373,6 +377,30 @@ Please use caution when using this feature. Only use the blinker when traffic an
|
||||
<source>Display error from the tmux session when an error has occurred from a system process.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Reset Mapbox Access Token</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Are you sure you want to reset the Mapbox access token?</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Reset sunnypilot Settings</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Are you sure you want to reset all sunnypilot settings?</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Review the rules, features, and limitations of sunnypilot</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>sunnypilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. sunnypilot is continuously calibrating, resetting is rarely required.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>DriveStats</name>
|
||||
@@ -901,6 +929,14 @@ Isso pode levar até um minuto.</translation>
|
||||
<source>Enable this toggle to pause lateral actuation with blinker when traveling below 30 MPH or 50 KM/H.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Block Lane Change: Road Edge Detection</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable this toggle to block lane change when road edge is detected on the stalk actuated side.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable Dynamic Lane Profile</source>
|
||||
<translation type="unfinished"></translation>
|
||||
@@ -925,6 +961,14 @@ Isso pode levar até um minuto.</translation>
|
||||
<source>Add custom offsets to Camera and Path in sunnypilot.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Auto Lane Change: Delay with Blind Spot</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Toggle to enable a delay timer for seamless lane changes when blind spot monitoring (BSM) detects a obstructing vehicle, ensuring safe maneuvering.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable Gap Adjust Cruise ℹ</source>
|
||||
<translation type="unfinished"></translation>
|
||||
@@ -953,10 +997,6 @@ Isso pode levar até um minuto.</translation>
|
||||
<source>Enables live tune for Torque lateral control.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>You must restart your car or your device to apply these changes.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Torque Lateral Controller Self-Tune</source>
|
||||
<translation type="unfinished"></translation>
|
||||
@@ -1026,11 +1066,7 @@ Isso pode levar até um minuto.</translation>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Block Lane Change: Road Edge Detection</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable this toggle to block lane change when road edge is detected on the stalk actuated side.</source>
|
||||
<source>You must restart your car or your device to apply these changes.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
@@ -1064,6 +1100,30 @@ Isso pode levar até um minuto.</translation>
|
||||
<source>Enable this will notify when the leading vehicle drives away.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Retain hotspot/tethering state</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enabling this toggle will retain the hotspot/tethering toggle state across reboots.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Driving Screen Off: Non-Critical Events</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>When <b>Driving Screen Off Timer</b> is not set to <b>"Always On"</b>:</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enabled: Wake the brightness of the screen to display all events.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Disabled: Wake the brightness of the screen to display critical events.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Disable Onroad Uploads</source>
|
||||
<translation type="unfinished"></translation>
|
||||
@@ -1072,10 +1132,6 @@ Isso pode levar até um minuto.</translation>
|
||||
<source>Disable uploads completely when onroad. Necessary to avoid high data usage when connected to Wi-Fi hotspot. Turn on this feature if you are looking to utilize map-based features, such as Speed Limit Control (SLC) and Map-based Turn Speed Control (MTSC).</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>VIEW</source>
|
||||
<translation type="obsolete">VER</translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Debug snapshot on screen center tap</source>
|
||||
<translation type="unfinished"></translation>
|
||||
@@ -1084,14 +1140,6 @@ Isso pode levar até um minuto.</translation>
|
||||
<source>Stores snapshot file with current state of some modules.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Retain hotspot/tethering state</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enabling this toggle will retain the hotspot/tethering toggle state across reboots.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>SPVehiclesTogglesPanel</name>
|
||||
@@ -1099,6 +1147,18 @@ Isso pode levar até um minuto.</translation>
|
||||
<source>Toyota/Lexus</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Allow M.A.D.S. toggling w/ LKAS Button (Beta)</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Allows M.A.D.S. engagement/disengagement with "LKAS" button from the steering wheel.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Note: Enabling this toggle may have unexpected behavior with steering control. It is the driver's responsibility to observe their environment and make decisions accordingly.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>SPVisualsPanel</name>
|
||||
@@ -1170,12 +1230,6 @@ Isso pode levar até um minuto.</translation>
|
||||
<source>Huge thanks to the dragonpilot team for making this possible!</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>"Enable Mapbox Navigation"
|
||||
You must restart your car or your device to apply these changes.
|
||||
Reboot now?</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Speedometer: Display True Speed</source>
|
||||
<translation type="unfinished"></translation>
|
||||
@@ -1204,6 +1258,12 @@ Reboot now?</source>
|
||||
<source>Enable this will display the CPU core with the highest temperature on the sidebar.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>"Enable Mapbox Navigation"
|
||||
You must restart your car or your device to apply these changes.
|
||||
Reboot now?</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>SettingsWindow</name>
|
||||
@@ -1479,6 +1539,10 @@ Reboot now?</source>
|
||||
<source>Uninstall</source>
|
||||
<translation>Desinstalar</translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Current Driving Model</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Database updates can be downloaded while the car is off.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
@@ -1508,10 +1572,6 @@ Estimated time: 30-90 minutes</source>
|
||||
<source>Are you sure you want to reboot to start downloading the selected database? Estimated time: 30-90 minutes</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Current Driving Model</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>SpeedLimitOffsetType</name>
|
||||
@@ -1746,10 +1806,6 @@ Estimated time: 30-90 minutes</source>
|
||||
<source>Use the sunnypilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>When enabled, pressing the accelerator pedal will disengage sunnypilot.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Custom Stock Longitudinal Control</source>
|
||||
<translation type="unfinished"></translation>
|
||||
@@ -1759,6 +1815,10 @@ Estimated time: 30-90 minutes</source>
|
||||
This feature must be used along with SLC, and/or V-TSC, and/or M-TSC.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>When enabled, pressing the accelerator pedal will disengage sunnypilot.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>TorqueFriction</name>
|
||||
|
||||
@@ -354,7 +354,19 @@ Please use caution when using this feature. Only use the blinker when traffic an
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Fleet Manager PIN</source>
|
||||
<source>TOGGLE</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable or disable PIN requirement for Fleet Manager access.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Are you sure you want to turn off PIN requirement?</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Turn Off</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
@@ -365,6 +377,22 @@ Please use caution when using this feature. Only use the blinker when traffic an
|
||||
<source>Display error from the tmux session when an error has occurred from a system process.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Reset Mapbox Access Token</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Are you sure you want to reset the Mapbox access token?</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Reset sunnypilot Settings</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Are you sure you want to reset all sunnypilot settings?</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>DriveStats</name>
|
||||
@@ -999,7 +1027,19 @@ This may take up to a minute.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>You must restart your car or your device to apply these changes.</source>
|
||||
<source>Block Lane Change: Road Edge Detection</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable this toggle to block lane change when road edge is detected on the stalk actuated side.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Auto Lane Change: Delay with Blind Spot</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Toggle to enable a delay timer for seamless lane changes when blind spot monitoring (BSM) detects a obstructing vehicle, ensuring safe maneuvering.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
@@ -1007,11 +1047,7 @@ This may take up to a minute.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Block Lane Change: Road Edge Detection</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable this toggle to block lane change when road edge is detected on the stalk actuated side.</source>
|
||||
<source>You must restart your car or your device to apply these changes.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
@@ -1073,6 +1109,22 @@ This may take up to a minute.</source>
|
||||
<source>Enabling this toggle will retain the hotspot/tethering toggle state across reboots.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Driving Screen Off: Non-Critical Events</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>When <b>Driving Screen Off Timer</b> is not set to <b>"Always On"</b>:</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enabled: Wake the brightness of the screen to display all events.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Disabled: Wake the brightness of the screen to display critical events.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>SPVehiclesTogglesPanel</name>
|
||||
@@ -1080,6 +1132,18 @@ This may take up to a minute.</source>
|
||||
<source>Toyota/Lexus</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Allow M.A.D.S. toggling w/ LKAS Button (Beta)</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Allows M.A.D.S. engagement/disengagement with "LKAS" button from the steering wheel.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Note: Enabling this toggle may have unexpected behavior with steering control. It is the driver's responsibility to observe their environment and make decisions accordingly.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>SPVisualsPanel</name>
|
||||
@@ -1460,6 +1524,10 @@ Reboot now?</source>
|
||||
<source>Uninstall</source>
|
||||
<translation>卸载</translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Current Driving Model</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Database updates can be downloaded while the car is off.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
@@ -1489,10 +1557,6 @@ Estimated time: 30-90 minutes</source>
|
||||
<source>Are you sure you want to reboot to start downloading the selected database? Estimated time: 30-90 minutes</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Current Driving Model</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>SpeedLimitOffsetType</name>
|
||||
|
||||
@@ -358,7 +358,19 @@ Please use caution when using this feature. Only use the blinker when traffic an
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Fleet Manager PIN</source>
|
||||
<source>TOGGLE</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable or disable PIN requirement for Fleet Manager access.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Are you sure you want to turn off PIN requirement?</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Turn Off</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
@@ -369,6 +381,22 @@ Please use caution when using this feature. Only use the blinker when traffic an
|
||||
<source>Display error from the tmux session when an error has occurred from a system process.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Reset Mapbox Access Token</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Are you sure you want to reset the Mapbox access token?</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Reset sunnypilot Settings</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Are you sure you want to reset all sunnypilot settings?</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>DriveStats</name>
|
||||
@@ -1005,7 +1033,19 @@ This may take up to a minute.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>You must restart your car or your device to apply these changes.</source>
|
||||
<source>Block Lane Change: Road Edge Detection</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable this toggle to block lane change when road edge is detected on the stalk actuated side.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Auto Lane Change: Delay with Blind Spot</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Toggle to enable a delay timer for seamless lane changes when blind spot monitoring (BSM) detects a obstructing vehicle, ensuring safe maneuvering.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
@@ -1013,11 +1053,7 @@ This may take up to a minute.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Block Lane Change: Road Edge Detection</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable this toggle to block lane change when road edge is detected on the stalk actuated side.</source>
|
||||
<source>You must restart your car or your device to apply these changes.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
@@ -1079,6 +1115,22 @@ This may take up to a minute.</source>
|
||||
<source>Enabling this toggle will retain the hotspot/tethering toggle state across reboots.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Driving Screen Off: Non-Critical Events</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>When <b>Driving Screen Off Timer</b> is not set to <b>"Always On"</b>:</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enabled: Wake the brightness of the screen to display all events.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Disabled: Wake the brightness of the screen to display critical events.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>SPVehiclesTogglesPanel</name>
|
||||
@@ -1086,6 +1138,18 @@ This may take up to a minute.</source>
|
||||
<source>Toyota/Lexus</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Allow M.A.D.S. toggling w/ LKAS Button (Beta)</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Allows M.A.D.S. engagement/disengagement with "LKAS" button from the steering wheel.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Note: Enabling this toggle may have unexpected behavior with steering control. It is the driver's responsibility to observe their environment and make decisions accordingly.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>SPVisualsPanel</name>
|
||||
@@ -1466,6 +1530,10 @@ Reboot now?</source>
|
||||
<source>Uninstall</source>
|
||||
<translation>解除安裝</translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Current Driving Model</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Database updates can be downloaded while the car is off.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
@@ -1495,10 +1563,6 @@ Estimated time: 30-90 minutes</source>
|
||||
<source>Are you sure you want to reboot to start downloading the selected database? Estimated time: 30-90 minutes</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Current Driving Model</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>SpeedLimitOffsetType</name>
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -3,6 +3,7 @@ import subprocess
|
||||
from flask import render_template, request, session
|
||||
from functools import wraps
|
||||
from pathlib import Path
|
||||
from common.params import Params
|
||||
from system.hardware import PC
|
||||
from system.loggerd.config import ROOT as REALDATA
|
||||
from system.loggerd.uploader import listdir_by_creation
|
||||
@@ -23,7 +24,7 @@ else:
|
||||
def login_required(f):
|
||||
@wraps(f)
|
||||
def decorated_route(*args, **kwargs):
|
||||
if not session.get("logged_in"):
|
||||
if not session.get("logged_in") and Params().get_bool("FleetManagerPin"):
|
||||
session["previous_page"] = request.url
|
||||
return render_template("login.html")
|
||||
return f(*args, **kwargs)
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user