mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-07-09 15:02:05 +08:00
Merge branch 'devel-zht' into 0.6.4-zht
This commit is contained in:
dragonpilot
@@ -79,6 +79,7 @@ Supported Cars
|
||||
| Honda | CR-V 2015-16 | Touring | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
|
||||
| Honda | CR-V 2017-19 | Honda Sensing | Yes | Stock | 0mph | 12mph | Bosch |
|
||||
| Honda | CR-V Hybrid 2017-2019 | Honda Sensing | Yes | Stock | 0mph | 12mph | Bosch |
|
||||
| Honda | Fit 2018-19 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph | Inverted Nidec |
|
||||
| Honda | Odyssey 2018-19 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 0mph | Inverted Nidec |
|
||||
| Honda | Passport 2019 | All | Yes | Yes | 25mph<sup>1</sup>| 12mph | Inverted Nidec |
|
||||
| Honda | Pilot 2016-18 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
|
||||
@@ -111,7 +112,7 @@ Supported Cars
|
||||
| Toyota | Highlander Hybrid 2017-19 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Prius 2016 | TSS-P | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Prius 2017-19 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Prius Prime 2017-19 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Prius Prime 2017-20 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Rav4 2016 | TSS-P | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
|
||||
| Toyota | Rav4 2017-18 | All | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
|
||||
| Toyota | Rav4 2019 | All | Yes | Yes | 0mph | 0mph | Toyota |
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -122,6 +122,15 @@ keys = {
|
||||
"DragonGreyPandaMode": [TxType.PERSISTENT],
|
||||
"DragonDrivingUI": [TxType.PERSISTENT],
|
||||
"DragonDisplaySteeringLimitAlert": [TxType.PERSISTENT],
|
||||
"DragonChargingCtrl": [TxType.PERSISTENT],
|
||||
"DragonCharging": [TxType.PERSISTENT],
|
||||
"DragonDisCharging": [TxType.PERSISTENT],
|
||||
"DragonToyotaLaneDepartureWarning": [TxType.PERSISTENT],
|
||||
"DragonUILane": [TxType.PERSISTENT],
|
||||
"DragonUILead": [TxType.PERSISTENT],
|
||||
"DragonUIPath": [TxType.PERSISTENT],
|
||||
"DragonUIBlinker": [TxType.PERSISTENT],
|
||||
"DragonEnableDriverMonitoring": [TxType.PERSISTENT],
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -365,6 +365,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
|
||||
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
|
||||
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_ANGLE : 31|16@0- (0.056,0) [-500|500] "" XXX
|
||||
|
||||
BO_ 610 EPS_STATUS: 5 EPS
|
||||
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -361,6 +361,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
|
||||
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
|
||||
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_ANGLE : 31|16@0- (0.056,0) [-500|500] "" XXX
|
||||
|
||||
BO_ 610 EPS_STATUS: 5 EPS
|
||||
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
|
||||
@@ -385,4 +386,4 @@ CM_ SG_ 610 TYPE "seems 1 on Corolla, 0 on all others";
|
||||
CM_ SG_ 1009 SET_SPEED "units seem to be whatever the car is set to";
|
||||
VAL_ 956 GEAR 0 "D" 1 "S" 8 "N" 16 "R" 32 "P";
|
||||
VAL_ 610 IPAS_STATE 5 "override" 3 "enabled" 1 "disabled";
|
||||
VAL_ 610 LKA_STATE 25 "temporary_fault" 9 "temporary_fault2" 5 "active" 1 "standby";
|
||||
VAL_ 610 LKA_STATE 25 "temporary_fault" 9 "temporary_fault2" 5 "active" 1 "standby";
|
||||
|
||||
@@ -95,7 +95,7 @@ class CarController(object):
|
||||
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
|
||||
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
|
||||
# dragonpilot, don't check for param too often as it's a kernel call
|
||||
if frame % 100 == 0:
|
||||
if frame % 500 == 0:
|
||||
self.dragon_enable_steering_on_signal = False if params.get("DragonEnableSteeringOnSignal") == "0" else True
|
||||
self.dragon_allow_gas = False if params.get("DragonAllowGas") == "0" else True
|
||||
self.dragon_lat_ctrl = False if params.get("DragonLatCtrl") == "0" else True
|
||||
|
||||
@@ -122,15 +122,17 @@ class CarController(object):
|
||||
self.dragon_enable_steering_on_signal = False
|
||||
self.dragon_allow_gas = False
|
||||
self.dragon_lat_ctrl = True
|
||||
self.dragon_lane_departure_warning = True
|
||||
|
||||
def update(self, enabled, CS, frame, actuators,
|
||||
pcm_cancel_cmd, hud_alert, forwarding_camera, left_line,
|
||||
right_line, lead, left_lane_depart, right_lane_depart):
|
||||
# dragonpilot, don't check for param too often as it's a kernel call
|
||||
if frame % 100 == 0:
|
||||
if frame % 500 == 0:
|
||||
self.dragon_enable_steering_on_signal = False if params.get("DragonEnableSteeringOnSignal") == "0" else True
|
||||
self.dragon_allow_gas = False if params.get("DragonAllowGas") == "0" else True
|
||||
self.dragon_lat_ctrl = False if params.get("DragonLatCtrl") == "0" else True
|
||||
self.dragon_lane_departure_warning = False if params.get("DragonToyotaLaneDepartureWarning") == "0" else True
|
||||
|
||||
# *** compute control surfaces ***
|
||||
|
||||
@@ -158,7 +160,7 @@ class CarController(object):
|
||||
self.last_fault_frame = frame
|
||||
|
||||
# Cut steering for 2s after fault
|
||||
if not enabled or (frame - self.last_fault_frame < 200):
|
||||
if not enabled: # or (frame - self.last_fault_frame < 200):
|
||||
apply_steer = 0
|
||||
apply_steer_req = 0
|
||||
else:
|
||||
@@ -293,8 +295,16 @@ class CarController(object):
|
||||
if pcm_cancel_cmd:
|
||||
send_ui = True
|
||||
|
||||
# dragonpilot, lane depart warning mod
|
||||
if self.dragon_lane_departure_warning:
|
||||
dragon_left_lane_depart = left_lane_depart
|
||||
dragon_right_lane_depart = right_lane_depart
|
||||
else:
|
||||
dragon_left_lane_depart = False
|
||||
dragon_right_lane_depart = False
|
||||
|
||||
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
|
||||
can_sends.append(create_ui_command(self.packer, steer, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart))
|
||||
can_sends.append(create_ui_command(self.packer, steer, pcm_cancel_cmd, left_line, right_line, dragon_left_lane_depart, dragon_right_lane_depart))
|
||||
|
||||
if frame % 100 == 0 and ECU.DSU in self.fake_ecus and self.car_fingerprint not in TSS2_CAR:
|
||||
can_sends.append(create_fcw_command(self.packer, fcw))
|
||||
|
||||
@@ -89,7 +89,7 @@ def get_can_parser(CP):
|
||||
signals.append(("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2", 0))
|
||||
checks.append(("PCM_CRUISE_2", 33))
|
||||
|
||||
if CP.carFingerprint in NO_DSU_CAR:
|
||||
if CP.carFingerprint in NO_DSU_CAR or CP.carFingerprint == CAR.LEXUS_ISH:
|
||||
signals += [("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0)]
|
||||
|
||||
if CP.carFingerprint == CAR.PRIUS:
|
||||
@@ -185,7 +185,7 @@ class CarState(object):
|
||||
|
||||
if self.CP.carFingerprint in TSS2_CAR:
|
||||
self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']
|
||||
elif self.CP.carFingerprint in NO_DSU_CAR:
|
||||
elif self.CP.carFingerprint in NO_DSU_CAR or self.CP.carFingerprint == CAR.LEXUS_ISH:
|
||||
# cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] is zeroed to where the steering angle is at start.
|
||||
# need to apply an offset as soon as the steering angle measurements are both received
|
||||
self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
|
||||
|
||||
@@ -101,9 +101,17 @@ FINGERPRINTS = {
|
||||
# with ipas
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2,898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
{
|
||||
# Taiwan Prius 4.5
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 740: 5, 742: 8, 743: 8, 764: 8, 800: 8, 810: 2, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
#2019 LE
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
#2020 Prius Prime Limited
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2024: 8, 2026: 8, 2027: 8, 2029: 8, 2030: 8, 2031: 8
|
||||
}],
|
||||
#Corolla w/ added Pedal Support (512L and 513L)
|
||||
CAR.COROLLA: [{
|
||||
|
||||
@@ -0,0 +1,113 @@
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdbool.h>
|
||||
#include <math.h>
|
||||
#include <unistd.h>
|
||||
#include <assert.h>
|
||||
|
||||
#include <GLES3/gl3.h>
|
||||
#include <EGL/egl.h>
|
||||
#include <EGL/eglext.h>
|
||||
|
||||
#include "nanovg.h"
|
||||
#define NANOVG_GLES3_IMPLEMENTATION
|
||||
#include "nanovg_gl.h"
|
||||
#include "nanovg_gl_utils.h"
|
||||
|
||||
#include "framebuffer.h"
|
||||
#include "spinner.h"
|
||||
|
||||
// external resources linked in
|
||||
extern const unsigned char _binary_opensans_semibold_ttf_start[];
|
||||
extern const unsigned char _binary_opensans_semibold_ttf_end[];
|
||||
|
||||
extern const unsigned char _binary_img_spinner_track_png_start[];
|
||||
extern const unsigned char _binary_img_spinner_track_png_end[];
|
||||
|
||||
extern const unsigned char _binary_img_spinner_comma_png_start[];
|
||||
extern const unsigned char _binary_img_spinner_comma_png_end[];
|
||||
|
||||
int spin(int argc, char** argv) {
|
||||
int err;
|
||||
|
||||
const char* spintext = NULL;
|
||||
if (argc == 1) {
|
||||
spintext = argv[0];
|
||||
} else if (argc >= 2) {
|
||||
spintext = argv[1];
|
||||
}
|
||||
|
||||
// spinner
|
||||
int fb_w, fb_h;
|
||||
EGLDisplay display;
|
||||
EGLSurface surface;
|
||||
FramebufferState *fb = framebuffer_init("spinner", 0x00001000, false,
|
||||
&display, &surface, &fb_w, &fb_h);
|
||||
assert(fb);
|
||||
|
||||
NVGcontext *vg = nvgCreateGLES3(NVG_ANTIALIAS | NVG_STENCIL_STROKES);
|
||||
assert(vg);
|
||||
|
||||
int font = nvgCreateFontMem(vg, "Bold", (unsigned char*)_binary_opensans_semibold_ttf_start, _binary_opensans_semibold_ttf_end-_binary_opensans_semibold_ttf_start, 0);
|
||||
assert(font >= 0);
|
||||
|
||||
int spinner_img = nvgCreateImageMem(vg, 0, (unsigned char*)_binary_img_spinner_track_png_start, _binary_img_spinner_track_png_end - _binary_img_spinner_track_png_start);
|
||||
assert(spinner_img >= 0);
|
||||
int spinner_img_s = 360;
|
||||
int spinner_img_x = ((fb_w/2)-(spinner_img_s/2));
|
||||
int spinner_img_y = 260;
|
||||
int spinner_img_xc = (fb_w/2);
|
||||
int spinner_img_yc = (fb_h/2)-100;
|
||||
int spinner_comma_img = nvgCreateImageMem(vg, 0, (unsigned char*)_binary_img_spinner_comma_png_start, _binary_img_spinner_comma_png_end - _binary_img_spinner_comma_png_start);
|
||||
assert(spinner_comma_img >= 0);
|
||||
|
||||
for (int cnt = 0; ; cnt++) {
|
||||
glClearColor(0.1, 0.1, 0.1, 1.0);
|
||||
glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
|
||||
glEnable(GL_BLEND);
|
||||
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
|
||||
nvgBeginFrame(vg, fb_w, fb_h, 1.0f);
|
||||
|
||||
// background
|
||||
nvgBeginPath(vg);
|
||||
NVGpaint bg = nvgLinearGradient(vg, fb_w, 0, fb_w, fb_h,
|
||||
nvgRGBA(0, 0, 0, 175), nvgRGBA(0, 0, 0, 255));
|
||||
nvgFillPaint(vg, bg);
|
||||
nvgRect(vg, 0, 0, fb_w, fb_h);
|
||||
nvgFill(vg);
|
||||
|
||||
// spin track
|
||||
nvgSave(vg);
|
||||
nvgTranslate(vg, spinner_img_xc, spinner_img_yc);
|
||||
nvgRotate(vg, (3.75*M_PI * cnt/120.0));
|
||||
nvgTranslate(vg, -spinner_img_xc, -spinner_img_yc);
|
||||
NVGpaint spinner_imgPaint = nvgImagePattern(vg, spinner_img_x, spinner_img_y,
|
||||
spinner_img_s, spinner_img_s, 0, spinner_img, 0.6f);
|
||||
nvgBeginPath(vg);
|
||||
nvgFillPaint(vg, spinner_imgPaint);
|
||||
nvgRect(vg, spinner_img_x, spinner_img_y, spinner_img_s, spinner_img_s);
|
||||
nvgFill(vg);
|
||||
nvgRestore(vg);
|
||||
|
||||
// comma
|
||||
NVGpaint comma_imgPaint = nvgImagePattern(vg, spinner_img_x, spinner_img_y,
|
||||
spinner_img_s, spinner_img_s, 0, spinner_comma_img, 1.0f);
|
||||
nvgBeginPath(vg);
|
||||
nvgFillPaint(vg, comma_imgPaint);
|
||||
nvgRect(vg, spinner_img_x, spinner_img_y, spinner_img_s, spinner_img_s);
|
||||
nvgFill(vg);
|
||||
|
||||
// message
|
||||
if (spintext) {
|
||||
nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_TOP);
|
||||
nvgFontSize(vg, 96.0f);
|
||||
nvgText(vg, fb_w/2, (fb_h*2/3)+24, spintext, NULL);
|
||||
}
|
||||
|
||||
nvgEndFrame(vg);
|
||||
eglSwapBuffers(display, surface);
|
||||
assert(glGetError() == GL_NO_ERROR);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,14 @@
|
||||
#ifndef COMMON_SPINNER_H
|
||||
#define COMMON_SPINNER_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
int spin(int argc, char** argv);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,64 @@
|
||||
from numpy import clip
|
||||
import pickle
|
||||
import csv
|
||||
import os
|
||||
|
||||
# HOW TO
|
||||
# import this module to where you want to use it, such as from ```selfdrive.controls.lib.curvature_learner import CurvatureLearner```
|
||||
# create the object ```self.curvature_offset = CurvatureLearner(debug=False)```
|
||||
# call the update method ```self.curvature_offset.update(angle_steers - angle_offset, self.LP.d_poly)```
|
||||
# The learned curvature offsets will save and load automatically
|
||||
# If you still need help, check out how I have it implemented in the devel_curvaturefactorlearner branch
|
||||
# by Zorrobyte
|
||||
# version 4
|
||||
|
||||
class CurvatureLearner:
|
||||
def __init__(self, debug=False):
|
||||
self.offset = 0.
|
||||
self.learning_rate = 12000
|
||||
self.frame = 0
|
||||
self.debug = debug
|
||||
try:
|
||||
self.learned_offsets = pickle.load(open("/data/curvaturev4.p", "rb"))
|
||||
except (OSError, IOError):
|
||||
self.learned_offsets = {
|
||||
"center": 0.,
|
||||
"inner": 0.,
|
||||
"outer": 0.
|
||||
}
|
||||
pickle.dump(self.learned_offsets, open("/data/curvaturev4.p", "wb"))
|
||||
os.chmod("/data/curvaturev4.p", 0o777)
|
||||
|
||||
def update(self, angle_steers=0., d_poly=None, v_ego=0.):
|
||||
if angle_steers > 0.1:
|
||||
if abs(angle_steers) < 2.:
|
||||
self.learned_offsets["center"] -= d_poly[3] / self.learning_rate
|
||||
self.offset = self.learned_offsets["center"]
|
||||
elif 2. < abs(angle_steers) < 5.:
|
||||
self.learned_offsets["inner"] -= d_poly[3] / self.learning_rate
|
||||
self.offset = self.learned_offsets["inner"]
|
||||
elif abs(angle_steers) > 5.:
|
||||
self.learned_offsets["outer"] -= d_poly[3] / self.learning_rate
|
||||
self.offset = self.learned_offsets["outer"]
|
||||
elif angle_steers < -0.1:
|
||||
if abs(angle_steers) < 2.:
|
||||
self.learned_offsets["center"] += d_poly[3] / self.learning_rate
|
||||
self.offset = self.learned_offsets["center"]
|
||||
elif 2. < abs(angle_steers) < 5.:
|
||||
self.learned_offsets["inner"] += d_poly[3] / self.learning_rate
|
||||
self.offset = self.learned_offsets["inner"]
|
||||
elif abs(angle_steers) > 5.:
|
||||
self.learned_offsets["outer"] += d_poly[3] / self.learning_rate
|
||||
self.offset = self.learned_offsets["outer"]
|
||||
|
||||
self.offset = clip(self.offset, -0.3, 0.3)
|
||||
self.frame += 1
|
||||
|
||||
if self.frame == 12000: # every 2 mins
|
||||
pickle.dump(self.learned_offsets, open("/data/curvaturev4.p", "wb"))
|
||||
self.frame = 0
|
||||
if self.debug:
|
||||
with open('/data/curvdebug.csv', 'a') as csv_file:
|
||||
csv_file_writer = csv.writer(csv_file, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
|
||||
csv_file_writer.writerow([self.learned_offsets, v_ego])
|
||||
return self.offset
|
||||
@@ -100,6 +100,7 @@ class DriverStatus():
|
||||
self.awareness_time = float(params.get("DragonSteeringMonitorTimer"))
|
||||
self.awareness_time = 86400 if self.awareness_time <= 0. else self.awareness_time * 60.
|
||||
self.dragon_enable_driver_safety_check = False if params.get("DragonEnableDriverSafetyCheck") == "0" else True
|
||||
self.dragon_enable_driver_monitoring = False if params.get("DragonEnableDriverMonitoring") == "0" else True
|
||||
|
||||
self._set_timers(active_monitoring=True)
|
||||
|
||||
@@ -162,6 +163,9 @@ class DriverStatus():
|
||||
self.pose.roll, self.pose.pitch, self.pose.yaw = head_orientation_from_descriptor(driver_monitoring.faceOrientation, driver_monitoring.facePosition, cal_rpy)
|
||||
self.blink.left_blink = driver_monitoring.leftBlinkProb * (driver_monitoring.leftEyeProb>_EYE_THRESHOLD)
|
||||
self.blink.right_blink = driver_monitoring.rightBlinkProb * (driver_monitoring.rightEyeProb>_EYE_THRESHOLD)
|
||||
# we simply set to rhd to avoid driver monitoring
|
||||
if not self.dragon_enable_driver_monitoring:
|
||||
self.is_rhd_region = True
|
||||
self.face_detected = driver_monitoring.faceProb > _FACE_THRESHOLD and not self.is_rhd_region
|
||||
|
||||
self.driver_distracted = self._is_driver_distracted(self.pose, self.blink)>0
|
||||
|
||||
@@ -9,6 +9,8 @@ from selfdrive.controls.lib.lateral_mpc import libmpc_py
|
||||
from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT
|
||||
from selfdrive.controls.lib.lane_planner import LanePlanner
|
||||
import selfdrive.messaging as messaging
|
||||
from selfdrive.controls.lib.curvature_learner import CurvatureLearner
|
||||
|
||||
|
||||
LOG_MPC = os.environ.get('LOG_MPC', False)
|
||||
|
||||
@@ -28,6 +30,7 @@ class PathPlanner(object):
|
||||
self.setup_mpc(CP.steerRateCost)
|
||||
self.solution_invalid_cnt = 0
|
||||
self.path_offset_i = 0.0
|
||||
self.curvature_offset = CurvatureLearner(debug=False)
|
||||
|
||||
def setup_mpc(self, steer_rate_cost):
|
||||
self.libmpc = libmpc_py.libmpc
|
||||
@@ -57,7 +60,13 @@ class PathPlanner(object):
|
||||
# Run MPC
|
||||
self.angle_steers_des_prev = self.angle_steers_des_mpc
|
||||
VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio)
|
||||
curvature_factor = VM.curvature_factor(v_ego)
|
||||
#curvature_factor = VM.curvature_factor(v_ego)
|
||||
if active:
|
||||
curvfac = self.curvature_offset.update(angle_steers - angle_offset, self.LP.d_poly, v_ego)
|
||||
else:
|
||||
curvfac = 0.
|
||||
|
||||
curvature_factor = VM.curvature_factor(v_ego) + curvfac
|
||||
|
||||
# TODO: Check for active, override, and saturation
|
||||
# if active:
|
||||
|
||||
@@ -22,6 +22,9 @@ autonavi_main = "com.autonavi.amapauto.MainMapActivity"
|
||||
mixplorer = "com.mixplorer"
|
||||
mixplorer_main = "com.mixplorer.activities.BrowseActivity"
|
||||
|
||||
gpsservice = "cn.dragonpilot.gpsservice"
|
||||
gpsservice_main = "cn.dragonpilot.gpsservice.MainService"
|
||||
|
||||
def main(gctx=None):
|
||||
|
||||
dragon_enable_tomtom = True if params.get('DragonEnableTomTom') == "1" else False
|
||||
@@ -29,7 +32,8 @@ def main(gctx=None):
|
||||
dragon_enable_mixplorer = True if params.get('DragonEnableMixplorer') == "1" else False
|
||||
dragon_boot_tomtom = True if params.get("DragonBootTomTom") == "1" else False
|
||||
dragon_boot_autonavi = True if params.get("DragonBootAutonavi") == "1" else False
|
||||
dragon_greypanda_mode = True if params.get('DragonGreyPandaMode') == "1" else False
|
||||
dragon_greypanda_mode = True if params.get("DragonGreyPandaMode") == "1" else False
|
||||
dragon_grepanda_mode_started = False
|
||||
tomtom_is_running = False
|
||||
autonavi_is_running = False
|
||||
mixplorer_is_running = False
|
||||
@@ -40,46 +44,41 @@ def main(gctx=None):
|
||||
frame = 0
|
||||
start_delay = None
|
||||
stop_delay = None
|
||||
high_accuracy_mode_enabled = False
|
||||
|
||||
put_nonblocking('DragonRunTomTom', '0')
|
||||
put_nonblocking('DragonRunAutonavi', '0')
|
||||
put_nonblocking('DragonRunMixplorer', '0')
|
||||
|
||||
# we want to disable all app when boot
|
||||
system("pm disable %s ; pm disable %s ; pm disable %s" % (tomtom, autonavi, mixplorer))
|
||||
system("pm disable %s" % tomtom)
|
||||
system("pm disable %s" % autonavi)
|
||||
system("pm disable %s" % mixplorer)
|
||||
system("pm disable %s" % gpsservice)
|
||||
|
||||
thermal_sock = messaging.sub_sock(service_list['thermal'].port)
|
||||
|
||||
while dragon_enable_tomtom or dragon_enable_autonavi or dragon_enable_mixplorer:
|
||||
if (dragon_enable_tomtom or dragon_enable_autonavi) and not high_accuracy_mode_enabled:
|
||||
if dragon_greypanda_mode:
|
||||
system("settings put secure location_providers_allowed -gps,network,wifi")
|
||||
system("settings put secure location_providers_allowed +gps")
|
||||
else:
|
||||
system("settings put secure location_providers_allowed +gps,network,wifi")
|
||||
high_accuracy_mode_enabled = True
|
||||
while dragon_enable_tomtom or dragon_enable_autonavi or dragon_enable_mixplorer or dragon_greypanda_mode:
|
||||
|
||||
# allow user to manually start/stop app
|
||||
if dragon_enable_tomtom:
|
||||
status = params.get('DragonRunTomTom')
|
||||
if not status == "0":
|
||||
tomtom_is_running = exec_app(status, tomtom, tomtom_main)
|
||||
put_nonblocking('DragonRunTomTom', '0')
|
||||
params.put('DragonRunTomTom', '0')
|
||||
manual_tomtom = status != "0"
|
||||
|
||||
if dragon_enable_autonavi:
|
||||
status = params.get('DragonRunAutonavi')
|
||||
if not status == "0":
|
||||
autonavi_is_running = exec_app(status, autonavi, autonavi_main)
|
||||
put_nonblocking('DragonRunAutonavi', '0')
|
||||
params.put('DragonRunAutonavi', '0')
|
||||
manual_autonavi = status != "0"
|
||||
|
||||
if dragon_enable_mixplorer:
|
||||
status = params.get('DragonRunMixplorer')
|
||||
if not status == "0":
|
||||
mixplorer_is_running = exec_app(status, mixplorer, mixplorer_main)
|
||||
put_nonblocking('DragonRunMixplorer', '0')
|
||||
params.put('DragonRunMixplorer', '0')
|
||||
|
||||
# if manual control is set, we do not allow any of the auto actions
|
||||
auto_tomtom = not manual_tomtom and dragon_enable_tomtom and dragon_boot_tomtom
|
||||
@@ -89,6 +88,11 @@ def main(gctx=None):
|
||||
started = msg.thermal.started
|
||||
# car on
|
||||
if started:
|
||||
if dragon_greypanda_mode and not dragon_grepanda_mode_started:
|
||||
dragon_grepanda_mode_started = True
|
||||
system("pm enable %s" % gpsservice)
|
||||
system("am startservice %s/%s" % (gpsservice, gpsservice_main))
|
||||
|
||||
stop_delay = None
|
||||
if start_delay is None:
|
||||
start_delay = frame + 5
|
||||
@@ -122,6 +126,10 @@ def main(gctx=None):
|
||||
|
||||
# car off
|
||||
else:
|
||||
if dragon_greypanda_mode and dragon_grepanda_mode_started:
|
||||
dragon_grepanda_mode_started = False
|
||||
system("pm disable %s" % gpsservice)
|
||||
|
||||
start_delay = None
|
||||
if stop_delay is None:
|
||||
stop_delay = frame + 30
|
||||
@@ -142,7 +150,8 @@ def main(gctx=None):
|
||||
|
||||
def exec_app(status, app, app_main):
|
||||
if status == "1":
|
||||
system("pm enable %s && am start -n %s/%s" % (app, app, app_main))
|
||||
system("pm enable %s" % app)
|
||||
system("am start -n %s/%s" % (app, app_main))
|
||||
return True
|
||||
if status == "-1":
|
||||
system("pm disable %s" % app)
|
||||
|
||||
@@ -42,6 +42,15 @@ default_conf = {
|
||||
'DragonGreyPandaMode': '0',
|
||||
'DragonDrivingUI': '1',
|
||||
'DragonDisplaySteeringLimitAlert': '1',
|
||||
'DragonChargingCtrl': '0',
|
||||
'DragonCharging': 70,
|
||||
'DragonDisCharging': 60,
|
||||
'DragonToyotaLaneDepartureWarning': '1',
|
||||
'DragonUILane': '1',
|
||||
'DragonUILead': '1',
|
||||
'DragonUIPath': '1',
|
||||
'DragonUIBlinker': '0',
|
||||
'DragonEnableDriverMonitoring': '1',
|
||||
}
|
||||
|
||||
deprecated_conf = {
|
||||
|
||||
@@ -126,7 +126,7 @@ managed_processes = {
|
||||
daemon_processes = {
|
||||
"athenad": "selfdrive.athena.athenad",
|
||||
}
|
||||
android_packages = ("com.autonavi.amapauto", "com.mixplorer", "com.tomtom.speedcams.android.map", "ai.comma.plus.offroad", "ai.comma.plus.frame")
|
||||
android_packages = ("cn.dragonpilot.gpsservice", "com.autonavi.amapauto", "com.mixplorer", "com.tomtom.speedcams.android.map", "ai.comma.plus.offroad", "ai.comma.plus.frame")
|
||||
|
||||
running = {}
|
||||
def get_running():
|
||||
@@ -603,14 +603,6 @@ def main():
|
||||
spinner_proc = subprocess.Popen(["./spinner", "http://dragonpilot.cn"],
|
||||
cwd=os.path.join(BASEDIR, "selfdrive", "ui", "spinner"),
|
||||
close_fds=True)
|
||||
|
||||
if params.get("DragonEnableLogger") == "0":
|
||||
del managed_processes['loggerd']
|
||||
del managed_processes['tombstoned']
|
||||
|
||||
if params.get("DragonEnableUploader") == "0":
|
||||
del managed_processes['uploader']
|
||||
|
||||
try:
|
||||
manager_update()
|
||||
manager_init()
|
||||
@@ -622,6 +614,13 @@ def main():
|
||||
if os.getenv("PREPAREONLY") is not None:
|
||||
return
|
||||
|
||||
if params.get("DragonEnableLogger") == "0":
|
||||
del managed_processes['loggerd']
|
||||
del managed_processes['tombstoned']
|
||||
|
||||
if params.get("DragonEnableUploader") == "0":
|
||||
del managed_processes['uploader']
|
||||
|
||||
# SystemExit on sigterm
|
||||
signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(1))
|
||||
|
||||
|
||||
+36
-2
@@ -152,8 +152,16 @@ def thermald_thread():
|
||||
# Make sure charging is enabled
|
||||
charging_disabled = False
|
||||
os.system('echo "1" > /sys/class/power_supply/battery/charging_enabled')
|
||||
|
||||
# dragonpilot
|
||||
ts_last_ip = 0.
|
||||
ts_last_update_vars = 0.
|
||||
ts_last_charging_ctrl = 0.
|
||||
|
||||
ip_addr = '255.255.255.255'
|
||||
dragon_charging_ctrl = True if params.get('DragonChargingCtrl') == "1" else False
|
||||
dragon_charging_max = int(params.get('DragonCharging'))
|
||||
dragon_discharging_min = int(params.get('DragonDisCharging'))
|
||||
|
||||
while 1:
|
||||
health = messaging.recv_sock(health_sock, wait=True)
|
||||
@@ -181,7 +189,9 @@ def thermald_thread():
|
||||
msg.thermal.batteryVoltage = int(f.read())
|
||||
with open("/sys/class/power_supply/usb/present") as f:
|
||||
msg.thermal.usbOnline = bool(int(f.read()))
|
||||
# update ip every 5 seconds
|
||||
|
||||
# dragonpilot ip Mod
|
||||
# update ip every 10 seconds
|
||||
ts = sec_since_boot()
|
||||
if ts - ts_last_ip > 10.:
|
||||
try:
|
||||
@@ -195,7 +205,7 @@ def thermald_thread():
|
||||
result = subprocess.check_output(["ifconfig", "wlan0"])
|
||||
ip_addr = re.findall(r"inet addr:((\d+\.){3}\d+)", result)[0][0]
|
||||
else:
|
||||
ip_addr = '<N/A>'
|
||||
ip_addr = 'N/A'
|
||||
ts_last_ip = ts
|
||||
msg.thermal.ipAddr = ip_addr
|
||||
|
||||
@@ -289,6 +299,30 @@ def thermald_thread():
|
||||
thermal_sock.send(msg.to_bytes())
|
||||
print(msg)
|
||||
|
||||
# dragonpilot
|
||||
ts = sec_since_boot()
|
||||
# update variable status every 10 secs
|
||||
if ts - ts_last_update_vars > 10.:
|
||||
dragon_charging_ctrl = True if params.get('DragonChargingCtrl') == "1" else False
|
||||
dragon_charging_max = int(params.get('DragonCharging'))
|
||||
dragon_discharging_min = int(params.get('DragonDisCharging'))
|
||||
ts_last_update_vars = ts
|
||||
|
||||
# we only update charging status once every min
|
||||
if ts - ts_last_charging_ctrl > 60.:
|
||||
if dragon_charging_ctrl:
|
||||
if msg.thermal.batteryPercent >= dragon_charging_max and not charging_disabled:
|
||||
os.system('echo "0" > /sys/class/power_supply/battery/charging_enabled')
|
||||
charging_disabled = True
|
||||
if msg.thermal.batteryPercent <= dragon_discharging_min and charging_disabled:
|
||||
os.system('echo "1" > /sys/class/power_supply/battery/charging_enabled')
|
||||
charging_disabled = False
|
||||
else:
|
||||
if charging_disabled:
|
||||
os.system('echo "1" > /sys/class/power_supply/battery/charging_enabled')
|
||||
charging_disabled = False
|
||||
ts_last_charging_ctrl = ts
|
||||
|
||||
# report to server once per minute
|
||||
if (count % int(60. / DT_TRML)) == 0:
|
||||
cloudlog.event("STATUS_PACKET",
|
||||
|
||||
@@ -16,7 +16,7 @@ CXXFLAGS = -std=c++11 -fPIC -O2 $(WARN_FLAGS)
|
||||
ZMQ_LIBS = -l:libczmq.a -l:libzmq.a -lgnustl_shared
|
||||
|
||||
CEREAL_LIBS = -l:libcapn.a
|
||||
CEREAL_OBJS = ../../cereal/gen/c/log.capnp.o
|
||||
CEREAL_OBJS = ../../cereal/gen/c/log.capnp.o ../../cereal/gen/c/car.capnp.o
|
||||
|
||||
NANOVG_FLAGS = -I$(PHONELIBS)/nanovg
|
||||
JSON_FLAGS = -I$(PHONELIBS)/json/src
|
||||
|
||||
+113
-42
@@ -168,6 +168,11 @@ typedef struct UIScene {
|
||||
float angleSteersDes;
|
||||
float angleSteers;
|
||||
|
||||
// for blinker, from kegman
|
||||
bool leftBlinker;
|
||||
bool rightBlinker;
|
||||
int blinker_blinkingrate;
|
||||
|
||||
} UIScene;
|
||||
|
||||
typedef struct {
|
||||
@@ -215,6 +220,7 @@ typedef struct UIState {
|
||||
void *livempc_sock_raw;
|
||||
void *plus_sock_raw;
|
||||
void *map_data_sock_raw;
|
||||
void *carstate_sock_raw;
|
||||
|
||||
void *uilayout_sock_raw;
|
||||
|
||||
@@ -304,6 +310,10 @@ typedef struct UIState {
|
||||
int dragon_enable_dashcam_timeout;
|
||||
int dragon_ui_volume_boost_timeout;
|
||||
int dragon_driving_ui_timeout;
|
||||
int dragon_ui_lane_timeout;
|
||||
int dragon_ui_lead_timeout;
|
||||
int dragon_ui_path_timeout;
|
||||
int dragon_ui_blinker_timeout;
|
||||
|
||||
bool dragon_ui_speed;
|
||||
bool dragon_ui_event;
|
||||
@@ -314,6 +324,10 @@ typedef struct UIState {
|
||||
bool dragon_enable_dashcam;
|
||||
float dragon_ui_volume_boost;
|
||||
bool dragon_driving_ui;
|
||||
bool dragon_ui_lane;
|
||||
bool dragon_ui_lead;
|
||||
bool dragon_ui_path;
|
||||
bool dragon_ui_blinker;
|
||||
|
||||
} UIState;
|
||||
|
||||
@@ -534,6 +548,7 @@ static void ui_init(UIState *s) {
|
||||
s->radarstate_sock_raw = sub_sock(s->ctx, "tcp://127.0.0.1:8012");
|
||||
s->livempc_sock_raw = sub_sock(s->ctx, "tcp://127.0.0.1:8035");
|
||||
s->plus_sock_raw = sub_sock(s->ctx, "tcp://127.0.0.1:8037");
|
||||
s->carstate_sock_raw = sub_sock(s->ctx, "tcp://127.0.0.1:8021");
|
||||
|
||||
#ifdef SHOW_SPEEDLIMIT
|
||||
s->map_data_sock_raw = sub_sock(s->ctx, "tcp://127.0.0.1:8065");
|
||||
@@ -709,6 +724,10 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs,
|
||||
read_param_bool(&s->dragon_enable_dashcam, "DragonEnableDashcam");
|
||||
read_param_float(&s->dragon_ui_volume_boost, "DragonUIVolumeBoost");
|
||||
read_param_bool(&s->dragon_driving_ui, "DragonDrivingUI");
|
||||
read_param_bool(&s->dragon_ui_lane, "DragonUILane");
|
||||
read_param_bool(&s->dragon_ui_lead, "DragonUILead");
|
||||
read_param_bool(&s->dragon_ui_path, "DragonUIPath");
|
||||
read_param_bool(&s->dragon_ui_blinker, "DragonUIBlinker");
|
||||
|
||||
|
||||
// Set offsets so params don't get read at the same time
|
||||
@@ -726,6 +745,10 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs,
|
||||
s->dragon_enable_dashcam_timeout = 100;
|
||||
s->dragon_ui_volume_boost_timeout = 100;
|
||||
s->dragon_driving_ui_timeout = 100;
|
||||
s->dragon_ui_lane_timeout = 100;
|
||||
s->dragon_ui_lead_timeout = 100;
|
||||
s->dragon_ui_path_timeout = 100;
|
||||
s->dragon_ui_blinker_timeout = 100;
|
||||
}
|
||||
|
||||
// Projects a point in car to space to the corresponding point in full frame
|
||||
@@ -1045,27 +1068,30 @@ static void ui_draw_vision_lanes(UIState *s) {
|
||||
update_all_lane_lines_data(s, scene->model.right_lane, pvd + MODEL_LANE_PATH_CNT);
|
||||
s->model_changed = false;
|
||||
}
|
||||
// Draw left lane edge
|
||||
ui_draw_lane(
|
||||
if (s->dragon_ui_lane) {
|
||||
// Draw left lane edge
|
||||
ui_draw_lane(
|
||||
s, &scene->model.left_lane,
|
||||
pvd,
|
||||
nvgRGBAf(1.0, 1.0, 1.0, scene->model.left_lane.prob));
|
||||
|
||||
// Draw right lane edge
|
||||
ui_draw_lane(
|
||||
// Draw right lane edge
|
||||
ui_draw_lane(
|
||||
s, &scene->model.right_lane,
|
||||
pvd + MODEL_LANE_PATH_CNT,
|
||||
nvgRGBAf(1.0, 1.0, 1.0, scene->model.right_lane.prob));
|
||||
|
||||
}
|
||||
if(s->livempc_or_radarstate_changed) {
|
||||
update_all_track_data(s);
|
||||
s->livempc_or_radarstate_changed = false;
|
||||
}
|
||||
// Draw vision path
|
||||
ui_draw_track(s, false, &s->track_vertices[0]);
|
||||
if (scene->engaged) {
|
||||
// Draw MPC path when engaged
|
||||
ui_draw_track(s, true, &s->track_vertices[1]);
|
||||
if (s->dragon_ui_path) {
|
||||
// Draw vision path
|
||||
ui_draw_track(s, false, &s->track_vertices[0]);
|
||||
if (scene->engaged) {
|
||||
// Draw MPC path when engaged
|
||||
ui_draw_track(s, true, &s->track_vertices[1]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1081,7 +1107,7 @@ static void ui_draw_world(UIState *s) {
|
||||
ui_draw_vision_lanes(s);
|
||||
}
|
||||
|
||||
if (scene->lead_status) {
|
||||
if (s->dragon_ui_lead && scene->lead_status) {
|
||||
// Draw lead car indicator
|
||||
float fillAlpha = 0;
|
||||
float speedBuff = 10.;
|
||||
@@ -1284,28 +1310,57 @@ static void ui_draw_vision_speed(UIState *s) {
|
||||
const int viz_speed_x = ui_viz_rx+((ui_viz_rw/2)-(viz_speed_w/2));
|
||||
char speed_str[32];
|
||||
|
||||
nvgBeginPath(s->vg);
|
||||
nvgRect(s->vg, viz_speed_x, box_y, viz_speed_w, header_h);
|
||||
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
|
||||
// blinker, from kegman
|
||||
if (s->dragon_ui_blinker) {
|
||||
if(s->scene.leftBlinker) {
|
||||
nvgBeginPath(s->vg);
|
||||
nvgMoveTo(s->vg, viz_speed_x, box_y + header_h/4);
|
||||
nvgLineTo(s->vg, viz_speed_x - viz_speed_w/2, box_y + header_h/4 + header_h/4);
|
||||
nvgLineTo(s->vg, viz_speed_x, box_y + header_h/2 + header_h/4);
|
||||
nvgClosePath(s->vg);
|
||||
nvgFillColor(s->vg, nvgRGBA(23,134,68,s->scene.blinker_blinkingrate>=50?210:60));
|
||||
nvgFill(s->vg);
|
||||
}
|
||||
|
||||
if (s->is_metric) {
|
||||
snprintf(speed_str, sizeof(speed_str), "%d", (int)(speed * 3.6 + 0.5));
|
||||
} else {
|
||||
snprintf(speed_str, sizeof(speed_str), "%d", (int)(speed * 2.2369363 + 0.5));
|
||||
if(s->scene.rightBlinker) {
|
||||
nvgBeginPath(s->vg);
|
||||
nvgMoveTo(s->vg, viz_speed_x+viz_speed_w, box_y + header_h/4);
|
||||
nvgLineTo(s->vg, viz_speed_x+viz_speed_w + viz_speed_w/2, box_y + header_h/4 + header_h/4);
|
||||
nvgLineTo(s->vg, viz_speed_x+viz_speed_w, box_y + header_h/2 + header_h/4);
|
||||
nvgClosePath(s->vg);
|
||||
nvgFillColor(s->vg, nvgRGBA(23,134,68,s->scene.blinker_blinkingrate>=50?210:60));
|
||||
nvgFill(s->vg);
|
||||
}
|
||||
|
||||
if(s->scene.leftBlinker || s->scene.rightBlinker) {
|
||||
s->scene.blinker_blinkingrate -= 3;
|
||||
if(s->scene.blinker_blinkingrate<0) s->scene.blinker_blinkingrate = 120;
|
||||
}
|
||||
}
|
||||
nvgFontFace(s->vg, "sans-bold");
|
||||
nvgFontSize(s->vg, 96*2.5);
|
||||
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
|
||||
nvgText(s->vg, viz_speed_x+viz_speed_w/2, 240, speed_str, NULL);
|
||||
if (s->dragon_ui_speed) {
|
||||
nvgBeginPath(s->vg);
|
||||
nvgRect(s->vg, viz_speed_x, box_y, viz_speed_w, header_h);
|
||||
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
|
||||
|
||||
nvgFontFace(s->vg, "sans-regular");
|
||||
nvgFontSize(s->vg, 36*2.5);
|
||||
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 200));
|
||||
if (s->is_metric) {
|
||||
snprintf(speed_str, sizeof(speed_str), "%d", (int)(speed * 3.6 + 0.5));
|
||||
} else {
|
||||
snprintf(speed_str, sizeof(speed_str), "%d", (int)(speed * 2.2369363 + 0.5));
|
||||
}
|
||||
nvgFontFace(s->vg, "sans-bold");
|
||||
nvgFontSize(s->vg, 96*2.5);
|
||||
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
|
||||
nvgText(s->vg, viz_speed_x+viz_speed_w/2, 240, speed_str, NULL);
|
||||
|
||||
if (s->is_metric) {
|
||||
nvgText(s->vg, viz_speed_x+viz_speed_w/2, 320, "kph", NULL);
|
||||
} else {
|
||||
nvgText(s->vg, viz_speed_x+viz_speed_w/2, 320, "mph", NULL);
|
||||
nvgFontFace(s->vg, "sans-regular");
|
||||
nvgFontSize(s->vg, 36*2.5);
|
||||
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 200));
|
||||
|
||||
if (s->is_metric) {
|
||||
nvgText(s->vg, viz_speed_x+viz_speed_w/2, 320, "kph", NULL);
|
||||
} else {
|
||||
nvgText(s->vg, viz_speed_x+viz_speed_w/2, 320, "mph", NULL);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1322,7 +1377,7 @@ static void ui_draw_vision_event(UIState *s) {
|
||||
const int img_turn_size = 160*1.5;
|
||||
const int img_turn_x = viz_event_x-(img_turn_size/4);
|
||||
const int img_turn_y = viz_event_y+bdr_s-25;
|
||||
float img_turn_alpha = 1.0f;
|
||||
float img_turn_alpha = 0.7f;
|
||||
nvgBeginPath(s->vg);
|
||||
NVGpaint imgPaint = nvgImagePattern(s->vg, img_turn_x, img_turn_y,
|
||||
img_turn_size, img_turn_size, 0, s->img_turn, img_turn_alpha);
|
||||
@@ -1345,14 +1400,14 @@ static void ui_draw_vision_event(UIState *s) {
|
||||
nvgBeginPath(s->vg);
|
||||
nvgCircle(s->vg, bg_wheel_x, (bg_wheel_y + (bdr_s*1.5)), bg_wheel_size);
|
||||
if (is_engaged) {
|
||||
nvgFillColor(s->vg, nvgRGBA(23, 134, 68, 255));
|
||||
nvgFillColor(s->vg, nvgRGBA(23, 134, 68, 180));
|
||||
} else if (is_warning) {
|
||||
nvgFillColor(s->vg, nvgRGBA(218, 111, 37, 255));
|
||||
nvgFillColor(s->vg, nvgRGBA(218, 111, 37, 180));
|
||||
} else if (is_engageable) {
|
||||
nvgFillColor(s->vg, nvgRGBA(23, 51, 73, 255));
|
||||
nvgFillColor(s->vg, nvgRGBA(23, 51, 73, 180));
|
||||
}
|
||||
nvgFill(s->vg);
|
||||
img_wheel_alpha = 1.0f;
|
||||
img_wheel_alpha = 0.7f;
|
||||
}
|
||||
nvgBeginPath(s->vg);
|
||||
NVGpaint imgPaint = nvgImagePattern(s->vg, img_wheel_x, img_wheel_y,
|
||||
@@ -1436,9 +1491,7 @@ static void ui_draw_vision_header(UIState *s) {
|
||||
#ifdef SHOW_SPEEDLIMIT
|
||||
ui_draw_vision_speedlimit(s);
|
||||
#endif
|
||||
if (s->dragon_ui_speed) {
|
||||
ui_draw_vision_speed(s);
|
||||
}
|
||||
ui_draw_vision_speed(s);
|
||||
if (s->dragon_ui_event) {
|
||||
ui_draw_vision_event(s);
|
||||
}
|
||||
@@ -1525,12 +1578,12 @@ static void ui_draw_infobar(UIState *s) {
|
||||
|
||||
nvgBeginPath(s->vg);
|
||||
nvgRoundedRect(s->vg, rect_x, rect_y, rect_w, rect_h, 15);
|
||||
nvgFillColor(s->vg, nvgRGBA(0, 0, 0, 100));
|
||||
nvgFillColor(s->vg, nvgRGBA(0, 0, 0, 180));
|
||||
nvgFill(s->vg);
|
||||
|
||||
nvgFontSize(s->vg, hasSidebar? 40:50);
|
||||
nvgFontFace(s->vg, "courbd");
|
||||
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 175));
|
||||
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 180));
|
||||
nvgTextAlign(s->vg, NVG_ALIGN_CENTER);
|
||||
nvgText(s->vg, text_x, text_y, infobar, NULL);
|
||||
}
|
||||
@@ -2129,6 +2182,15 @@ void handle_message(UIState *s, void *which) {
|
||||
struct cereal_LiveMapData datad;
|
||||
cereal_read_LiveMapData(&datad, eventd.liveMapData);
|
||||
s->scene.map_valid = datad.mapValid;
|
||||
} else if (eventd.which == cereal_Event_carState) {
|
||||
struct cereal_CarState datad;
|
||||
cereal_read_CarState(&datad, eventd.carState);
|
||||
|
||||
if(s->scene.leftBlinker!=datad.leftBlinker || s->scene.rightBlinker!=datad.rightBlinker) {
|
||||
s->scene.blinker_blinkingrate = 100;
|
||||
}
|
||||
s->scene.leftBlinker = datad.leftBlinker;
|
||||
s->scene.rightBlinker = datad.rightBlinker;
|
||||
}
|
||||
capn_free(&ctx);
|
||||
zmq_msg_close(&msg);
|
||||
@@ -2287,12 +2349,16 @@ static void ui_update(UIState *s) {
|
||||
polls[6].socket = s->uilayout_sock_raw;
|
||||
polls[6].events = ZMQ_POLLIN;
|
||||
|
||||
#ifdef SHOW_SPEEDLIMIT
|
||||
//#ifdef SHOW_SPEEDLIMIT
|
||||
// plus_sock_num++;
|
||||
// num_polls++;
|
||||
// polls[7].socket = s->map_data_sock_raw;
|
||||
// polls[7].events = ZMQ_POLLIN;
|
||||
//#endif
|
||||
plus_sock_num++;
|
||||
num_polls++;
|
||||
polls[7].socket = s->map_data_sock_raw;
|
||||
polls[7].socket = s->carstate_sock_raw;
|
||||
polls[7].events = ZMQ_POLLIN;
|
||||
#endif
|
||||
|
||||
polls[plus_sock_num].socket = s->plus_sock_raw; // plus_sock should be last
|
||||
polls[plus_sock_num].events = ZMQ_POLLIN;
|
||||
@@ -2308,6 +2374,7 @@ static void ui_update(UIState *s) {
|
||||
|
||||
if (polls[0].revents || polls[1].revents || polls[2].revents ||
|
||||
polls[3].revents || polls[4].revents || polls[6].revents ||
|
||||
polls[7].revents ||
|
||||
polls[plus_sock_num].revents) {
|
||||
// awake on any (old) activity
|
||||
set_awake(s, true);
|
||||
@@ -2684,6 +2751,10 @@ int main(int argc, char* argv[]) {
|
||||
read_param_bool_timeout(&s->dragon_enable_dashcam, "DragonEnableDashcam", &s->dragon_enable_dashcam_timeout);
|
||||
read_param_float_timeout(&s->dragon_ui_volume_boost, "DragonUIVolumeBoost", &s->dragon_ui_volume_boost_timeout);
|
||||
read_param_bool_timeout(&s->dragon_driving_ui, "DragonDrivingUI", &s->dragon_driving_ui_timeout);
|
||||
read_param_bool_timeout(&s->dragon_ui_lane, "DragonUILane", &s->dragon_ui_lane_timeout);
|
||||
read_param_bool_timeout(&s->dragon_ui_lead, "DragonUILead", &s->dragon_ui_lead_timeout);
|
||||
read_param_bool_timeout(&s->dragon_ui_path, "DragonUIPath", &s->dragon_ui_path_timeout);
|
||||
read_param_bool_timeout(&s->dragon_ui_blinker, "DragonUIBlinker", &s->dragon_ui_blinker_timeout);
|
||||
|
||||
pthread_mutex_unlock(&s->lock);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user