Merge branch 'master' of https://github.com/commaai/openpilot into mqb-long

panda

@@ -1 +1 @@
-Subproject commit b1205ad3bffbb9f75e7747920e2b91f249620fb2
+Subproject commit 0b85d22b804200698748d6f8ce58ab607ebca988

selfdrive/boardd/boardd.cc

@@ -237,6 +237,7 @@ void can_recv_thread(std::vector<Panda *> pandas) {
   // run at 100hz
   const uint64_t dt = 10000000ULL;
   uint64_t next_frame_time = nanos_since_boot() + dt;
+  std::vector<can_frame> raw_can_data;
 
   while (!do_exit) {
     if (!check_all_connected(pandas)){
@@ -244,8 +245,8 @@ void can_recv_thread(std::vector<Panda *> pandas) {
       break;
     }
 
-    std::vector<can_frame> raw_can_data;
     bool comms_healthy = true;
+    raw_can_data.clear();
     for (const auto& panda : pandas) {
       comms_healthy &= panda->can_receive(raw_can_data);
     }
@@ -627,10 +628,6 @@ void pigeon_thread(Panda *panda) {
 }
 
 int main(int argc, char *argv[]) {
-  std::vector<std::thread> threads;
-  std::vector<Panda *> pandas;
-  Panda *peripheral_panda;
-
   LOGW("starting boardd");
 
   if (!Hardware::PC()) {
@@ -644,37 +641,29 @@ int main(int argc, char* argv[]) {
   LOGW("attempting to connect");
   PubMaster pm({"pandaStates", "peripheralState"});
 
-  // connect loop
-  while (!do_exit) {
   std::vector<std::string> serials(argv + 1, argv + argc);
   if (serials.size() == 0) serials.push_back("");
 
   // connect to all provided serials
-    for (int i=0; i<serials.size(); i++) {
+  std::vector<Panda *> pandas;
+  for (int i = 0; i < serials.size() && !do_exit; /**/) {
     Panda *p = usb_connect(serials[i], i);
-      if (p != NULL) {
+    if (!p) {
-        pandas.push_back(p);
-      }
-    }
-
       // send empty pandaState & peripheralState and try again
-    if (pandas.size() != serials.size()) {
       send_empty_panda_state(&pm);
       send_empty_peripheral_state(&pm);
       util::sleep_for(500);
-    } else {
+      continue;
-      break;
-    }
     }
 
-  if (pandas.size() == 0) {
+    pandas.push_back(p);
-    // do_exit was set while not connected to a panda
+    ++i;
-    return 0;
   }
 
-  peripheral_panda = pandas[0];
+  if (!do_exit) {
-
     LOGW("connected to board");
+    Panda *peripheral_panda = pandas[0];
+    std::vector<std::thread> threads;
 
     threads.emplace_back(panda_state_thread, &pm, pandas, getenv("STARTED") != nullptr);
     threads.emplace_back(peripheral_control_thread, peripheral_panda);
@@ -684,9 +673,10 @@ int main(int argc, char* argv[]) {
     threads.emplace_back(can_recv_thread, pandas);
 
     for (auto &t : threads) t.join();
+  }
 
   // we have exited, clean up pandas
-  for (const auto& panda : pandas){
+  for (Panda *panda : pandas) {
     delete panda;
   }
 }

selfdrive/boardd/panda.cc

@@ -364,12 +364,14 @@ uint8_t Panda::len_to_dlc(uint8_t len) {
 }
 
 void Panda::can_send(capnp::List<cereal::CanData>::Reader can_data_list) {
-  send.resize(72 * can_data_list.size()); // TODO: need to include 1 byte for each usb 64bytes frame
+  if (send.size() < (can_data_list.size() * CANPACKET_MAX_SIZE)) {
+    send.resize(can_data_list.size() * CANPACKET_MAX_SIZE);
+  }
 
   int msg_count = 0;
   while (msg_count < can_data_list.size()) {
     uint32_t pos = 0;
-    while (pos < 256) {
+    while (pos < USB_TX_SOFT_LIMIT) {
       if (msg_count == can_data_list.size()) { break; }
       auto cmsg = can_data_list[msg_count];
 
@@ -384,26 +386,31 @@ void Panda::can_send(capnp::List<cereal::CanData>::Reader can_data_list) {
       assert(can_data.size() <= (hw_type == cereal::PandaState::PandaType::RED_PANDA) ? 64 : 8);
       assert(can_data.size() == dlc_to_len[data_len_code]);
 
-      *(uint32_t*)&send[pos+1] = (cmsg.getAddress() << 3);
+      can_header header;
-      if (cmsg.getAddress() >= 0x800) {
+      header.addr = cmsg.getAddress();
-        *(uint32_t*)&send[pos+1] |= (1 << 2);
+      header.extended = (cmsg.getAddress() >= 0x800) ? 1 : 0;
-      }
+      header.data_len_code = data_len_code;
-      send[pos] = data_len_code << 4 | ((bus - bus_offset) << 1);
+      header.bus = bus - bus_offset;
-      memcpy(&send[pos+5], can_data.begin(), can_data.size());
+      memcpy(&send[pos], &header, CANPACKET_HEAD_SIZE);
+      memcpy(&send[pos+CANPACKET_HEAD_SIZE], can_data.begin(), can_data.size());
 
       pos += CANPACKET_HEAD_SIZE + dlc_to_len[data_len_code];
       msg_count++;
     }
 
     if (pos > 0) { // Helps not to spam with ZLP
-      // insert counter
+      // Counter needs to be inserted every 64 bytes (first byte of 64 bytes USB packet)
       uint8_t counter = 0;
-      for (int i = 0; i < pos; i += 64) {
+      uint8_t to_write[USB_TX_SOFT_LIMIT+128];
-        send.insert(send.begin() + i, counter);
+      int ptr = 0;
+      for (int i = 0; i < pos; i += 63) {
+        to_write[ptr] = counter;
+        int copy_size = ((pos - i) < 63) ? (pos - i) : 63;
+        memcpy(&to_write[ptr+1], &(send.data()[i]) , copy_size);
+        ptr += copy_size + 1;
         counter++;
-        pos++;
       }
-      usb_bulk_write(3, (uint8_t*)send.data(), pos, 5);
+      usb_bulk_write(3, to_write, ptr, 5);
     }
   }
 }
@@ -415,7 +422,6 @@ bool Panda::can_receive(std::vector<can_frame>& out_vec) {
   // Not sure if this can happen
   if (recv < 0) recv = 0;
 
-  // TODO: Might change from full to overloaded? if > some threshold that is lower than RECV_SIZE, let's say 80-90%
   if (recv == RECV_SIZE) {
     LOGW("Receive buffer full");
   }
@@ -424,19 +430,18 @@ bool Panda::can_receive(std::vector<can_frame>& out_vec) {
     return false;
   }
 
-  out_vec.reserve(out_vec.size() + (recv / CANPACKET_HEAD_SIZE));
+  static uint8_t tail[CANPACKET_MAX_SIZE];
-
-  static uint8_t tail[72];
   uint8_t tail_size = 0;
   uint8_t counter = 0;
   for (int i = 0; i < recv; i += 64) {
+    // Check for counter every 64 bytes (length of USB packet)
     if (counter != data[i]) {
       LOGE("CAN: MALFORMED USB RECV PACKET");
       break;
     }
     counter++;
     uint8_t chunk_len = ((recv - i) > 64) ? 63 : (recv - i - 1); // as 1 is always reserved for counter
-    uint8_t chunk[72];
+    uint8_t chunk[CANPACKET_MAX_SIZE];
     memcpy(chunk, tail, tail_size);
     memcpy(&chunk[tail_size], &data[i+1], chunk_len);
     chunk_len += tail_size;
@@ -446,22 +451,21 @@ bool Panda::can_receive(std::vector<can_frame>& out_vec) {
       uint8_t data_len = dlc_to_len[(chunk[pos] >> 4)];
       uint8_t pckt_len = CANPACKET_HEAD_SIZE + data_len;
       if (pckt_len <= (chunk_len - pos)) {
-        can_frame canData;
+        can_header header;
+        memcpy(&header, &chunk[pos], CANPACKET_HEAD_SIZE);
+
+        can_frame &canData = out_vec.emplace_back();
         canData.busTime = 0;
-        canData.address = (*(uint32_t*)&chunk[pos+1]) >> 3;
+        canData.address = header.addr;
-        canData.src = ((chunk[pos] >> 1) & 0x7) + bus_offset;
+        canData.src = header.bus + bus_offset;
 
-        bool rejected = chunk[pos+1] & 0x1;
+        if (header.rejected) { canData.src += CANPACKET_REJECTED; }
-        bool returned = (chunk[pos+1] >> 1) & 0x1;
+        if (header.returned) { canData.src += CANPACKET_RETURNED; }
-        if (rejected) { canData.src += CANPACKET_REJECTED; }
+        canData.dat.assign((char*)&chunk[pos+CANPACKET_HEAD_SIZE], data_len);
-        if (returned) { canData.src += CANPACKET_RETURNED; }
-        canData.dat.assign((char*)&chunk[pos+5], data_len);
 
         pos += pckt_len;
-
-        // add to vector
-        out_vec.push_back(canData);
       } else {
+        // Keep partial CAN packet until next USB packet
         tail_size = (chunk_len - pos);
         memcpy(tail, &chunk[pos], tail_size);
         break;

selfdrive/boardd/panda.h

@@ -13,11 +13,12 @@
 #include "cereal/gen/cpp/car.capnp.h"
 #include "cereal/gen/cpp/log.capnp.h"
 
-// double the FIFO size
-#define RECV_SIZE (0x4000)
 #define TIMEOUT 0
 #define PANDA_BUS_CNT 4
-#define CANPACKET_HEAD_SIZE (0x5U)
+#define RECV_SIZE (0x4000U)
+#define USB_TX_SOFT_LIMIT   (0x100U)
+#define CANPACKET_HEAD_SIZE 5U
+#define CANPACKET_MAX_SIZE  72U
 #define CANPACKET_REJECTED  (0xC0U)
 #define CANPACKET_RETURNED  (0x80U)
 
@@ -44,6 +45,16 @@ struct __attribute__((packed)) health_t {
   uint8_t heartbeat_lost;
 };
 
+struct __attribute__((packed)) can_header {
+  uint8_t reserved : 1;
+  uint8_t bus : 3;
+  uint8_t data_len_code : 4;
+  uint8_t rejected : 1;
+  uint8_t returned : 1;
+  uint8_t extended : 1;
+  uint32_t addr : 29;
+};
+
 struct can_frame {
 	long address;
 	std::string dat;

selfdrive/car/honda/carstate.py

@@ -5,7 +5,7 @@ from opendbc.can.can_define import CANDefine
 from opendbc.can.parser import CANParser
 from selfdrive.config import Conversions as CV
 from selfdrive.car.interfaces import CarStateBase
-from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, SPEED_FACTOR, HONDA_BOSCH, HONDA_BOSCH_ALT_BRAKE_SIGNAL
+from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, SPEED_FACTOR, HONDA_BOSCH, HONDA_NIDEC_ALT_MAIN, HONDA_BOSCH_ALT_BRAKE_SIGNAL
 
 TransmissionType = car.CarParams.TransmissionType
 
@@ -161,8 +161,7 @@ class CarState(CarStateBase):
       self.gearbox_msg = "GEARBOX_15T"
 
     self.main_on_sig_msg = "SCM_FEEDBACK"
-    if CP.carFingerprint in (CAR.ACURA_ILX, CAR.ACURA_RDX, CAR.CRV, CAR.CRV_EU, CAR.FIT, CAR.FREED, CAR.HRV,
+    if CP.carFingerprint in HONDA_NIDEC_ALT_MAIN:
-                             CAR.ODYSSEY_CHN, CAR.PILOT, CAR.PILOT_2019, CAR.PASSPORT, CAR.RIDGELINE):
       self.main_on_sig_msg = "SCM_BUTTONS"
 
     self.shifter_values = can_define.dv[self.gearbox_msg]["GEAR_SHIFTER"]

selfdrive/car/honda/interface.py

@@ -3,7 +3,7 @@ from cereal import car
 from panda import Panda
 from common.numpy_fast import interp
 from common.params import Params
-from selfdrive.car.honda.values import CarControllerParams, CruiseButtons, CAR, HONDA_BOSCH, HONDA_BOSCH_ALT_BRAKE_SIGNAL
+from selfdrive.car.honda.values import CarControllerParams, CruiseButtons, CAR, HONDA_BOSCH, HONDA_NIDEC_ALT_MAIN, HONDA_BOSCH_ALT_BRAKE_SIGNAL
 from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
 from selfdrive.car.disable_ecu import disable_ecu
@@ -299,6 +299,10 @@ class CarInterface(CarInterfaceBase):
     if candidate in HONDA_BOSCH_ALT_BRAKE_SIGNAL:
       ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_ALT_BRAKE
 
+    # These cars use an alternate main on message
+    if candidate in HONDA_NIDEC_ALT_MAIN:
+      ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_NIDEC_ALT
+
     if ret.openpilotLongitudinalControl and candidate in HONDA_BOSCH:
       ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_BOSCH_LONG
 

selfdrive/car/honda/values.py

@@ -1365,5 +1365,7 @@ SPEED_FACTOR = {
 }
 
 HONDA_NIDEC_ALT_PCM_ACCEL = set([CAR.ODYSSEY])
+HONDA_NIDEC_ALT_MAIN = set([CAR.ACURA_ILX, CAR.ACURA_RDX, CAR.CRV, CAR.CRV_EU, CAR.FIT, CAR.FREED, CAR.HRV, CAR.ODYSSEY_CHN,
+                            CAR.PILOT, CAR.PILOT_2019, CAR.PASSPORT, CAR.RIDGELINE])
 HONDA_BOSCH = set([CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E])
 HONDA_BOSCH_ALT_BRAKE_SIGNAL = set([CAR.ACCORD, CAR.CRV_5G, CAR.ACURA_RDX_3G])

selfdrive/common/params.cc

@@ -158,12 +158,14 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"ApiCache_DriveStats", PERSISTENT},
     {"ApiCache_NavDestinations", PERSISTENT},
     {"ApiCache_Owner", PERSISTENT},
+    {"Offroad_CarUnrecognized", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
     {"Offroad_ChargeDisabled", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},
     {"Offroad_ConnectivityNeeded", CLEAR_ON_MANAGER_START},
     {"Offroad_ConnectivityNeededPrompt", CLEAR_ON_MANAGER_START},
     {"Offroad_InvalidTime", CLEAR_ON_MANAGER_START},
     {"Offroad_IsTakingSnapshot", CLEAR_ON_MANAGER_START},
     {"Offroad_NeosUpdate", CLEAR_ON_MANAGER_START},
+    {"Offroad_NoFirmware", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
     {"Offroad_StorageMissing", CLEAR_ON_MANAGER_START},
     {"Offroad_TemperatureTooHigh", CLEAR_ON_MANAGER_START},
     {"Offroad_UnofficialHardware", CLEAR_ON_MANAGER_START},

selfdrive/controls/controlsd.py

@@ -22,7 +22,7 @@ from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
 from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
 from selfdrive.controls.lib.latcontrol_angle import LatControlAngle
 from selfdrive.controls.lib.events import Events, ET
-from selfdrive.controls.lib.alertmanager import AlertManager
+from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
 from selfdrive.controls.lib.vehicle_model import VehicleModel
 from selfdrive.locationd.calibrationd import Calibration
 from selfdrive.hardware import HARDWARE, TICI, EON
@@ -170,6 +170,10 @@ class Controls:
       self.events.add(EventName.communityFeatureDisallowed, static=True)
     if not car_recognized:
       self.events.add(EventName.carUnrecognized, static=True)
+      if len(self.CP.carFw) > 0:
+        set_offroad_alert("Offroad_CarUnrecognized", True)
+      else:
+        set_offroad_alert("Offroad_NoFirmware", True)
     elif self.read_only:
       self.events.add(EventName.dashcamMode, static=True)
     elif self.joystick_mode:

selfdrive/controls/lib/alerts_offroad.json

@@ -40,5 +40,13 @@
   "Offroad_StorageMissing": {
     "text": "Storage drive not mounted.",
     "severity": 1
+  },
+  "Offroad_CarUnrecognized": {
+    "text": "openpilot was unable to identify your car. Your car is either unsupported or its ECUs are not recognized. Please submit a pull request to add the firmware versions to the proper vehicle. Need help? Join discord.comma.ai.",
+    "severity": 0
+  },
+  "Offroad_NoFirmware": {
+    "text": "openpilot was unable to identify your car. Check integrity of cables and ensure all connections are secure, particularly that the comma power is fully inserted in the OBD-II port of the vehicle. Need help? Join discord.comma.ai.",
+    "severity": 0
   }
 }

selfdrive/controls/lib/lateral_planner.py

@@ -170,7 +170,7 @@ class LateralPlanner():
       self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, CP.steerRateCost)
     else:
       d_path_xyz = self.path_xyz
-      path_cost = np.clip(abs(self.path_xyz[0,1]/self.path_xyz_stds[0,1]), 0.5, 5.0) * MPC_COST_LAT.PATH
+      path_cost = np.clip(abs(self.path_xyz[0,1]/self.path_xyz_stds[0,1]), 0.5, 1.5) * MPC_COST_LAT.PATH
       # Heading cost is useful at low speed, otherwise end of plan can be off-heading
       heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.0])
       self.lat_mpc.set_weights(path_cost, heading_cost, CP.steerRateCost)

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-6540e8c5a765975fd292b1efdef97b2d6391fa9c
+848bc88a069b182d6f50698dd785ba360915e8dc

selfdrive/ui/SConscript

@@ -116,7 +116,7 @@ if arch in ['x86_64', 'Darwin'] or GetOption('extras'):
   replay_lib_src = ["replay/replay.cc", "replay/camera.cc", "replay/filereader.cc", "replay/logreader.cc", "replay/framereader.cc", "replay/route.cc", "replay/util.cc"]
 
   replay_lib = qt_env.Library("qt_replay", replay_lib_src, LIBS=base_libs)
-  replay_libs = [replay_lib, 'avutil', 'avcodec', 'avformat', 'bz2', 'curl', 'swscale'] + qt_libs
+  replay_libs = [replay_lib, 'avutil', 'avcodec', 'avformat', 'bz2', 'curl', 'swscale', 'yuv'] + qt_libs
   qt_env.Program("replay/replay", ["replay/main.cc"], LIBS=replay_libs)
 
   qt_env.Program("watch3", ["watch3.cc"], LIBS=qt_libs + ['common', 'json11'])

selfdrive/ui/replay/framereader.cc

@@ -1,6 +1,7 @@
 #include "selfdrive/ui/replay/framereader.h"
 
 #include <cassert>
+#include "libyuv.h"
 
 namespace {
 
@@ -227,13 +228,26 @@ AVFrame *FrameReader::decodeFrame(AVPacket *pkt) {
 }
 
 bool FrameReader::copyBuffers(AVFrame *f, uint8_t *rgb, uint8_t *yuv) {
-  // images is going to be written to output buffers, no alignment (align = 1)
   if (yuv) {
+    if (sws_src_format == AV_PIX_FMT_NV12) {
+      // libswscale crash if height is not 16 bytes aligned for NV12->YUV420 conversion
+      assert(sws_src_format == AV_PIX_FMT_NV12);
+      uint8_t *u = yuv + width * height;
+      uint8_t *v = u + (width / 2) * (height / 2);
+      libyuv::NV12ToI420(f->data[0], f->linesize[0],
+                         f->data[1], f->linesize[1],
+                         yuv, width,
+                         u, width / 2,
+                         v, width / 2,
+                         width, height);
+    } else {
       av_image_fill_arrays(sws_frame->data, sws_frame->linesize, yuv, AV_PIX_FMT_YUV420P, width, height, 1);
       int ret = sws_scale(yuv_sws_ctx_, (const uint8_t **)f->data, f->linesize, 0, f->height, sws_frame->data, sws_frame->linesize);
       if (ret < 0) return false;
     }
+  }
 
+  // images is going to be written to output buffers, no alignment (align = 1)
   av_image_fill_arrays(sws_frame->data, sws_frame->linesize, rgb, AV_PIX_FMT_BGR24, width, height, 1);
   int ret = sws_scale(rgb_sws_ctx_, (const uint8_t **)f->data, f->linesize, 0, f->height, sws_frame->data, sws_frame->linesize);
   return ret >= 0;