diff --git a/README.md b/README.md index e1a289bd90..c8762963c0 100644 --- a/README.md +++ b/README.md @@ -10,7 +10,7 @@ The openpilot codebase has been written to be concise and enable rapid prototypi Community ------ -openpilot is supported by [comma.ai](https://comma.ai/). +openpilot is developed by [comma.ai](https://comma.ai/) and users like you. We have a [Twitter you should follow](https://twitter.com/comma_ai). @@ -41,57 +41,62 @@ Install openpilot on a neo device by entering ``https://openpilot.comma.ai`` dur Supported Cars ------ -| Make | Model | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below | -| ------- | ---------------------- | -------------------- | ------- | ------------ | -------------- | -------------- | -| Acura | ILX 2016 | AcuraWatch Plus | Yes | Yes | 25mph* | 25mph | -| Acura | ILX 2017 | AcuraWatch Plus | Yes | Yes | 25mph* | 25mph | -| Acura | RDX 2018 | AcuraWatch Plus | Yes | Yes | 25mph* | 12mph | -| GM | Volt 2017 | Driver Confidence II | Yes | Yes | 0mph | 7mph | -| GM | Volt 2018 | Driver Confidence II | Yes | Yes | 0mph | 7mph | -| Honda | Accord 2018 | All | Yes | Stock | 0mph | 3mph | -| Honda | Civic 2016 | Honda Sensing | Yes | Yes | 0mph | 12mph | -| Honda | Civic 2017 | Honda Sensing | Yes | Yes | 0mph | 12mph | -| Honda | Civic 2017 *(Hatch)* | Honda Sensing | Yes | Stock | 0mph | 12mph | -| Honda | Civic 2018 | Honda Sensing | Yes | Yes | 0mph | 12mph | -| Honda | CR-V 2015 | Honda Sensing | Yes | Yes | 25mph* | 12mph | -| Honda | CR-V 2016 | Honda Sensing | Yes | Yes | 25mph* | 12mph | -| Honda | CR-V 2017 | Honda Sensing | Yes | Stock | 0mph | 12mph | -| Honda | CR-V 2018 | Honda Sensing | Yes | Stock | 0mph | 12mph | -| Honda | Odyssey 2017 | Honda Sensing | Yes | Yes | 25mph* | 12mph | -| Honda | Odyssey 2018 | Honda Sensing | Yes | Yes | 25mph* | 12mph | -| Honda | Pilot 2017 | Honda Sensing | Yes | Yes | 25mph* | 12mph | -| Honda | Pilot 2018 | Honda Sensing | Yes | Yes | 25mph* | 12mph | -| Honda | Ridgeline 2017 | Honda Sensing | Yes | Yes | 25mph* | 12mph | -| Honda | Ridgeline 2018 | Honda Sensing | Yes | Yes | 25mph* | 12mph | -| Lexus | RX Hybrid 2017 | All | Yes | Yes | 0mph | 0mph | -| Lexus | RX Hybrid 2018 | All | Yes | Yes | 0mph | 0mph | -| Toyota | Corolla 2017 | All | Yes | Yes | 20mph | 0mph | -| Toyota | Corolla 2018 | All | Yes | Yes | 20mph | 0mph | -| Toyota | Prius 2016 | TSS-P | Yes | Yes | 0mph | 0mph | -| Toyota | Prius 2017 | All | Yes | Yes | 0mph | 0mph | -| Toyota | Prius 2018 | All | Yes | Yes | 0mph | 0mph | -| Toyota | Prius Prime 2017 | All | Yes | Yes | 0mph | 0mph | -| Toyota | Prius Prime 2018 | All | Yes | Yes | 0mph | 0mph | -| Toyota | Rav4 2016 | TSS-P | Yes | Yes | 20mph | 0mph | -| Toyota | Rav4 2017 | All | Yes | Yes | 20mph | 0mph | -| Toyota | Rav4 2018 | All | Yes | Yes | 20mph | 0mph | -| Toyota | Rav4 Hybrid 2017 | All | Yes | Yes | 0mph | 0mph | -| Toyota | Rav4 Hybrid 2018 | All | Yes | Yes | 0mph | 0mph | - - -*[Comma Pedal](https://community.comma.ai/wiki/index.php/Comma_Pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. Here is how to [build a Comma Pedal](https://medium.com/@jfrux/comma-pedal-building-with-macrofab-6328bea791e8). ***NOTE: The Comma Pedal is not officially supported by [comma.ai](https://comma.ai)*** +| Make | Model | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below | +| ------- | ---------------------- | -------------------- | ------- | ------------ | -------------- | -------------- | +| Acura | ILX 2016 | AcuraWatch Plus | Yes | Yes | 25mph1| 25mph | +| Acura | ILX 2017 | AcuraWatch Plus | Yes | Yes | 25mph1| 25mph | +| Acura | RDX 2018 | AcuraWatch Plus | Yes | Yes | 25mph1| 12mph | +| GM3| Volt 2017 | Driver Confidence II | Yes | Yes | 0mph | 7mph | +| GM3| Volt 2018 | Driver Confidence II | Yes | Yes | 0mph | 7mph | +| Honda | Accord 2018 | All | Yes | Stock | 0mph | 3mph | +| Honda | Civic 2016 | Honda Sensing | Yes | Yes | 0mph | 12mph | +| Honda | Civic 2017 | Honda Sensing | Yes | Yes | 0mph | 12mph | +| Honda | Civic 2017 *(Hatch)* | Honda Sensing | Yes | Stock | 0mph | 12mph | +| Honda | Civic 2018 | Honda Sensing | Yes | Yes | 0mph | 12mph | +| Honda | CR-V 2015 | Honda Sensing | Yes | Yes | 25mph1| 12mph | +| Honda | CR-V 2016 | Honda Sensing | Yes | Yes | 25mph1| 12mph | +| Honda | CR-V 2017 | Honda Sensing | Yes | Stock | 0mph | 12mph | +| Honda | CR-V 2018 | Honda Sensing | Yes | Stock | 0mph | 12mph | +| Honda | Odyssey 2017 | Honda Sensing | Yes | Yes | 25mph1| 12mph | +| Honda | Odyssey 2018 | Honda Sensing | Yes | Yes | 25mph1| 12mph | +| Honda | Pilot 2017 | Honda Sensing | Yes | Yes | 25mph1| 12mph | +| Honda | Pilot 2018 | Honda Sensing | Yes | Yes | 25mph1| 12mph | +| Honda | Ridgeline 2017 | Honda Sensing | Yes | Yes | 25mph1| 12mph | +| Honda | Ridgeline 2018 | Honda Sensing | Yes | Yes | 25mph1| 12mph | +| Lexus | RX Hybrid 2017 | All | Yes | Yes2| 0mph | 0mph | +| Lexus | RX Hybrid 2018 | All | Yes | Yes2| 0mph | 0mph | +| Toyota | Camry 20184 | All | Yes | Stock | 0mph5 | 0mph | +| Toyota | C-HR 20184 | All | Yes | Stock | 0mph | 0mph | +| Toyota | Corolla 2017 | All | Yes | Yes2| 20mph | 0mph | +| Toyota | Corolla 2018 | All | Yes | Yes2| 20mph | 0mph | +| Toyota | Prius 2016 | TSS-P | Yes | Yes2| 0mph | 0mph | +| Toyota | Prius 2017 | All | Yes | Yes2| 0mph | 0mph | +| Toyota | Prius 2018 | All | Yes | Yes2| 0mph | 0mph | +| Toyota | Prius Prime 2017 | All | Yes | Yes2| 0mph | 0mph | +| Toyota | Prius Prime 2018 | All | Yes | Yes2| 0mph | 0mph | +| Toyota | Rav4 2016 | TSS-P | Yes | Yes2| 20mph | 0mph | +| Toyota | Rav4 2017 | All | Yes | Yes2| 20mph | 0mph | +| Toyota | Rav4 2018 | All | Yes | Yes2| 20mph | 0mph | +| Toyota | Rav4 Hybrid 2017 | All | Yes | Yes2| 0mph | 0mph | +| Toyota | Rav4 Hybrid 2018 | All | Yes | Yes2| 0mph | 0mph | + + +1[Comma Pedal](https://community.comma.ai/wiki/index.php/Comma_Pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. Here is how to [build a Comma Pedal](https://medium.com/@jfrux/comma-pedal-building-with-macrofab-6328bea791e8). ***NOTE: The Comma Pedal is not officially supported by [comma.ai](https://comma.ai)*** +2When disconnecting the Driver Support Unit (DSU), otherwise longitudinal control is stock ACC. For DSU locations, see [Toyota Wiki page](https://community.comma.ai/wiki/index.php/Toyota) +3[GM installation guide](https://www.zoneos.com/volt.htm) +4It needs an extra 120Ohm resistor ([pic1](https://i.imgur.com/CmdKtTP.jpg), [pic2](https://i.imgur.com/s2etUo6.jpg)) on bus 3 and giraffe switches set to 01X1 (11X1 for stock LKAS), where X depends on if you have the [comma power](https://comma.ai/shop/products/power/). +528mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control. Community Maintained Cars ------ -| Make | Model | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below | -| ------- | -----------------------| -------------------- | ------- | ------------ | ----------- | ------------ | -| Tesla | Model S 2012 | All | Yes | Not yet | Not applicable | 0mph | -| Tesla | Model S 2013 | All | Yes | Not yet | Not applicable | 0mph | +| Make | Model | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below | +| ------- | ---------------------- | -------------------- | ------- | ------------ | -------------- | -------------- | +| Honda | Fit 2018 | Honda Sensing | Yes | Yes | 25mph1| 12mph | -[[Tesla Pull Request]](https://github.com/commaai/openpilot/pull/246) +[[Honda Fit Pull Request]](https://github.com/commaai/openpilot/pull/266). -*Community Maintained Cars are not confirmed by comma.ai to meet our safety model. Be extra cautious using them. +Community Maintained Cars are not confirmed by comma.ai to meet our safety model. Be extra cautious using them. In Progress Cars ------ @@ -107,7 +112,7 @@ How can I add support for my car? If your car has adaptive cruise control and lane keep assist, you are in luck. Using a [panda](https://comma.ai/shop/products/panda-obd-ii-dongle/) and [cabana](https://community.comma.ai/cabana/), you can understand how to make your car drive by wire. -We've written a [porting guide](https://medium.com/@comma_ai/openpilot-port-guide-for-toyota-models-e5467f4b5fe6) for Toyota that might help you after you have the basics figured out. +We've written guides for [Brand](https://medium.com/@comma_ai/how-to-write-a-car-port-for-openpilot-7ce0785eda84) and [Model](https://medium.com/@comma_ai/openpilot-port-guide-for-toyota-models-e5467f4b5fe6) ports. These guides might help you after you have the basics figured out. - BMW, Audi, Volvo, and Mercedes all use [FlexRay](https://en.wikipedia.org/wiki/FlexRay) and are unlikely to be supported any time soon. - We put time into a Ford port, but the steering has a 10 second cutout limitation that makes it unusable. @@ -174,7 +179,7 @@ Contributing ------ We welcome both pull requests and issues on -[github](http://github.com/commaai/openpilot). Bug fixes and new car support encouraged. +[github](http://github.com/commaai/openpilot). Bug fixes and new car ports encouraged. Want to get paid to work on openpilot? [comma.ai is hiring](https://comma.ai/jobs/) diff --git a/RELEASES.md b/RELEASES.md index a515873642..0a81ac30ed 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,10 @@ +Version 0.5.1 (2018-08-01) +======================== + * Fix radar error on Civic sedan 2018 + * Improve thermal management logic + * Alpha Toyota C-HR and Camry support! + * Auto-switch Driver Monitoring to 3 min counter when inaccurate + Version 0.5 (2018-07-11) ======================== * Driver Monitoring (beta) option in settings! diff --git a/SAFETY.md b/SAFETY.md index 0c8ae7e51e..a2f77a5509 100644 --- a/SAFETY.md +++ b/SAFETY.md @@ -6,7 +6,10 @@ Like other ACC and LKA systems, openpilot requires the driver to be alert and to pay attention at all times. We repeat, **driver alertness is necessary, but not sufficient, for openpilot to be used safely**. -Even with an attentive driver, we must make further efforts for the system to be +In order to enforce driver alertness, openpilot includes a driver monitoring feature +that alerts the driver when distracted. + +However, even with an attentive driver, we must make further efforts for the system to be safe. We have designed openpilot with two other safety considerations. 1. The driver must always be capable to immediately retake manual control of the vehicle, @@ -20,7 +23,7 @@ Following are details of the car specific safety implementations: Honda/Acura ------ - - While the system is engaged, gas, brake and steer limits are subject to the same limits used by + - While the system is engaged, gas, brake and steer commands are subject to the same limits used by the stock system. - Without an interceptor, the gas is controlled by the Powertrain Control Module (PCM). @@ -28,16 +31,16 @@ Honda/Acura interceptor, the gas is clipped to 60%. - The brake is controlled by the 0x1FA CAN message. This message allows full - braking, although the board and the software clip it to 1/4th of the max. - This is around .3g of braking. + braking, although the panda firmware and openpilot clip it to 1/4th of the max. + This is approximately 0.3g of braking. - Steering is controlled by the 0xE4 CAN message. The Electronic Power Steering (EPS) controller in the car limits the torque to a very small amount, so regardless of the message, the controller cannot jerk the wheel. - Brake and gas pedal pressed signals are contained in the 0x17C CAN message. A rising edge of - either signal triggers a disengagement, which is enforced by the board and in software. The - green led on the board signifies if the board is allowing control messages. + either signals triggers a disengagement, which is enforced by the panda firmware and by openpilot. The + white led on the panda signifies if the panda is allowing control messages. - Honda CAN uses both a counter and a checksum to ensure integrity and prevent replay of the same message. @@ -45,29 +48,62 @@ Honda/Acura Toyota/Lexus ------ - - While the system is engaged, gas, brake and steer limits are subject to the same limits used by + - While the system is engaged, gas, brake and steer commands are subject to the same limits used by the stock system. - - With the stock Driving Support Unit (DSU) enabled, the acceleration is controlled - by the stock system and is subject to the stock adaptive cruise control limits. Without the - stock DSU connected, the acceleration command is controlled by the 0x343 CAN message and its - value is limited by the board and the software to between .3g of deceleration and .15g of - acceleration. The acceleration command is ignored by the Engine Control Module (ECM) while the - cruise control system is disengaged. - - - Steering torque is controlled through the 0x2E4 CAN message and it's limited by the board and in - software to a value of -1500 and 1500. In addition, the vehicle EPS unit will not respond to - commands outside these limits. A steering torque rate limit is enforced by the board and in - software so that the commanded steering torque must rise from 0 to max value no faster than - 1.5s. Commanded steering torque is limited by the board and in software to be no more than 350 + - With the stock Driving Support Unit (DSU) connected (or in DSU-less models like Camry and C-HR), + the acceleration is controlled by the stock system and is subject to the stock adaptive cruise + control limits. Without the stock DSU connected, the acceleration command is controlled by the + 0x343 CAN message and its value is limited between .3g of deceleration and .15g of acceleration + by the panda firmware and by openpilot. The acceleration command is ignored by the Engine Control + Module (ECM) while the cruise control system is disengaged. + + - Steering torque is controlled through the 0x2E4 CAN message and it's limited by the panda firmware and by + openpilot to a value between -1500 and 1500. In addition, the vehicle EPS unit will not respond to + commands outside these limits. A steering torque rate limit is enforced by the panda firmware and by + openpilot, so that the commanded steering torque must rise from 0 to max value no faster than + 1.5s. Commanded steering torque is limited by the panda firmware and by openpilot to be no more than 350 units above the actual EPS generated motor torque to ensure limited differences between commanded and actual torques. - Brake and gas pedal pressed signals are contained in the 0x224 and 0x1D2 CAN messages, - respectively. A rising edge of either signal triggers a disengagement, which is enforced by the - board and in software. Additionally, the cruise control system disengages on the rising edge of + respectively. A rising edge of either signals triggers a disengagement, which is enforced by the + panda firmware and by openpilot. Additionally, the cruise control system disengages on the rising edge of the brake pedal pressed signal. - The cruise control system state is contained in the 0x1D2 message. No control messages are - allowed if the cruise control system is not active. This is enforced by the software and the - board. The green led on the board signifies if the board is allowing control messages. + allowed if the cruise control system is not active. This is enforced by openpilot and the + panda firmware. The white led on the panda signifies if the panda is allowing control messages. + +GM/Chevrolet +------ + + - While the system is engaged, gas, brake and steer commands are subject to the same limits used by + the stock system. + + - The gas and regen are controlled by the 0x2CB message and it's limited by the panda firmware and by + openpilot to a value between 1404 and 3072. the minimum value correspond to a mild decel due to regen, + while 3072 correspond to approximately 0.18g of acceleration from stop. + + - The friction brakes are controlled by the 0x315 message and its value is limited by the panda firmware + and openpilot to 350. This is approximately 0.3g of braking. + + - Steering torque is controlled through the 0x180 CAN message and it's limited by the panda firmware and by + openpilot to a value between -255 and 255. In addition, the vehicle EPS unit will not fault when + commands outside these limits. A steering torque rate limit is enforced by the panda firmware and by + openpilot, so that the commanded steering torque must rise from 0 to max value no faster than + 0.75s. Commanded steering torque is gradually limited by the panda firmware and by openpilot if the driver's + torque exceeds 12 units in the opposite dicrection to ensure limited applied torque against the + driver's will. + + - Brake pedal and gas pedal potentiometer signals are contained in the 0xF1 and 0x1A1 CAN messages, + respectively. A rising edge of either signals triggers a disengagement, which is enforced by the + panda firmware and by openpilot. Additionally, the cruise control system disengages on the rising edge of + the brake pedal pressed signal. The regen paddle pressed signal is in the 0xBD message. When the + regen paddle is pressed, a disengagement is enforced by both the firmware and by openpilot. + + - GM CAN uses both a counter and a checksum to ensure integrity and prevent + replay of the same message. + +**Extra note"**: comma.ai strongly discourages the use of openpilot forks with safety code either missing or + not fully meeting the above requirements. diff --git a/apk/ai.comma.plus.frame.apk b/apk/ai.comma.plus.frame.apk index 8291050eee..9229bf3cb7 100644 --- a/apk/ai.comma.plus.frame.apk +++ b/apk/ai.comma.plus.frame.apk @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:db186a9f1e9d1564a6f82cf294eb1322224e4956a6781bfcd6e6acf40dcd92a8 -size 2126976 +oid sha256:89c2ebca16b5c6dae5e614a19e48cd02d1078da7bcf99c989a385b90ec3de915 +size 2586712 diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk index 018d5e9c39..138dc4a978 100644 --- a/apk/ai.comma.plus.offroad.apk +++ b/apk/ai.comma.plus.offroad.apk @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:dd7b7e0a5c84bef76d7f52be9dcf2641ee4ab966901deeff7228f638e112cbe6 -size 18180759 +oid sha256:ce1c366e0ea2473c3863a76ecc904fd259931d481e50e0ab783f2ca66115265f +size 18180696 diff --git a/cereal/car.capnp b/cereal/car.capnp index ffc79e6476..65ec3ee957 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -63,6 +63,11 @@ struct CarEvent @0x9b1657f34caf3ad3 { promptDriverDistracted @38; driverDistracted @39; geofence @40; + driverMonitorOn @41; + driverMonitorOff @42; + preDriverUnresponsive @43; + promptDriverUnresponsive @44; + driverUnresponsive @45; } } @@ -302,6 +307,8 @@ struct CarParams { hondaBosch @5; ford @6; cadillac @7; + hyundai @8; + chrysler @9; } # things about the car in the manual diff --git a/cereal/log.capnp b/cereal/log.capnp index 474c0d2410..fbb00a2a19 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -21,6 +21,8 @@ struct Map(Key, Value) { struct InitData { kernelArgs @0 :List(Text); + kernelVersion @15 :Text; + gctx @1 :Text; dongleId @2 :Text; @@ -32,6 +34,7 @@ struct InitData { androidBuildInfo @5 :AndroidBuildInfo; androidSensors @6 :List(AndroidSensor); + androidProperties @16 :Map(Text, Text); chffrAndroidExtra @7 :ChffrAndroidExtra; iosBuildInfo @14 :IosBuildInfo; @@ -399,6 +402,7 @@ struct Live100Data { angleOffset @27 :Float32; gpsPlannerActive @40 :Bool; engageable @41 :Bool; # can OP be engaged? + driverMonitoringOn @43 :Bool; enum ControlState { disabled @0; @@ -1533,6 +1537,13 @@ struct OrbKeyFrame { struct DriverMonitoring { frameId @0 :UInt32; descriptor @1 :List(Float32); + std @2 :Float32; +} + +struct Boot { + wallTimeNanos @0 :UInt64; + lastKmsg @1 :Data; + lastPmsg @2 :Data; } struct Event { @@ -1599,5 +1610,6 @@ struct Event { uiLayoutState @57 :UiLayoutState; orbFeaturesSummary @58 :OrbFeaturesSummary; driverMonitoring @59 :DriverMonitoring; + boot @60 :Boot; } } diff --git a/common/transformations/coordinates.py b/common/transformations/coordinates.py index 1cd7a8e9d5..568fb9bf2b 100644 --- a/common/transformations/coordinates.py +++ b/common/transformations/coordinates.py @@ -12,12 +12,14 @@ esq = 6.69437999014 * 0.001 e1sq = 6.73949674228 * 0.001 -def geodetic2ecef(geodetic): +def geodetic2ecef(geodetic, radians=False): geodetic = np.array(geodetic) input_shape = geodetic.shape geodetic = np.atleast_2d(geodetic) - lat = (np.pi/180)*geodetic[:,0] - lon = (np.pi/180)*geodetic[:,1] + + ratio = 1.0 if radians else (np.pi / 180.0) + lat = ratio*geodetic[:,0] + lon = ratio*geodetic[:,1] alt = geodetic[:,2] xi = np.sqrt(1 - esq * np.sin(lat)**2) @@ -28,41 +30,41 @@ def geodetic2ecef(geodetic): return ecef.reshape(input_shape) -def ecef2geodetic(ecef): +def ecef2geodetic(ecef, radians=False): """ Convert ECEF coordinates to geodetic using ferrari's method """ - def ferrari(x, y, z): - # ferrari's method - r = np.sqrt(x * x + y * y) - Esq = a * a - b * b - F = 54 * b * b * z * z - G = r * r + (1 - esq) * z * z - esq * Esq - C = (esq * esq * F * r * r) / (pow(G, 3)) - S = np.cbrt(1 + C + np.sqrt(C * C + 2 * C)) - P = F / (3 * pow((S + 1 / S + 1), 2) * G * G) - Q = np.sqrt(1 + 2 * esq * esq * P) - r_0 = -(P * esq * r) / (1 + Q) + np.sqrt(0.5 * a * a*(1 + 1.0 / Q) - \ - P * (1 - esq) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r) - U = np.sqrt(pow((r - esq * r_0), 2) + z * z) - V = np.sqrt(pow((r - esq * r_0), 2) + (1 - esq) * z * z) - Z_0 = b * b * z / (a * V) - h = U * (1 - b * b / (a * V)) - lat = (180/np.pi)*np.arctan((z + e1sq * Z_0) / r) - lon = (180/np.pi)*np.arctan2(y, x) - return lat, lon, h - - geodetic = [] - ecef = np.array(ecef) + # Save shape and export column + ecef = np.atleast_1d(ecef) input_shape = ecef.shape ecef = np.atleast_2d(ecef) - for p in ecef: - geodetic.append(ferrari(*p)) - geodetic = np.array(geodetic) + x, y, z = ecef[:, 0], ecef[:, 1], ecef[:, 2] + + ratio = 1.0 if radians else (180.0 / np.pi) + + # Conver from ECEF to geodetic using Ferrari's methods + # https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#Ferrari.27s_solution + r = np.sqrt(x * x + y * y) + Esq = a * a - b * b + F = 54 * b * b * z * z + G = r * r + (1 - esq) * z * z - esq * Esq + C = (esq * esq * F * r * r) / (pow(G, 3)) + S = np.cbrt(1 + C + np.sqrt(C * C + 2 * C)) + P = F / (3 * pow((S + 1 / S + 1), 2) * G * G) + Q = np.sqrt(1 + 2 * esq * esq * P) + r_0 = -(P * esq * r) / (1 + Q) + np.sqrt(0.5 * a * a*(1 + 1.0 / Q) - \ + P * (1 - esq) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r) + U = np.sqrt(pow((r - esq * r_0), 2) + z * z) + V = np.sqrt(pow((r - esq * r_0), 2) + (1 - esq) * z * z) + Z_0 = b * b * z / (a * V) + h = U * (1 - b * b / (a * V)) + lat = ratio*np.arctan((z + e1sq * Z_0) / r) + lon = ratio*np.arctan2(y, x) + + # stack the new columns and return to the original shape + geodetic = np.column_stack((lat, lon, h)) return geodetic.reshape(input_shape) - - class LocalCoord(object): """ Allows conversions to local frames. In this case NED. diff --git a/selfdrive/assets/img_driver_face.png b/selfdrive/assets/img_driver_face.png new file mode 100644 index 0000000000..271c4e4dd0 --- /dev/null +++ b/selfdrive/assets/img_driver_face.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9a9b85f959e3881c65d018486ae2a6c41b25b4586cc61b0caad27d211fb517d9 +size 24112 diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 6a64183425..1f0e36e0dc 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -1,6 +1,6 @@ #!/usr/bin/env python from common.realtime import sec_since_boot -from cereal import car +from cereal import car, log from selfdrive.swaglog import cloudlog from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event @@ -209,7 +209,7 @@ class CarInterface(object): # pass in a car.CarControl # to be called @ 100hz - def apply(self, c): + def apply(self, c, perception_state=log.Live20Data.new_message()): self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, c.actuators, c.hudControl.visualAlert, c.cruiseControl.cancel) diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 59b6e35c74..7c0c63f546 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -4,7 +4,8 @@ from common.kalman.simple_kalman import KF1D from selfdrive.config import Conversions as CV from selfdrive.can.parser import CANParser from selfdrive.car.gm.values import DBC, CAR, parse_gear_shifter, \ - CruiseButtons, is_eps_status_ok + CruiseButtons, is_eps_status_ok, \ + STEER_THRESHOLD def get_powertrain_can_parser(CP, canbus): # this function generates lists for signal, messages and initial values @@ -91,7 +92,7 @@ class CarState(object): self.user_gas_pressed = self.pedal_gas > 0 self.steer_torque_driver = pt_cp.vl["PSCMStatus"]['LKADriverAppldTrq'] - self.steer_override = abs(self.steer_torque_driver) > 1.0 + self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD # 0 - inactive, 1 - active, 2 - temporary limited, 3 - failed self.lkas_status = pt_cp.vl["PSCMStatus"]['LKATorqueDeliveredStatus'] diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 53dc8b68e7..3d0a49feee 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -1,5 +1,5 @@ #!/usr/bin/env python -from cereal import car +from cereal import car, log from common.realtime import sec_since_boot from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET @@ -308,7 +308,7 @@ class CarInterface(object): # pass in a car.CarControl # to be called @ 100hz - def apply(self, c): + def apply(self, c, perception_state=log.Live20Data.new_message()): hud_v_cruise = c.hudControl.setSpeed if hud_v_cruise > 70: hud_v_cruise = 0 diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index 314bb4a3f6..b69109f356 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -47,6 +47,7 @@ FINGERPRINTS = { }], } +STEER_THRESHOLD = 1.0 DBC = { CAR.VOLT: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 0875a67234..b1b72321ef 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -1,3 +1,4 @@ +from cereal import car from collections import namedtuple from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.controls.lib.drive_helpers import rate_limit @@ -61,11 +62,12 @@ class CarController(object): self.brake_last = 0. self.enable_camera = enable_camera self.packer = CANPacker(dbc_name) + self.new_radar_config = False def update(self, sendcan, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ - hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \ - snd_beep, snd_chime): + radar_error, hud_v_cruise, hud_show_lanes, hud_show_car, \ + hud_alert, snd_beep, snd_chime): """ Controls thread """ @@ -167,6 +169,8 @@ class CarController(object): if (frame % radar_send_step) == 0: idx = (frame/radar_send_step) % 4 - can_sends.extend(hondacan.create_radar_commands(CS.v_ego, CS.CP.carFingerprint, idx)) + if not self.new_radar_config: # only change state once + self.new_radar_config = car.RadarState.Error.wrongConfig in radar_error + can_sends.extend(hondacan.create_radar_commands(CS.v_ego, CS.CP.carFingerprint, self.new_radar_config, idx)) sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes()) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 36dea1f7ff..466259c20a 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -2,7 +2,7 @@ from common.numpy_fast import interp from common.kalman.simple_kalman import KF1D from selfdrive.can.parser import CANParser from selfdrive.config import Conversions as CV -from selfdrive.car.honda.values import CAR, DBC +from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD def parse_gear_shifter(can_gear_shifter, car_fingerprint): @@ -284,10 +284,8 @@ class CarState(object): else: self.car_gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS'] - #rdx has different steer override threshold - steer_thrsld = 400 if self.CP.carFingerprint == CAR.ACURA_RDX else 1200 - self.steer_override = abs(cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR']) > steer_thrsld self.steer_torque_driver = cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR'] + self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD[self.CP.carFingerprint] self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index 92e74cd686..8dc5e1d9b5 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -117,7 +117,7 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, idx): return commands -def create_radar_commands(v_ego, car_fingerprint, idx): +def create_radar_commands(v_ego, car_fingerprint, new_radar_config, idx): """Creates an iterable of CAN messages for the radar system.""" commands = [] v_ego_kph = np.clip(int(round(v_ego * CV.MS_TO_KPH)), 0, 255) @@ -129,7 +129,8 @@ def create_radar_commands(v_ego, car_fingerprint, idx): if car_fingerprint == CAR.CIVIC: msg_0x301 = "\x02\x38\x44\x32\x4f\x00\x00" - commands.append(make_can_msg(0x300, msg_0x300, idx + 8, 1)) # add 8 on idx. + idx_offset = 0xc if new_radar_config else 0x8 # radar in civic 2018 requires 0xc + commands.append(make_can_msg(0x300, msg_0x300, idx + idx_offset, 1)) else: if car_fingerprint == CAR.CRV: msg_0x301 = "\x00\x00\x50\x02\x51\x00\x00" diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index db02a9725a..4cac8c092a 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -1,7 +1,7 @@ #!/usr/bin/env python import os import numpy as np -from cereal import car +from cereal import car, log from common.numpy_fast import clip, interp from common.realtime import sec_since_boot from selfdrive.swaglog import cloudlog @@ -567,7 +567,7 @@ class CarInterface(object): # pass in a car.CarControl # to be called @ 100hz - def apply(self, c): + def apply(self, c, perception_state=log.Live20Data.new_message()): if c.hudControl.speedVisible: hud_v_cruise = c.hudControl.setSpeed * CV.MS_TO_KPH else: @@ -600,6 +600,7 @@ class CarInterface(object): c.cruiseControl.override, \ c.cruiseControl.cancel, \ pcm_accel, \ + perception_state.radarErrors, \ hud_v_cruise, c.hudControl.lanesVisible, \ hud_show_car = c.hudControl.leadVisible, \ hud_alert = hud_alert, \ diff --git a/selfdrive/car/honda/radar_interface.py b/selfdrive/car/honda/radar_interface.py index 3081474487..7d30265c2f 100755 --- a/selfdrive/car/honda/radar_interface.py +++ b/selfdrive/car/honda/radar_interface.py @@ -28,6 +28,7 @@ class RadarInterface(object): self.pts = {} self.track_id = 0 self.radar_fault = False + self.radar_wrong_config = False self.radar_off_can = CP.radarOffCan self.delay = 0.1 # Delay of radar @@ -61,6 +62,7 @@ class RadarInterface(object): if ii == 0x400: # check for radar faults self.radar_fault = cpt['RADAR_STATE'] != 0x79 + self.radar_wrong_config = cpt['RADAR_STATE'] == 0x69 elif cpt['LONG_DIST'] < 255: if ii not in self.pts or cpt['NEW_TRACK']: self.pts[ii] = car.RadarState.RadarPoint.new_message() @@ -81,6 +83,8 @@ class RadarInterface(object): errors.append("commIssue") if self.radar_fault: errors.append("fault") + if self.radar_wrong_config: + errors.append("wrongConfig") ret.errors = errors ret.canMonoTimes = canMonoTimes diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index 71bb495e26..5188ca576e 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -104,5 +104,19 @@ DBC = { CAR.RIDGELINE: dbc_dict('honda_ridgeline_black_edition_2017_can_generated', 'acura_ilx_2016_nidec'), } + +STEER_THRESHOLD = { + CAR.ACCORD: 1200, + CAR.ACURA_ILX: 1200, + CAR.ACURA_RDX: 400, + CAR.CIVIC: 1200, + CAR.CIVIC_HATCH: 1200, + CAR.CRV: 1200, + CAR.CRV_5G: 1200, + CAR.ODYSSEY: 1200, + CAR.PILOT: 1200, + CAR.RIDGELINE: 1200, +} + # TODO: get these from dbc file HONDA_BOSCH = [CAR.ACCORD, CAR.CIVIC_HATCH, CAR.CRV_5G] diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index db3b8b6a53..5edbcbbd11 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -1,6 +1,6 @@ #!/usr/bin/env python import zmq -from cereal import car +from cereal import car, log from selfdrive.config import Conversions as CV from selfdrive.services import service_list from selfdrive.swaglog import cloudlog @@ -117,6 +117,6 @@ class CarInterface(object): return ret.as_reader() - def apply(self, c): + def apply(self, c, perception_state=log.Live20Data.new_message()): # in mock no carcontrols return False diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 7752add1b2..a8e317b1eb 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -4,7 +4,7 @@ from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\ create_steer_command, create_ui_command, \ create_ipas_steer_command, create_accel_command, \ create_fcw_command -from selfdrive.car.toyota.values import ECU, STATIC_MSGS +from selfdrive.car.toyota.values import ECU, STATIC_MSGS, NO_DSU_CAR from selfdrive.can.packer import CANPacker # Accel limits @@ -209,7 +209,7 @@ class CarController(object): else: can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False)) - if frame % 10 == 0 and ECU.CAM in self.fake_ecus: + if frame % 10 == 0 and ECU.CAM in self.fake_ecus and self.car_fingerprint not in NO_DSU_CAR: for addr in TARGET_IDS: can_sends.append(create_video_target(frame/10, addr)) diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 28bfc0fb57..53ff3aaa8a 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -1,4 +1,4 @@ -from selfdrive.car.toyota.values import CAR, DBC +from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD from selfdrive.can.parser import CANParser from selfdrive.config import Conversions as CV from common.kalman.simple_kalman import KF1D @@ -7,7 +7,7 @@ import numpy as np def parse_gear_shifter(can_gear, car_fingerprint): # TODO: Use values from DBC to parse this field - if car_fingerprint == CAR.PRIUS: + if car_fingerprint in [CAR.PRIUS, CAR.CHRH, CAR.CAMRYH]: if can_gear == 0x0: return "park" elif can_gear == 0x1: @@ -19,7 +19,7 @@ def parse_gear_shifter(can_gear, car_fingerprint): elif can_gear == 0x4: return "brake" elif car_fingerprint in [CAR.RAV4, CAR.RAV4H, - CAR.LEXUS_RXH, CAR.COROLLA]: + CAR.LEXUS_RXH, CAR.COROLLA, CAR.CHR, CAR.CAMRY]: if can_gear == 0x20: return "park" elif can_gear == 0x10: @@ -147,8 +147,6 @@ class CarState(object): self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1 self.right_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2 - # we could use the override bit from dbc, but it's triggered at too high torque values - self.steer_override = abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']) > 100 # 2 is standby, 10 is active. TODO: check that everything else is really a faulty state self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE'] self.steer_error = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [1, 5] @@ -156,6 +154,8 @@ class CarState(object): self.brake_error = 0 self.steer_torque_driver = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER'] self.steer_torque_motor = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_EPS'] + # we could use the override bit from dbc, but it's triggered at too high torque values + self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD self.user_brake = 0 self.v_cruise_pcm = cp.vl["PCM_CRUISE_2"]['SET_SPEED'] diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 0170c4c942..6a15859781 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -1,6 +1,6 @@ #!/usr/bin/env python from common.realtime import sec_since_boot -from cereal import car +from cereal import car, log from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event from selfdrive.controls.lib.vehicle_model import VehicleModel @@ -75,8 +75,8 @@ class CarInterface(object): if candidate == CAR.PRIUS: ret.safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.70 - ret.steerRatio = 15.59 # unknown end-to-end spec - tire_stiffness_factor = 0.7933 + ret.steerRatio = 15.00 # unknown end-to-end spec + tire_stiffness_factor = 0.6371 # hand-tune ret.mass = 3045 * CV.LB_TO_KG + std_cargo ret.steerKpV, ret.steerKiV = [[0.4], [0.01]] ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 @@ -110,6 +110,24 @@ class CarInterface(object): ret.steerKpV, ret.steerKiV = [[0.6], [0.1]] ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 + elif candidate in [CAR.CHR, CAR.CHRH]: + ret.safetyParam = 100 + ret.wheelbase = 2.63906 + ret.steerRatio = 13.6 + tire_stiffness_factor = 0.7933 + ret.mass = 3300. * CV.LB_TO_KG + std_cargo + ret.steerKpV, ret.steerKiV = [[0.723], [0.0428]] + ret.steerKf = 0.00006 + + elif candidate in [CAR.CAMRY, CAR.CAMRYH]: + ret.safetyParam = 100 + ret.wheelbase = 2.82448 + ret.steerRatio = 13.7 + tire_stiffness_factor = 0.7933 + ret.mass = 3400 * CV.LB_TO_KG + std_cargo #mean between normal and hybrid + ret.steerKpV, ret.steerKiV = [[0.6], [0.1]] + ret.steerKf = 0.00006 + ret.steerRateCost = 1. ret.centerToFront = ret.wheelbase * 0.44 @@ -118,7 +136,7 @@ class CarInterface(object): # min speed to enable ACC. if car can do stop and go, then set enabling speed # to a negative value, so it won't matter. - if candidate in [CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH]: # rav4 hybrid can do stop and go + if candidate in [CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH]: # rav4 hybrid can do stop and go ret.minEnableSpeed = -1. elif candidate in [CAR.RAV4, CAR.COROLLA]: # TODO: hack ICE to do stop and go ret.minEnableSpeed = 19. * CV.MPH_TO_MS @@ -150,9 +168,9 @@ class CarInterface(object): ret.brakeMaxBP = [5., 20.] ret.brakeMaxV = [1., 0.8] - ret.enableCamera = not check_ecu_msgs(fingerprint, candidate, ECU.CAM) - ret.enableDsu = not check_ecu_msgs(fingerprint, candidate, ECU.DSU) - ret.enableApgs = False #not check_ecu_msgs(fingerprint, candidate, ECU.APGS) + ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM) + ret.enableDsu = not check_ecu_msgs(fingerprint, ECU.DSU) + ret.enableApgs = False #not check_ecu_msgs(fingerprint, ECU.APGS) cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera) cloudlog.warn("ECU DSU Simulated: %r", ret.enableDsu) cloudlog.warn("ECU APGS Simulated: %r", ret.enableApgs) @@ -304,7 +322,7 @@ class CarInterface(object): # pass in a car.CarControl # to be called @ 100hz - def apply(self, c): + def apply(self, c, perception_state=log.Live20Data.new_message()): self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert, diff --git a/selfdrive/car/toyota/radar_interface.py b/selfdrive/car/toyota/radar_interface.py index 9af6aed47b..25bbfbfb39 100755 --- a/selfdrive/car/toyota/radar_interface.py +++ b/selfdrive/car/toyota/radar_interface.py @@ -1,11 +1,13 @@ #!/usr/bin/env python import os +import zmq +import time from selfdrive.can.parser import CANParser from cereal import car from common.realtime import sec_since_boot -import zmq from selfdrive.services import service_list import selfdrive.messaging as messaging +from selfdrive.car.toyota.values import NO_DSU_CAR RADAR_MSGS = list(range(0x210, 0x220)) @@ -30,15 +32,21 @@ class RadarInterface(object): self.delay = 0.0 # Delay of radar - # Nidec self.rcp = _create_radard_can_parser() + self.no_dsu_car = CP.carFingerprint in NO_DSU_CAR context = zmq.Context() self.logcan = messaging.sub_sock(context, service_list['can'].port) def update(self): - canMonoTimes = [] + ret = car.RadarState.new_message() + if self.no_dsu_car: + # TODO: make a adas dbc file for dsu-less models + time.sleep(0.05) + return ret + + canMonoTimes = [] updated_messages = set() while 1: tm = int(sec_since_boot() * 1e9) @@ -47,7 +55,6 @@ class RadarInterface(object): if 0x21f in updated_messages: break - ret = car.RadarState.new_message() errors = [] if not self.rcp.can_valid: errors.append("commIssue") diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 649146073f..2e0cf3e6fe 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -2,10 +2,14 @@ from selfdrive.car import dbc_dict class CAR: PRIUS = "TOYOTA PRIUS 2017" - RAV4H = "TOYOTA RAV4 2017 HYBRID" + RAV4H = "TOYOTA RAV4 HYBRID 2017" RAV4 = "TOYOTA RAV4 2017" COROLLA = "TOYOTA COROLLA 2017" LEXUS_RXH = "LEXUS RX HYBRID 2017" + CHR = "TOYOTA C-HR 2018" + CHRH = "TOYOTA C-HR HYBRID 2018" + CAMRY = "TOYOTA CAMRY 2018" + CAMRYH = "TOYOTA CAMRY HYBRID 2018" class ECU: @@ -59,14 +63,16 @@ STATIC_MSGS = [(0x141, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, (0x470, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 1, 100, '\x00\x00\x02\x7a'), ] +ECU_FINGERPRINT = { + ECU.CAM: 0x2e4, # steer torque cmd + ECU.DSU: 0x343, # accel cmd + ECU.APGS: 0x835, # angle cmd +} -def check_ecu_msgs(fingerprint, candidate, ecu): - # return True if fingerprint contains messages normally sent by a given ecu - ecu_msgs = [x[0] for x in STATIC_MSGS if (x[1] == ecu and - candidate in x[2] and - x[3] == 0)] - return any(msg for msg in fingerprint if msg in ecu_msgs) +def check_ecu_msgs(fingerprint, ecu): + # return True if fingerprint contains messages normally sent by a given ecu + return ECU_FINGERPRINT[ecu] in fingerprint FINGERPRINTS = { @@ -97,8 +103,25 @@ FINGERPRINTS = { CAR.LEXUS_RXH: [{ 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: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 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, 975: 6, 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, 1059: 1, 1063: 8, 1071: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 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, 1575: 8, 1595: 8, 1777: 8, 1779: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1956: 8, 1960: 8, 1964: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8, 2012: 8 }], + CAR.CHR: [{ + 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 705: 8, 740: 5, 800: 8, 810: 2, 812: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 921: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 1017: 8, 1020: 8, 1021: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8 + }], + CAR.CHRH: [{ + 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, 643: 7, 713: 8, 740: 5, 800: 8, 810: 2, 812: 8, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 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, 1014: 8, 1017: 8, 1020: 8, 1021: 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, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556:8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 + }], + CAR.CAMRY: [ + #XLE and LE + { + 36: 8, 37: 8, 119: 6, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 891: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 + }], + CAR.CAMRYH: [ + #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, 643: 7, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 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, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 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, 1235: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572:8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 + }], } +STEER_THRESHOLD = 100 DBC = { CAR.RAV4H: dbc_dict('toyota_rav4_hybrid_2017_pt_generated', 'toyota_prius_2017_adas'), @@ -106,4 +129,10 @@ DBC = { CAR.PRIUS: dbc_dict('toyota_prius_2017_pt_generated', 'toyota_prius_2017_adas'), CAR.COROLLA: dbc_dict('toyota_corolla_2017_pt_generated', 'toyota_prius_2017_adas'), CAR.LEXUS_RXH: dbc_dict('lexus_rx_hybrid_2017_pt_generated', 'toyota_prius_2017_adas'), + CAR.CHR: dbc_dict('toyota_chr_2018_pt_generated', 'toyota_prius_2017_adas'), + CAR.CHRH: dbc_dict('toyota_chr_hybrid_2018_pt_generated', 'toyota_prius_2017_adas'), + CAR.CAMRY: dbc_dict('toyota_chr_2018_pt_generated', 'toyota_prius_2017_adas'), + CAR.CAMRYH: dbc_dict('toyota_camry_hybrid_2018_pt_generated', 'toyota_prius_2017_adas'), } + +NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH] diff --git a/selfdrive/common/efd.c b/selfdrive/common/efd.c new file mode 100644 index 0000000000..78a7c09894 --- /dev/null +++ b/selfdrive/common/efd.c @@ -0,0 +1,56 @@ +#include +#include + +#ifdef __linux__ +#include +#else +#include +#include +#define EVENT_IDENT 42 +#endif + +#include "efd.h" + + +int efd_init() { +#ifdef __linux__ + return eventfd(0, EFD_CLOEXEC); +#else + int fd = kqueue(); + assert(fd >= 0); + + struct kevent kev; + EV_SET(&kev, EVENT_IDENT, EVFILT_USER, EV_ADD | EV_CLEAR, 0, 0, NULL); + + struct timespec timeout = {0, 0}; + int err = kevent(fd, &kev, 1, NULL, 0, &timeout); + assert(err != -1); + + return fd; +#endif +} + +void efd_write(int fd) { +#ifdef __linux__ + eventfd_write(fd, 1); +#else + struct kevent kev; + EV_SET(&kev, EVENT_IDENT, EVFILT_USER, 0, NOTE_TRIGGER, 0, NULL); + + struct timespec timeout = {0, 0}; + int err = kevent(fd, &kev, 1, NULL, 0, &timeout); + assert(err != -1); +#endif +} + +void efd_clear(int fd) { +#ifdef __linux__ + eventfd_t efd_cnt; + eventfd_read(fd, &efd_cnt); +#else + struct kevent kev; + struct timespec timeout = {0, 0}; + int nfds = kevent(fd, NULL, 0, &kev, 1, &timeout); + assert(nfds != -1); +#endif +} diff --git a/selfdrive/common/efd.h b/selfdrive/common/efd.h new file mode 100644 index 0000000000..056482ffa5 --- /dev/null +++ b/selfdrive/common/efd.h @@ -0,0 +1,17 @@ +#ifndef EFD_H +#define EFD_H + +#ifdef __cplusplus +extern "C" { +#endif + +// event fd: a semaphore that can be poll()'d +int efd_init(); +void efd_write(int fd); +void efd_clear(int fd); + +#ifdef __cplusplus +} +#endif + +#endif \ No newline at end of file diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 2b226fc6e0..664167ebce 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.5-release" +#define COMMA_VERSION "0.5.1-release" diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 1b8ad21282..f75f4c70a1 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -292,7 +292,7 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, return actuators, v_cruise_kph, driver_status, angle_offset -def data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, +def data_send(perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol, live100, livempc, AM, driver_status, LaC, LoC, angle_offset, passive): @@ -323,7 +323,7 @@ def data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_ CC.hudControl.audibleAlert = AM.audible_alert # send car controls over can - CI.apply(CC) + CI.apply(CC, perception_state) # ***** publish state to logger ***** # publish controls state at 100Hz @@ -337,6 +337,7 @@ def data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_ "alertStatus": AM.alert_status, "alertBlinkingRate": AM.alert_rate, "awarenessStatus": max(driver_status.awareness, 0.0) if isEnabled(state) else 0.0, + "driverMonitoringOn": bool(driver_status.monitor_on), "canMonoTimes": list(CS.canMonoTimes), "planMonoTime": plan_ts, "enabled": isEnabled(state), @@ -446,7 +447,6 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.): CP.safetyModel = car.CarParams.SafetyModels.noOutput fcw_enabled = params.get("IsFcwEnabled") == "1" - driver_monitor_on = params.get("IsDriverMonitoringEnabled") == "1" geofence = None try: from selfdrive.controls.lib.geofence import Geofence @@ -460,7 +460,7 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.): VM = VehicleModel(CP) LaC = LatControl(VM) AM = AlertManager() - driver_status = DriverStatus(driver_monitor_on) + driver_status = DriverStatus() if not passive: AM.add("startup", False) @@ -516,7 +516,7 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.): prof.checkpoint("State Control") # publish data - CC = data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol, + CC = data_send(PL.perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol, live100, livempc, AM, driver_status, LaC, LoC, angle_offset, passive) prof.checkpoint("Sent") diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py index 9d053c211f..2fd716c3d2 100644 --- a/selfdrive/controls/lib/alertmanager.py +++ b/selfdrive/controls/lib/alertmanager.py @@ -74,7 +74,7 @@ class AlertManager(object): Priority.MID, None, "beepSingle", .2, 0., 0.), "fcw": Alert( - "Brake!", + "BRAKE!", "Risk of Collision", AlertStatus.critical, AlertSize.full, Priority.HIGHEST, "fcw", "chimeRepeated", 1., 2., 2.), @@ -98,23 +98,53 @@ class AlertManager(object): Priority.LOW, None, None, .2, .2, .2), "preDriverDistracted": Alert( - "TAKE CONTROL: User Appears Distracted", + "KEEP EYES ON ROAD: User Appears Distracted", "", AlertStatus.normal, AlertSize.small, Priority.LOW, "steerRequired", None, 0., .1, .1, alert_rate=0.75), "promptDriverDistracted": Alert( - "TAKE CONTROL", + "KEEP EYES ON ROAD", "User Appears Distracted", AlertStatus.userPrompt, AlertSize.mid, Priority.MID, "steerRequired", "chimeRepeated", .1, .1, .1), "driverDistracted": Alert( - "DISENGAGEMENT REQUIRED", + "DISENGAGE IMMEDIATELY", "User Was Distracted", AlertStatus.critical, AlertSize.full, Priority.HIGH, "steerRequired", "chimeRepeated", .1, .1, .1), + "preDriverUnresponsive": Alert( + "TOUCH STEERING WHEEL: No Driver Monitoring", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, "steerRequired", None, 0., .1, .1, alert_rate=0.75), + + "promptDriverUnresponsive": Alert( + "TOUCH STEERING WHEEL", + "User Is Unresponsive", + AlertStatus.userPrompt, AlertSize.mid, + Priority.MID, "steerRequired", "chimeRepeated", .1, .1, .1), + + "driverUnresponsive": Alert( + "DISENGAGE IMMEDIATELY", + "User Was Unresponsive", + AlertStatus.critical, AlertSize.full, + Priority.HIGH, "steerRequired", "chimeRepeated", .1, .1, .1), + + "driverMonitorOff": Alert( + "DRIVER MONITOR IS UNAVAILABLE", + "Accuracy Is Low", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, None, None, .4, 0., 4.), + + "driverMonitorOn": Alert( + "DRIVER MONITOR IS AVAILABLE", + "Accuracy Is High", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, None, None, .4, 0., 4.), + "geofence": Alert( "DISENGAGEMENT REQUIRED", "Not in Geofenced Area", diff --git a/selfdrive/controls/lib/driver_monitor.py b/selfdrive/controls/lib/driver_monitor.py index a593f1091d..a70eb49845 100644 --- a/selfdrive/controls/lib/driver_monitor.py +++ b/selfdrive/controls/lib/driver_monitor.py @@ -18,11 +18,21 @@ _CAMERA_X_CONV = 0.375 # 160*864/320/1152 _PITCH_WEIGHT = 1.5 # pitch matters a lot more _METRIC_THRESHOLD = 0.4 _PITCH_POS_ALLOWANCE = 0.08 # rad, to not be too sensitive on positive pitch -_DTM = 0.2 # driver monitor runs at 5Hz -_DISTRACTED_FILTER_F = 0.3 # 0.3Hz -_DISTRACTED_FILTER_K = 2 * np.pi * _DISTRACTED_FILTER_F * _DTM / (1 + 2 * np.pi * _DISTRACTED_FILTER_F * _DTM) +_DTM = 0.1 # driver monitor runs at 10Hz _PITCH_NATURAL_OFFSET = 0.1 # people don't seem to look straight when they drive relaxed, rather a bit up +_STD_THRESHOLD = 0.1 # above this standard deviation consider the measurement invalid +_DISTRACTED_FILTER_F = 0.6 # 0.6Hz, 0.25s ts +_DISTRACTED_FILTER_K = 2 * np.pi * _DISTRACTED_FILTER_F * _DTM / (1 + 2 * np.pi * _DISTRACTED_FILTER_F * _DTM) +_VARIANCE_FILTER_F = 0.008 # 0.008Hz, 20s ts +_VARIANCE_FILTER_K = 2 * np.pi * _VARIANCE_FILTER_F * _DTM / (1 + 2 * np.pi * _VARIANCE_FILTER_F * _DTM) + +class MonitorChangeEvent: + NO_CHANGE = 0 + PARAM_ON = 1 + PARAM_OFF = 2 + VARIANCE_HIGH = 3 + VARIANCE_LOW = 4 class _DriverPose(): def __init__(self): @@ -32,16 +42,30 @@ class _DriverPose(): self.yaw_offset = 0. self.pitch_offset = 0. +def _monitor_hysteresys(variance_level, monitor_valid_prev): + var_thr = 0.63 if monitor_valid_prev else 0.37 + return variance_level < var_thr + class DriverStatus(): - def __init__(self, monitor_on): + def __init__(self, monitor_on=False): self.pose = _DriverPose() self.monitor_on = monitor_on + self.monitor_param_on = monitor_on + self.monitor_valid = True # variance needs to be low + self.monitor_change_event = MonitorChangeEvent.NO_CHANGE self.awareness = 1. self.driver_distracted = False self.driver_distraction_level = 0. + self.variance_high = False + self.variance_level = 0. self.ts_last_check = 0. self._set_timers() + def _reset_filters(self): + self.driver_distraction_level = 0. + self.variance_level = 0. + self.monitor_valid = True + def _set_timers(self): if self.monitor_on: self.threshold_pre = _DISTRACTED_PRE_TIME / _DISTRACTED_TIME @@ -64,14 +88,23 @@ class DriverStatus(): #print "%02.4f" % np.degrees(pose.pitch), "%02.4f" % np.degrees(pitch_error), "%03.4f" % np.degrees(pose.pitch_offset), metric return 1 if metric > _METRIC_THRESHOLD else 0 - def get_pose(self, driver_monitoring, params): - ts = sec_since_boot() - # don's check for param too often as it's a kernel call - if ts - self.ts_last_check > 1.: - self.monitor_on = params.get("IsDriverMonitoringEnabled") == "1" - self._set_timers() - self.ts_last_check = ts + def _monitor_change_check(self, monitor_param_on_prev, monitor_valid_prev, monitor_on_prev): + if not monitor_param_on_prev and self.monitor_param_on: + self._reset_filters() + return MonitorChangeEvent.PARAM_ON + elif monitor_param_on_prev and not self.monitor_param_on: + self._reset_filters() + return MonitorChangeEvent.PARAM_OFF + elif monitor_on_prev and not self.monitor_valid: + return MonitorChangeEvent.VARIANCE_HIGH + elif self.monitor_param_on and not monitor_valid_prev and self.monitor_valid: + return MonitorChangeEvent.VARIANCE_LOW + else: + return MonitorChangeEvent.NO_CHANGE + + + def get_pose(self, driver_monitoring, params): self.pose.pitch = driver_monitoring.descriptor[0] self.pose.yaw = driver_monitoring.descriptor[1] @@ -79,9 +112,28 @@ class DriverStatus(): self.pose.yaw_offset = (driver_monitoring.descriptor[3] * _CAMERA_X_CONV + _CAMERA_OFFSET_X) * _CAMERA_FOV_X self.pose.pitch_offset = -driver_monitoring.descriptor[4] * _CAMERA_FOV_Y # positive y is down self.driver_distracted = self._is_driver_distracted(self.pose) - # first order filter + # first order filters self.driver_distraction_level = (1. - _DISTRACTED_FILTER_K) * self.driver_distraction_level + \ _DISTRACTED_FILTER_K * self.driver_distracted + self.variance_high = driver_monitoring.std > _STD_THRESHOLD + self.variance_level = (1. - _VARIANCE_FILTER_K) * self.variance_level + \ + _VARIANCE_FILTER_K * self.variance_high + + monitor_param_on_prev = self.monitor_param_on + monitor_valid_prev = self.monitor_valid + monitor_on_prev = self.monitor_on + + # don't check for param too often as it's a kernel call + ts = sec_since_boot() + if ts - self.ts_last_check > 1.: + self.monitor_param_on = params.get("IsDriverMonitoringEnabled") == "1" + self.ts_last_check = ts + + self.monitor_valid = _monitor_hysteresys(self.variance_level, monitor_valid_prev) + self.monitor_on = self.monitor_valid and self.monitor_param_on + self.monitor_change_event = self._monitor_change_check(monitor_param_on_prev, monitor_valid_prev, monitor_on_prev) + self._set_timers() + def update(self, events, driver_engaged, ctrl_active, standstill): @@ -90,20 +142,28 @@ class DriverStatus(): if (driver_engaged and self.awareness > 0.) or not ctrl_active: # always reset if driver is in control (unless we are in red alert state) or op isn't active self.awareness = 1. + if not ctrl_active: + # hold monitor_level constant when control isn't active + self.variance_level = 0. if self.monitor_valid else 1. if (not self.monitor_on or (self.driver_distraction_level > 0.63 and self.driver_distracted)) and \ not (standstill and self.awareness - self.step_change <= self.threshold_prompt): self.awareness = max(self.awareness - self.step_change, -0.1) + alert = None if self.awareness <= 0.: # terminal red alert: disengagement required - events.append(create_event('driverDistracted', [ET.WARNING])) + alert = 'driverDistracted' if self.monitor_on else 'driverUnresponsive' elif self.awareness <= self.threshold_prompt: # prompt orange alert - events.append(create_event('promptDriverDistracted', [ET.WARNING])) + alert = 'promptDriverDistracted' if self.monitor_on else 'promptDriverUnresponsive' elif self.awareness <= self.threshold_pre: # pre green alert - events.append(create_event('preDriverDistracted', [ET.WARNING])) + alert = 'preDriverDistracted' if self.monitor_on else 'preDriverUnresponsive' + if alert is not None: + events.append(create_event(alert, [ET.WARNING])) + + self.monitor_change_event = MonitorChangeEvent.NO_CHANGE return events diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 11d1d847db..451330fe06 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -1,11 +1,11 @@ #!/usr/bin/env python import os import zmq - -import numpy as np import math +import numpy as np +from copy import copy +from cereal import log from collections import defaultdict - from common.realtime import sec_since_boot from common.numpy_fast import interp import selfdrive.messaging as messaging @@ -303,6 +303,7 @@ class Planner(object): self.last_gps_planner_plan = None self.gps_planner_active = False + self.perception_state = log.Live20Data.new_message() def choose_solution(self, v_cruise_setpoint, enabled): if enabled: @@ -374,6 +375,7 @@ class Planner(object): self.PP.c_prob = 1.0 if l20 is not None: + self.perception_state = copy(l20.live20) self.last_l20_ts = l20.logMonoTime self.last_l20 = cur_time self.radar_dead = False diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 5f69085cf2..301d04c647 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -26,6 +26,7 @@ DIMSV = 2 XV, SPEEDV = 0, 1 VISION_POINT = -1 + class EKFV1D(EKF): def __init__(self): super(EKFV1D, self).__init__(False) @@ -137,7 +138,8 @@ def radard_thread(gctx=None): # run kalman filter only if prob is high enough if PP.lead_prob > 0.7: - ekfv.update(speedSensorV.read(PP.lead_dist, covar=PP.lead_var)) + reading = speedSensorV.read(PP.lead_dist, covar=np.matrix(PP.lead_var)) + ekfv.update_scalar(reading) ekfv.predict(tsv) ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(PP.d_poly, float(ekfv.state[XV])), float(ekfv.state[SPEEDV]), False) @@ -145,6 +147,7 @@ def radard_thread(gctx=None): ekfv.state[XV] = PP.lead_dist ekfv.covar = (np.diag([PP.lead_var, ekfv.var_init])) ekfv.state[SPEEDV] = 0. + if VISION_POINT in ar_pts: del ar_pts[VISION_POINT] diff --git a/selfdrive/loggerd/loggerd b/selfdrive/loggerd/loggerd index 724f78ac6b..dd8549ff21 100755 --- a/selfdrive/loggerd/loggerd +++ b/selfdrive/loggerd/loggerd @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:819e9c55b925cdaa7b9f818c09c649926420fb6892f47e4234954961b9d9191c -size 1622728 +oid sha256:9102036c0e855b6c95c8579a7f9b447a981b4dd505a06cfc403eeb319bc74295 +size 1626840 diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 5c2d3a4108..81a4bd981e 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -118,7 +118,6 @@ persistent_processes = [ 'uploader', 'ui', 'gpsd', - 'ubloxd', 'updated', ] @@ -129,7 +128,7 @@ car_started_processes = [ 'radard', 'visiond', 'proclogd', - 'orbd', + 'ubloxd', ] def register_managed_process(name, desc, car_started=False): @@ -298,6 +297,9 @@ def manager_thread(): cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) + # save boot log + subprocess.call(["./loggerd", "--bootlog"], cwd=os.path.join(BASEDIR, "selfdrive/loggerd")) + for p in persistent_processes: start_managed_process(p) diff --git a/selfdrive/sensord/gpsd b/selfdrive/sensord/gpsd index c7a6786aff..9b4fdd3ad5 100755 --- a/selfdrive/sensord/gpsd +++ b/selfdrive/sensord/gpsd @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:a539d59efc3c5009af06ea1dffb2acecbdc3798aaf414d08d0c9cb258e7771cb +oid sha256:9ecc73ffc43883c4407af3c88678262bc0374fb057814fc8b2c09e56619cbe15 size 1171544 diff --git a/selfdrive/sensord/sensord b/selfdrive/sensord/sensord index 588cb02e16..132f440c61 100755 --- a/selfdrive/sensord/sensord +++ b/selfdrive/sensord/sensord @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:aafc76b775889ecfb96feb3a2072c3b168a0e026cf2c0ad2f603349cdf6ab779 +oid sha256:9368e1101d7bcf298761955ae0577aa98235d2f4775278d79d591b4bdcb9f249 size 1159016 diff --git a/selfdrive/thermald.py b/selfdrive/thermald.py index 97e63b1438..97eefc3f55 100755 --- a/selfdrive/thermald.py +++ b/selfdrive/thermald.py @@ -2,7 +2,7 @@ import os import zmq from smbus2 import SMBus - +from cereal import log from selfdrive.version import training_version from selfdrive.swaglog import cloudlog import selfdrive.messaging as messaging @@ -10,9 +10,9 @@ from selfdrive.services import service_list from selfdrive.loggerd.config import ROOT from common.params import Params from common.realtime import sec_since_boot +from common.numpy_fast import clip -import cereal -ThermalStatus = cereal.log.ThermalData.ThermalStatus +ThermalStatus = log.ThermalData.ThermalStatus def read_tz(x): with open("/sys/devices/virtual/thermal/thermal_zone%d/temp" % x) as f: @@ -74,9 +74,9 @@ _FAN_SPEEDS = [0, 16384, 32768, 65535] # max fan speed only allowed if battery if hot _BAT_TEMP_THERSHOLD = 45. -def handle_fan(max_temp, bat_temp, fan_speed): - new_speed_h = next(speed for speed, temp_h in zip(_FAN_SPEEDS, _TEMP_THRS_H) if temp_h > max_temp) - new_speed_l = next(speed for speed, temp_l in zip(_FAN_SPEEDS, _TEMP_THRS_L) if temp_l > max_temp) +def handle_fan(max_cpu_temp, bat_temp, fan_speed): + new_speed_h = next(speed for speed, temp_h in zip(_FAN_SPEEDS, _TEMP_THRS_H) if temp_h > max_cpu_temp) + new_speed_l = next(speed for speed, temp_l in zip(_FAN_SPEEDS, _TEMP_THRS_L) if temp_l > max_cpu_temp) if new_speed_h > fan_speed: # update speed if using the high thresholds results in fan speed increment @@ -164,31 +164,32 @@ def thermald_thread(): msg.thermal.usbOnline = bool(int(f.read())) # TODO: add car battery voltage check - max_temp = max(msg.thermal.cpu0, msg.thermal.cpu1, - msg.thermal.cpu2, msg.thermal.cpu3) / 10.0 + max_cpu_temp = max(msg.thermal.cpu0, msg.thermal.cpu1, + msg.thermal.cpu2, msg.thermal.cpu3) / 10.0 + max_comp_temp = max(max_cpu_temp, msg.thermal.mem / 10., msg.thermal.gpu / 10.) bat_temp = msg.thermal.bat/1000. - fan_speed = handle_fan(max_temp, bat_temp, fan_speed) + fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed) msg.thermal.fanSpeed = fan_speed - # thermal logic here - - if max_temp < 70.0: - thermal_status = ThermalStatus.green - if max_temp > 85.0: - cloudlog.warning("over temp: %r", max_temp) - thermal_status = ThermalStatus.yellow - - # from controls - overtemp_proc = any(t > 950 for t in - (msg.thermal.cpu0, msg.thermal.cpu1, msg.thermal.cpu2, - msg.thermal.cpu3, msg.thermal.mem, msg.thermal.gpu)) - overtemp_bat = msg.thermal.bat > 60000 # 60c - if overtemp_proc or overtemp_bat: - # TODO: hysteresis - thermal_status = ThermalStatus.red - - if max_temp > 107.0 or msg.thermal.bat >= 63000: + # thermal logic with hysterisis + if max_cpu_temp > 107. or bat_temp >= 63.: + # onroad not allowed thermal_status = ThermalStatus.danger + elif max_comp_temp > 95. or bat_temp > 60.: + # hysteresis between onroad not allowed and engage not allowed + thermal_status = clip(thermal_status, ThermalStatus.red, ThermalStatus.danger) + elif max_cpu_temp > 90.0: + # hysteresis between engage not allowed and uploader not allowed + thermal_status = clip(thermal_status, ThermalStatus.yellow, ThermalStatus.red) + elif max_cpu_temp > 85.0: + # uploader not allowed + thermal_status = ThermalStatus.yellow + elif max_cpu_temp > 75.0: + # hysteresis between uploader not allowed and all good + thermal_status = clip(thermal_status, ThermalStatus.green, ThermalStatus.yellow) + else: + # all good + thermal_status = ThermalStatus.green # **** starting logic **** @@ -225,7 +226,7 @@ def thermald_thread(): # if any CPU gets above 107 or the battery gets above 63, kill all processes # controls will warn with CPU above 95 or battery above 60 - if msg.thermal.thermalStatus >= ThermalStatus.danger: + if thermal_status >= ThermalStatus.danger: # TODO: Add a better warning when this is happening should_start = False diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index 6948a492f5..bc53a6a41f 100644 --- a/selfdrive/ui/ui.c +++ b/selfdrive/ui/ui.c @@ -65,6 +65,8 @@ const int box_w = vwp_w-sbr_w-(bdr_s*2); const int box_h = vwp_h-(bdr_s*2); const int viz_w = vwp_w-(bdr_s*2); const int header_h = 420; +const int footer_h = 280; +const int footer_y = vwp_h-bdr_s-footer_h; const uint8_t bg_colors[][4] = { [STATUS_STOPPED] = {0x07, 0x23, 0x39, 0xff}, @@ -110,6 +112,7 @@ typedef struct UIScene { float curvature; int engaged; bool engageable; + bool monitoring_active; bool uilayout_sidebarcollapsed; bool uilayout_mapenabled; @@ -158,6 +161,7 @@ typedef struct UIState { int font_sans_semibold; int font_sans_bold; int img_wheel; + int img_face; zsock_t *thermal_sock; void *thermal_sock_raw; @@ -379,6 +383,9 @@ static void ui_init(UIState *s) { assert(s->img_wheel >= 0); s->img_wheel = nvgCreateImage(s->vg, "../assets/img_chffr_wheel.png", 1); + assert(s->img_face >= 0); + s->img_face = nvgCreateImage(s->vg, "../assets/img_driver_face.png", 1); + // init gl s->frame_program = load_program(frame_vertex_shader, frame_fragment_shader); assert(s->frame_program); @@ -981,6 +988,31 @@ static void ui_draw_vision_wheel(UIState *s) { nvgFill(s->vg); } +static void ui_draw_vision_face(UIState *s) { + const UIScene *scene = &s->scene; + const int face_size = 96; + const int face_x = (scene->ui_viz_rx + face_size + (bdr_s * 2)); + const int face_y = (footer_y + ((footer_h - face_size) / 2)); + const int face_img_size = (face_size * 1.5); + const int face_img_x = (face_x - (face_img_size / 2)); + const int face_img_y = (face_y - (face_size / 4)); + float face_img_alpha = scene->monitoring_active ? 1.0f : 0.15f; + float face_bg_alpha = scene->monitoring_active ? 0.3f : 0.1f; + NVGcolor face_bg = nvgRGBA(0, 0, 0, (255 * face_bg_alpha)); + NVGpaint face_img = nvgImagePattern(s->vg, face_img_x, face_img_y, + face_img_size, face_img_size, 0, s->img_face, face_img_alpha); + + nvgBeginPath(s->vg); + nvgCircle(s->vg, face_x, (face_y + (bdr_s * 1.5)), face_size); + nvgFillColor(s->vg, face_bg); + nvgFill(s->vg); + + nvgBeginPath(s->vg); + nvgRect(s->vg, face_img_x, face_img_y, face_img_size, face_img_size); + nvgFillPaint(s->vg, face_img); + nvgFill(s->vg); +} + static void ui_draw_vision_header(UIState *s) { const UIScene *scene = &s->scene; int ui_viz_rx = scene->ui_viz_rx; @@ -1000,6 +1032,18 @@ static void ui_draw_vision_header(UIState *s) { ui_draw_vision_wheel(s); } +static void ui_draw_vision_footer(UIState *s) { + const UIScene *scene = &s->scene; + int ui_viz_rx = scene->ui_viz_rx; + int ui_viz_rw = scene->ui_viz_rw; + + nvgBeginPath(s->vg); + nvgRect(s->vg, ui_viz_rx, footer_y, ui_viz_rw, footer_h); + + // Driver Monitoring + ui_draw_vision_face(s); +} + static void ui_draw_vision_alert(UIState *s, int va_size, int va_color, const char* va_text1, const char* va_text2) { const UIScene *scene = &s->scene; @@ -1109,8 +1153,11 @@ static void ui_draw_vision(UIState *s) { } else if (scene->cal_status == CALIBRATION_UNCALIBRATED) { // Calibration Status ui_draw_calibration_status(s); + } else { + ui_draw_vision_footer(s); } + nvgEndFrame(s->vg); glDisable(GL_BLEND); } @@ -1399,6 +1446,7 @@ static void ui_update(UIState *s) { s->scene.engaged = datad.enabled; s->scene.engageable = datad.engageable; s->scene.gps_planner_active = datad.gpsPlannerActive; + s->scene.monitoring_active = datad.driverMonitoringOn; // printf("recv %f\n", datad.vEgo); s->scene.frontview = datad.rearViewCam; @@ -1634,12 +1682,6 @@ static void* light_sensor_thread(void *args) { int SENSOR_LIGHT = 7; - struct stat buffer; - if (stat("/sys/bus/i2c/drivers/cyccg", &buffer) == 0) { - LOGD("LeEco light sensor detected"); - SENSOR_LIGHT = 5; - } - device->activate(device, SENSOR_LIGHT, 0); device->activate(device, SENSOR_LIGHT, 1); device->setDelay(device, SENSOR_LIGHT, ms2ns(100)); @@ -1653,9 +1695,7 @@ static void* light_sensor_thread(void *args) { LOG_100("light_sensor_poll failed: %d", n); } if (n > 0) { - if (SENSOR_LIGHT == 5) s->light_sensor = buffer[0].light * 2; - else s->light_sensor = buffer[0].light; - //printf("new light sensor value: %f\n", s->light_sensor); + s->light_sensor = buffer[0].light; } } diff --git a/selfdrive/visiond/visiond b/selfdrive/visiond/visiond index b292f4fa36..df29fc06ca 100755 --- a/selfdrive/visiond/visiond +++ b/selfdrive/visiond/visiond @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:a21acff35bda607b6780df7730e005a3b289acfc737a1340a7b9f7a3178d83a0 -size 13965736 +oid sha256:97110fee2fdc891137a643a7eab38bd269f472658ac05e950fe6dceab306822c +size 12539240