Merge branch 'commaai:master' into PA-dev

Edison-CBS · Sep 15, 2024 · d101fe7 · d101fe7
2 parents b701334 + dcbeda1
commit d101fe7
Show file tree

Hide file tree

Showing 5 changed files with 19 additions and 10 deletions.
diff --git a/cereal/log.capnp b/cereal/log.capnp
@@ -735,7 +735,6 @@ struct ControlsState @0x97ff69c53601abf1 {
   upAccelCmd @4 :Float32;
   uiAccelCmd @5 :Float32;
   ufAccelCmd @33 :Float32;
-  aTarget @35 :Float32;
   curvature @37 :Float32;  # path curvature from vehicle model
   desiredCurvature @61 :Float32;  # lag adjusted curvatures used by lateral controllers
   forceDecel @51 :Bool;
@@ -881,6 +880,7 @@ struct ControlsState @0x97ff69c53601abf1 {
   vCruiseClusterDEPRECATED @63 :Float32;  # set speed to display in the UI
   startMonoTimeDEPRECATED @48 :UInt64;
   cumLagMsDEPRECATED @15 :Float32;
+  aTargetDEPRECATED @35 :Float32;
 }
 
 struct DrivingModelData {

diff --git a/selfdrive/pandad/panda.cc b/selfdrive/pandad/panda.cc
@@ -47,7 +47,7 @@ std::vector<std::string> Panda::list(bool usb_only) {
 
 #ifndef __APPLE__
   if (!usb_only) {
-    for (auto s : PandaSpiHandle::list()) {
+    for (const auto &s : PandaSpiHandle::list()) {
       if (std::find(serials.begin(), serials.end(), s) == serials.end()) {
         serials.push_back(s);
       }
@@ -169,7 +169,7 @@ void Panda::pack_can_buffer(const capnp::List<cereal::CanData>::Reader &can_data
   int32_t pos = 0;
   uint8_t send_buf[2 * USB_TX_SOFT_LIMIT];
 
-  for (auto cmsg : can_data_list) {
+  for (const auto &cmsg : can_data_list) {
     // check if the message is intended for this panda
     uint8_t bus = cmsg.getSrc();
     if (bus < bus_offset || bus >= (bus_offset + PANDA_BUS_OFFSET)) {
@@ -206,7 +206,7 @@ void Panda::pack_can_buffer(const capnp::List<cereal::CanData>::Reader &can_data
   if (pos > 0) write_func(send_buf, pos);
 }
 
-void Panda::can_send(capnp::List<cereal::CanData>::Reader can_data_list) {
+void Panda::can_send(const capnp::List<cereal::CanData>::Reader &can_data_list) {
   pack_can_buffer(can_data_list, [=](uint8_t* data, size_t size) {
     handle->bulk_write(3, data, size, 5);
   });

diff --git a/selfdrive/pandad/panda.h b/selfdrive/pandad/panda.h
@@ -79,7 +79,7 @@ class Panda {
   void set_can_speed_kbps(uint16_t bus, uint16_t speed);
   void set_data_speed_kbps(uint16_t bus, uint16_t speed);
   void set_canfd_non_iso(uint16_t bus, bool non_iso);
-  void can_send(capnp::List<cereal::CanData>::Reader can_data_list);
+  void can_send(const capnp::List<cereal::CanData>::Reader &can_data_list);
   bool can_receive(std::vector<can_frame>& out_vec);
   void can_reset_communications();
 

diff --git a/tools/longitudinal_maneuvers/.gitignore b/tools/longitudinal_maneuvers/.gitignore
@@ -0,0 +1 @@
+/longitudinal_reports/
diff --git a/tools/longitudinal_maneuvers/maneuversd.py b/tools/longitudinal_maneuvers/maneuversd.py
@@ -28,8 +28,10 @@ class Maneuver:
   _ready_cnt: int = 0
   _repeated: int = 0
 
-  def get_accel(self, v_ego: float, long_active: bool, standstill: bool) -> float:
-    ready = abs(v_ego - self.initial_speed) < 0.3 and long_active and not standstill
+  def get_accel(self, v_ego: float, long_active: bool, standstill: bool, cruise_standstill: bool) -> float:
+    ready = abs(v_ego - self.initial_speed) < 0.3 and long_active and not cruise_standstill
+    if self.initial_speed < 0.01:
+      ready = v_ego < 0.1 and standstill
     self._ready_cnt = (self._ready_cnt + 1) if ready else 0
 
     if self._ready_cnt > (3. / DT_MDL):
@@ -70,6 +72,12 @@ def active(self):
 
 
 MANEUVERS = [
+  Maneuver(
+   "start from stop",
+   [Action(1.5, 3)],
+   repeat=3,
+   initial_speed=0.,
+  ),
   Maneuver(
    "creep: alternate between +1m/s^2 and -1m/s^2",
    [
@@ -112,7 +120,7 @@ def main():
   cloudlog.info("joystickd is waiting for CarParams")
   CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
 
-  sm = messaging.SubMaster(['carState', 'controlsState', 'selfdriveState', 'modelV2'], poll='modelV2')
+  sm = messaging.SubMaster(['carState', 'carControl', 'controlsState', 'selfdriveState', 'modelV2'], poll='modelV2')
   pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'alertDebug'])
 
   maneuvers = iter(MANEUVERS)
@@ -135,12 +143,12 @@ def main():
     v_ego = max(sm['carState'].vEgo, 0)
 
     if maneuver is not None:
-      accel = maneuver.get_accel(v_ego, sm['carControl'].longActive, sm['carState'].cruiseState.standstill)
+      accel = maneuver.get_accel(v_ego, sm['carControl'].longActive, sm['carState'].standstill, sm['carState'].cruiseState.standstill)
 
       if maneuver.active:
         alert_msg.alertDebug.alertText1 = f'Maneuver Active: {accel:0.2f} m/s^2'
       else:
-        alert_msg.alertDebug.alertText1 = f'Reaching Speed: {maneuver.initial_speed * CV.MS_TO_MPH:0.2f} mph'
+        alert_msg.alertDebug.alertText1 = f'Setting up to {maneuver.initial_speed * CV.MS_TO_MPH:0.2f} mph'
       alert_msg.alertDebug.alertText2 = f'{maneuver.description}'
     else:
       alert_msg.alertDebug.alertText1 = 'Maneuvers Finished'