diff --git a/panda b/panda index b1205ad3bf..0b85d22b80 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit b1205ad3bffbb9f75e7747920e2b91f249620fb2 +Subproject commit 0b85d22b804200698748d6f8ce58ab607ebca988 diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 50749a1061..ce73662e45 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -237,6 +237,7 @@ void can_recv_thread(std::vector pandas) { // run at 100hz const uint64_t dt = 10000000ULL; uint64_t next_frame_time = nanos_since_boot() + dt; + std::vector raw_can_data; while (!do_exit) { if (!check_all_connected(pandas)){ @@ -244,8 +245,8 @@ void can_recv_thread(std::vector pandas) { break; } - std::vector raw_can_data; bool comms_healthy = true; + raw_can_data.clear(); for (const auto& panda : pandas) { comms_healthy &= panda->can_receive(raw_can_data); } @@ -626,11 +627,7 @@ void pigeon_thread(Panda *panda) { delete pigeon; } -int main(int argc, char* argv[]) { - std::vector threads; - std::vector pandas; - Panda *peripheral_panda; - +int main(int argc, char *argv[]) { LOGW("starting boardd"); if (!Hardware::PC()) { @@ -644,49 +641,42 @@ int main(int argc, char* argv[]) { LOGW("attempting to connect"); PubMaster pm({"pandaStates", "peripheralState"}); - // connect loop - while (!do_exit) { - std::vector serials(argv + 1, argv + argc); - if (serials.size() == 0) serials.push_back(""); - - // connect to all provided serials - for (int i=0; i serials(argv + 1, argv + argc); + if (serials.size() == 0) serials.push_back(""); - // send empty pandaState & peripheralState and try again - if (pandas.size() != serials.size()) { + // connect to all provided serials + std::vector pandas; + for (int i = 0; i < serials.size() && !do_exit; /**/) { + Panda *p = usb_connect(serials[i], i); + if (!p) { + // send empty pandaState & peripheralState and try again send_empty_panda_state(&pm); send_empty_peripheral_state(&pm); util::sleep_for(500); - } else { - break; + continue; } - } - if (pandas.size() == 0) { - // do_exit was set while not connected to a panda - return 0; + pandas.push_back(p); + ++i; } - peripheral_panda = pandas[0]; + if (!do_exit) { + LOGW("connected to board"); + Panda *peripheral_panda = pandas[0]; + std::vector threads; - LOGW("connected to board"); + threads.emplace_back(panda_state_thread, &pm, pandas, getenv("STARTED") != nullptr); + threads.emplace_back(peripheral_control_thread, peripheral_panda); + threads.emplace_back(pigeon_thread, peripheral_panda); - threads.emplace_back(panda_state_thread, &pm, pandas, getenv("STARTED") != nullptr); - threads.emplace_back(peripheral_control_thread, peripheral_panda); - threads.emplace_back(pigeon_thread, peripheral_panda); + threads.emplace_back(can_send_thread, pandas, getenv("FAKESEND") != nullptr); + threads.emplace_back(can_recv_thread, pandas); - threads.emplace_back(can_send_thread, pandas, getenv("FAKESEND") != nullptr); - threads.emplace_back(can_recv_thread, pandas); - - for (auto &t : threads) t.join(); + for (auto &t : threads) t.join(); + } // we have exited, clean up pandas - for (const auto& panda : pandas){ + for (Panda *panda : pandas) { delete panda; } } diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index 546a7904f2..01c9f9fa8f 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -364,12 +364,14 @@ uint8_t Panda::len_to_dlc(uint8_t len) { } void Panda::can_send(capnp::List::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::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); - if (cmsg.getAddress() >= 0x800) { - *(uint32_t*)&send[pos+1] |= (1 << 2); - } - send[pos] = data_len_code << 4 | ((bus - bus_offset) << 1); - memcpy(&send[pos+5], can_data.begin(), can_data.size()); + can_header header; + header.addr = cmsg.getAddress(); + header.extended = (cmsg.getAddress() >= 0x800) ? 1 : 0; + header.data_len_code = data_len_code; + header.bus = bus - bus_offset; + 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) { - send.insert(send.begin() + i, counter); + uint8_t to_write[USB_TX_SOFT_LIMIT+128]; + 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& 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& out_vec) { return false; } - out_vec.reserve(out_vec.size() + (recv / CANPACKET_HEAD_SIZE)); - - static uint8_t tail[72]; + static uint8_t tail[CANPACKET_MAX_SIZE]; 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& 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.src = ((chunk[pos] >> 1) & 0x7) + bus_offset; + canData.address = header.addr; + canData.src = header.bus + bus_offset; - bool rejected = chunk[pos+1] & 0x1; - bool returned = (chunk[pos+1] >> 1) & 0x1; - if (rejected) { canData.src += CANPACKET_REJECTED; } - if (returned) { canData.src += CANPACKET_RETURNED; } - canData.dat.assign((char*)&chunk[pos+5], data_len); + if (header.rejected) { canData.src += CANPACKET_REJECTED; } + if (header.returned) { canData.src += CANPACKET_RETURNED; } + canData.dat.assign((char*)&chunk[pos+CANPACKET_HEAD_SIZE], 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; diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h index e42955d2b2..415bb30103 100644 --- a/selfdrive/boardd/panda.h +++ b/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; diff --git a/selfdrive/boardd/tests/test_boardd b/selfdrive/boardd/tests/test_boardd new file mode 100755 index 0000000000..f249724f6a Binary files /dev/null and b/selfdrive/boardd/tests/test_boardd differ diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 6dc4d85da8..affe377276 100644 --- a/selfdrive/car/honda/carstate.py +++ b/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, - CAR.ODYSSEY_CHN, CAR.PILOT, CAR.PILOT_2019, CAR.PASSPORT, CAR.RIDGELINE): + if CP.carFingerprint in HONDA_NIDEC_ALT_MAIN: self.main_on_sig_msg = "SCM_BUTTONS" self.shifter_values = can_define.dv[self.gearbox_msg]["GEAR_SHIFTER"] diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 1f49137eda..63e3f05770 100755 --- a/selfdrive/car/honda/interface.py +++ b/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 diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index 70ff7e006c..a50b8ce154 100644 --- a/selfdrive/car/honda/values.py +++ b/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]) diff --git a/selfdrive/common/params.cc b/selfdrive/common/params.cc index 7e65ec128c..01bf9a8f4f 100644 --- a/selfdrive/common/params.cc +++ b/selfdrive/common/params.cc @@ -158,12 +158,14 @@ std::unordered_map 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}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index bd184f200f..bbd84d458f 100755 --- a/selfdrive/controls/controlsd.py +++ b/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: diff --git a/selfdrive/controls/lib/alerts_offroad.json b/selfdrive/controls/lib/alerts_offroad.json index 9a863551a8..4affe0cfe3 100644 --- a/selfdrive/controls/lib/alerts_offroad.json +++ b/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 } } diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index f18c3370d6..74b27f595d 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/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) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 5027b0c179..92ae208dd0 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -6540e8c5a765975fd292b1efdef97b2d6391fa9c \ No newline at end of file +848bc88a069b182d6f50698dd785ba360915e8dc \ No newline at end of file diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index d4c8fe71e2..bc21022dd9 100644 --- a/selfdrive/ui/SConscript +++ b/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']) diff --git a/selfdrive/ui/replay/framereader.cc b/selfdrive/ui/replay/framereader.cc index 1524ad9748..fdbfc0672c 100644 --- a/selfdrive/ui/replay/framereader.cc +++ b/selfdrive/ui/replay/framereader.cc @@ -1,6 +1,7 @@ #include "selfdrive/ui/replay/framereader.h" #include +#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) { - 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; + 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;