diff --git a/cereal/log.capnp b/cereal/log.capnp
index bc2d5de6f236bb..c719a83e8649cf 100644
--- a/cereal/log.capnp
+++ b/cereal/log.capnp
@@ -2066,6 +2066,10 @@ struct Sentinel {
type @0 :SentinelType;
}
+struct ModelLongButton {
+ enabled @0 :Bool;
+}
+
struct Event {
# in nanoseconds?
logMonoTime @0 :UInt64;
@@ -2150,6 +2154,7 @@ struct Event {
wideEncodeIdx @77 :EncodeIndex;
dragonConf @78 :DragonConf;
liveTrafficData @79:LiveTrafficData;
+ modelLongButton @80 :ModelLongButton;
}
}
diff --git a/cereal/service_list.yaml b/cereal/service_list.yaml
index c0af1611f48d1a..97046ae5da9191 100644
--- a/cereal/service_list.yaml
+++ b/cereal/service_list.yaml
@@ -83,6 +83,8 @@ modelV2: [8077, true, 20., 20]
dragonConf: [8088, false, 2.]
+modelLongButton: [8084, false, 0.]
+
testModel: [8040, false, 0.]
testLiveLocation: [8045, false, 0.]
testJoystick: [8056, false, 0.]
diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py
index fee2399e8b2875..c1efc80dd6035c 100755
--- a/selfdrive/controls/lib/planner.py
+++ b/selfdrive/controls/lib/planner.py
@@ -17,6 +17,7 @@
from selfdrive.controls.lib.fcw import FCWChecker
from selfdrive.controls.lib.long_mpc import LongitudinalMpc
from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
+from selfdrive.controls.lib.long_mpc_model import LongitudinalMpcModel
from common.op_params import opParams
op_params = opParams()
osm = op_params.get('osm')
@@ -117,6 +118,7 @@ def __init__(self, CP):
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
+ self.mpc_model = LongitudinalMpcModel()
self.v_acc_start = 0.0
self.a_acc_start = 0.0
@@ -126,6 +128,8 @@ def __init__(self, CP):
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
+ self.v_model = 0.0
+ self.a_model = 0.0
self.osm = True
self.longitudinalPlanSource = 'cruise'
@@ -144,7 +148,8 @@ def __init__(self, CP):
self.v_model = 0.0
self.a_model = 0.0
- def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2, steeringAngle):
+ def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2, steeringAngle, model_enabled):
+ #possible_futures = [self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint]
center_x = -2.5 # Wheel base 2.5m
lead1_check = True
lead2_check = True
@@ -166,6 +171,9 @@ def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2, steeringAn
solutions['mpc1'] = self.mpc1.v_mpc
if self.mpc2.prev_lead_status and lead2_check:
solutions['mpc2'] = self.mpc2.v_mpc
+ if self.mpc_model.valid and model_enabled:
+ solutions['model'] = self.mpc_model.v_mpc
+ solutions['cruise'] = self.v_cruise
slowest = min(solutions, key=solutions.get)
@@ -180,6 +188,9 @@ def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2, steeringAn
elif slowest == 'cruise':
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
+ elif slowest == 'model':
+ self.v_acc = self.mpc_model.v_mpc
+ self.a_acc = self.mpc_model.a_mpc
# dp - slow on curve from 0.7.6.1
elif self.dp_slow_on_curve and slowest == 'model':
self.v_acc = self.v_model
@@ -187,9 +198,29 @@ def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2, steeringAn
self.v_acc_future = v_cruise_setpoint
if lead1_check:
- self.v_acc_future = min([self.mpc1.v_mpc_future, self.v_acc_future])
+ self.v_acc_future = min([self.mpc1.v_mpc_future, self.v_acc_future, self.mpc_model.v_mpc_future])
if lead2_check:
- self.v_acc_future = min([self.mpc2.v_mpc_future, self.v_acc_future])
+ self.v_acc_future = min([self.mpc2.v_mpc_future, self.v_acc_future, self.mpc_model.v_mpc_future ])
+
+ def parse_modelV2_data(self, sm):
+ modelV2 = sm['modelV2']
+ distances, speeds, accelerations = [], [], []
+ if not sm.updated['modelV2'] or len(modelV2.position.x) == 0:
+ return distances, speeds, accelerations
+
+ model_t = modelV2.position.t
+ mpc_times = list(range(10))
+
+ model_t_idx = [sorted(range(len(model_t)), key=[abs(idx - t) for t in model_t].__getitem__)[0] for idx in mpc_times] # matches 0 to 9 interval to idx from t
+
+ for t in model_t_idx: # everything is derived from x position since velocity is outputting weird values
+ speeds.append(modelV2.velocity.x[t])
+ distances.append(modelV2.position.x[t])
+ if model_t_idx.index(t) > 0: # skip first since we can't calculate (and don't want to use v_ego)
+ accelerations.append((speeds[-1] - speeds[-2]) / model_t[t])
+
+ accelerations.insert(0, accelerations[1] - (accelerations[2] - accelerations[1])) # extrapolate back first accel from second and third, less weight
+ return distances, speeds, accelerations
def update(self, sm, pm, CP, VM, PP):
"""Gets called when new radarState is available"""
@@ -360,11 +391,18 @@ def update(self, sm, pm, CP, VM, PP):
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
+ self.mpc_model.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(pm, sm['carState'], lead_1)
self.mpc2.update(pm, sm['carState'], lead_2)
- self.choose_solution(v_cruise_setpoint, enabled, lead_1, lead_2, sm['carState'].steeringAngle)
+ distances, speeds, accelerations = self.parse_modelV2_data(sm)
+ self.mpc_model.update(sm['carState'].vEgo, sm['carState'].aEgo,
+ distances,
+ speeds,
+ accelerations)
+
+ self.choose_solution(v_cruise_setpoint, enabled, lead_1, lead_2, sm['carState'].steeringAngle, sm['modelLongButton'].enabled)
# determine fcw
if self.mpc1.new_lead:
diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py
index 2f1e9467622f9f..53d698d6abd032 100755
--- a/selfdrive/controls/plannerd.py
+++ b/selfdrive/controls/plannerd.py
@@ -23,7 +23,7 @@ def plannerd_thread(sm=None, pm=None):
VM = VehicleModel(CP)
if sm is None:
- sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters', 'dragonConf', 'liveMapData'],
+ sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters', 'dragonConf', 'liveMapData', 'modelLongButton', 'modelV2'],
poll=['radarState', 'model'])
if pm is None:
diff --git a/selfdrive/ui/android/ui.cc b/selfdrive/ui/android/ui.cc
index 95b72719697ce4..d6f19a651ad8d6 100644
--- a/selfdrive/ui/android/ui.cc
+++ b/selfdrive/ui/android/ui.cc
@@ -2,6 +2,7 @@
#include <stdlib.h>
#include <math.h>
#include <sys/resource.h>
+#include <iostream>
#include <algorithm>
@@ -29,6 +30,29 @@ static void ui_set_brightness(UIState *s, int brightness) {
}
}
+static void send_ml(UIState *s, bool enabled) {
+ MessageBuilder msg;
+ auto mlStatus = msg.initEvent().initModelLongButton();
+ mlStatus.setEnabled(enabled);
+ s->pm->send("modelLongButton", msg);
+}
+
+static bool handle_ml_touch(UIState *s, int touch_x, int touch_y) {
+ //mlButton manager
+ int padding = 40;
+ int btn_w = 500;
+ int btn_h = 138;
+ int xs[2] = {1920 / 2 - btn_w / 2, 1920 / 2 + btn_w / 2};
+ int y_top = 915 - btn_h / 2;
+ if (xs[0] <= touch_x + padding && touch_x - padding <= xs[1] && y_top - padding <= touch_y) {
+ s->scene.mlButtonEnabled = !s->scene.mlButtonEnabled;
+ send_ml(s, s->scene.mlButtonEnabled);
+ printf("ml button: %d\n", s->scene.mlButtonEnabled);
+ return true;
+ }
+ return false;
+}
+
static void handle_display_state(UIState *s, bool user_input) {
static int awake_timeout = 0;
@@ -103,6 +127,10 @@ static bool handle_dp_btn_touch(UIState *s, int touch_x, int touch_y) {
Params().write_db_value("dp_last_modified", time_str, 11);
return true;
}
+ if (handle_ml_touch(s, touch_x, touch_y)) {
+ s->scene.uilayout_sidebarcollapsed = true; // collapse sidebar when tapping any SA button
+ return true; // only allow one button to be pressed at a time
+ }
}
return false;
}
@@ -161,6 +189,7 @@ int main(int argc, char* argv[]) {
UIState uistate = {};
UIState *s = &uistate;
ui_init(s);
+ sa_init(s, true);
s->sound = &sound;
TouchState touch = {0};
@@ -196,6 +225,12 @@ int main(int argc, char* argv[]) {
if (!s->started) {
usleep(50 * 1000);
}
+
+ if (s->started) {
+ sa_init(s, false); // reset ml button and regrab params
+ }
+
+
double u1 = millis_since_boot();
ui_update(s);
diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc
index c0ee6f989f75cc..d7ea19ab33ad79 100644
--- a/selfdrive/ui/paint.cc
+++ b/selfdrive/ui/paint.cc
@@ -548,6 +548,34 @@ static void ui_draw_driver_view(UIState *s) {
ui_draw_circle_image(s->vg, x, y, face_size, s->img_face, scene->dmonitoring_state.getFaceDetected());
}
+static void ui_draw_ml_button(UIState *s) {
+ int btn_w = 500;
+ int btn_h = 138;
+ int x = 1920 / 2;
+ int y = 915;
+ int btn_x = x - btn_w / 2;
+ int btn_y = y - btn_h / 2;
+
+ nvgBeginPath(s->vg);
+ nvgRoundedRect(s->vg, btn_x, btn_y, btn_w, btn_h, 25);
+ if (s->scene.mlButtonEnabled) { // change outline color based on status of button
+ nvgStrokeColor(s->vg, nvgRGBA(55, 184, 104, 255));
+ } else {
+ nvgStrokeColor(s->vg, nvgRGBA(184, 55, 55, 255));
+ }
+ nvgStrokeWidth(s->vg, 12);
+ nvgStroke(s->vg);
+
+ nvgBeginPath(s->vg); // dark background for readability
+ nvgRoundedRect(s->vg, btn_x, btn_y, btn_w, btn_h, 25);
+ nvgFillColor(s->vg, nvgRGBA(75, 75, 75, 75));
+ nvgFill(s->vg);
+
+ nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
+ nvgFontSize(s->vg, 65);
+ nvgText(s->vg, x, y + btn_h / 8, "Toggle Model Long", NULL);
+}
+
static void ui_draw_vision_header(UIState *s) {
const Rect &viz_rect = s->scene.viz_rect;
if (!s->scene.dpFullScreenApp) {
@@ -567,6 +595,7 @@ static void ui_draw_vision_header(UIState *s) {
if (s->scene.dpUiEvent) {
ui_draw_vision_event(s);
}
+ ui_draw_ml_button(s);
}
static void ui_draw_vision_footer(UIState *s) {
diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc
index 0b2a110bacd8ad..8eb966df22c1b1 100644
--- a/selfdrive/ui/ui.cc
+++ b/selfdrive/ui/ui.cc
@@ -8,6 +8,8 @@
#include <math.h>
#include <poll.h>
#include <sys/mman.h>
+//#include "json11.hpp"
+//#include <fstream>
#include "common/util.h"
#include "common/swaglog.h"
@@ -24,6 +26,34 @@ int write_param_float(float param, const char* param_name, bool persistent_param
return Params(persistent_param).write_db_value(param_name, s, size < sizeof(s) ? size : sizeof(s));
}
+void sa_init(UIState *s, bool full_init) {
+ if (full_init) {
+ s->pm = new PubMaster({"modelLongButton"});
+ }
+
+ //s->ui_debug = false; // change to true while debugging
+
+ // stock additions todo: run opparams first (in main()?) to ensure json values exist
+ //std::ifstream op_params_file("/data/op_params.json");
+ //std::string op_params_content((std::istreambuf_iterator<char>(op_params_file)),
+ //(std::istreambuf_iterator<char>()));
+
+ //std::string err;
+ //auto json = json11::Json::parse(op_params_content, err);
+ //if (!json.is_null() && err.empty()) {
+ //printf("successfully parsed opParams json\n");
+ //s->scene.dfButtonStatus = DF_TO_IDX[json["dynamic_follow"].string_value()];
+ //s->scene.lsButtonStatus = LS_TO_IDX[json["lane_speed_alerts"].string_value()];
+// printf("dfButtonStatus: %d\n", s->scene.dfButtonStatus);
+// printf("lsButtonStatus: %d\n", s->scene.lsButtonStatus);
+ //} else { // error parsing json
+ //printf("ERROR PARSING OPPARAMS JSON!\n");
+ //s->scene.dfButtonStatus = 0;
+ //s->scene.lsButtonStatus = 0;
+ //}
+ s->scene.mlButtonEnabled = false; // state isn't saved yet
+}
+
void ui_init(UIState *s) {
s->sm = new SubMaster({"modelV2", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "thermal", "liveMapData",
"health", "carParams", "ubloxGnss", "driverState", "dMonitoringState", "sensorEvents",
diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp
index 18a743559861ac..39477ea7eadc93 100644
--- a/selfdrive/ui/ui.hpp
+++ b/selfdrive/ui/ui.hpp
@@ -107,6 +107,8 @@ typedef struct {
typedef struct UIScene {
+ bool mlButtonEnabled;
+
mat4 extrinsic_matrix; // Last row is 0 so we can use mat4.
bool world_objects_visible;
@@ -232,7 +234,7 @@ typedef struct UIState {
int img_speed;
SubMaster *sm;
-
+ PubMaster *pm;
Sound *sound;
UIStatus status;
UIScene scene;
@@ -278,6 +280,7 @@ typedef struct UIState {
} UIState;
void ui_init(UIState *s);
+void sa_init(UIState *s, bool full_init);
void ui_update(UIState *s);
int write_param_float(float param, const char* param_name, bool persistent_param = false);