diff --git a/README.md b/README.md index e1a289bd9092bd..c8762963c09cc2 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 a515873642a5ca..0a81ac30ed248f 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 0c8ae7e51eeb9a..a2f77a55093901 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 eaf67b1c70390f..5b9dc2666ba331 100644 Binary files a/apk/ai.comma.plus.frame.apk and b/apk/ai.comma.plus.frame.apk differ diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk index db4a96af8b18d4..489bdd7e73d6c9 100644 Binary files a/apk/ai.comma.plus.offroad.apk and b/apk/ai.comma.plus.offroad.apk differ diff --git a/cereal/car.capnp b/cereal/car.capnp index ffc79e6476dc69..65ec3ee957d444 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 474c0d2410bd47..fbb00a2a194407 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 1cd7a8e9d560db..568fb9bf2b3f4e 100644 --- a/common/transformations/coordinates.py +++ b/common/transformations/coordinates.py @@ -12,12 +12,14 @@ 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 00000000000000..ddde478cd789ec Binary files /dev/null and b/selfdrive/assets/img_driver_face.png differ diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 6a64183425e0ea..1f0e36e0dc23ef 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 @@ def update(self, c): # 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 59b6e35c748312..7c0c63f54631ce 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -4,7 +4,8 @@ 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 @@ def update(self, pt_cp): 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 53dc8b68e78ee9..3d0a49feeeb423 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 @@ def update(self, c): # 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 314bb4a3f645fd..b69109f356735d 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -47,6 +47,7 @@ def parse_gear_shifter(can_gear): }], } +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 0875a67234a02d..b1b72321efc7d4 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 @@ def __init__(self, dbc_name, enable_camera=True): 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 @@ def update(self, sendcan, enabled, CS, frame, actuators, \ 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 36dea1f7ff1fbb..466259c20ac10f 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -2,7 +2,7 @@ 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 @@ def update(self, cp): 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 92e74cd6868155..8dc5e1d9b5d0de 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 db02a9725a969e..4cac8c092a7c0f 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 @@ def update(self, c): # 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 @@ def apply(self, c): 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 3081474487540d..7d30265c2f471e 100755 --- a/selfdrive/car/honda/radar_interface.py +++ b/selfdrive/car/honda/radar_interface.py @@ -28,6 +28,7 @@ def __init__(self, CP): 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 @@ def update(self): 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 @@ def update(self): 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 71bb495e26976e..5188ca576e147b 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -104,5 +104,19 @@ class CAR: 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 db3b8b6a53dac3..5edbcbbd113f78 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 @@ def update(self, c): 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 7752add1b23c2a..a8e317b1ebaaf2 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -4,7 +4,7 @@ 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 @@ def update(self, sendcan, enabled, CS, frame, actuators, 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 28bfc0fb5761a7..53ff3aaa8a17c4 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 @@ 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 @@ def update(self, cp): 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 @@ def update(self, cp): 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 0170c4c942b990..6a1585978181bd 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 @@ def get_params(candidate, fingerprint): 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 @@ def get_params(candidate, fingerprint): 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 @@ def get_params(candidate, fingerprint): # 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 @@ def get_params(candidate, fingerprint): 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 @@ def update(self, c): # 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 9af6aed47bf9c3..25bbfbfb39e53b 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 @@ def __init__(self, CP): 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 @@ def update(self): 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 649146073f6518..2e0cf3e6fee842 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -2,10 +2,14 @@ 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 @@ class ECU: (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 @@ def check_ecu_msgs(fingerprint, candidate, ecu): 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 @@ def check_ecu_msgs(fingerprint, candidate, ecu): 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 00000000000000..78a7c098949b56 --- /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 00000000000000..056482ffa51f3e --- /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 2b226fc6e0cd8c..664167ebcedb2a 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 1b8ad21282a78c..f75f4c70a1b331 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 9d053c211fb496..2fd716c3d2134a 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 a593f1091dc6f9..a70eb498455c6f 100644 --- a/selfdrive/controls/lib/driver_monitor.py +++ b/selfdrive/controls/lib/driver_monitor.py @@ -18,11 +18,21 @@ _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 @@ def __init__(self): 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 @@ def _is_driver_distracted(self, pose): #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 @@ def get_pose(self, driver_monitoring, params): 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 @@ def update(self, events, driver_engaged, ctrl_active, standstill): 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 11d1d847db08ad..451330fe0643a8 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 @@ def __init__(self, CP, fcw_enabled): 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 @@ def update(self, CS, LaC, LoC, v_cruise_kph, force_slow_decel): 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 5f69085cf2b5e2..301d04c6476598 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -26,6 +26,7 @@ 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 7ed8ac45d5a288..67d21aad9b07a7 100755 Binary files a/selfdrive/loggerd/loggerd and b/selfdrive/loggerd/loggerd differ diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 5c2d3a4108bf18..81a4bd981e86d8 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -118,7 +118,6 @@ def get_running(): 'uploader', 'ui', 'gpsd', - 'ubloxd', 'updated', ] @@ -129,7 +128,7 @@ def get_running(): '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 20e5332795d31b..84705bbc8c4d47 100755 Binary files a/selfdrive/sensord/gpsd and b/selfdrive/sensord/gpsd differ diff --git a/selfdrive/sensord/sensord b/selfdrive/sensord/sensord index 58f9d233c70ba6..49d7db520572c3 100755 Binary files a/selfdrive/sensord/sensord and b/selfdrive/sensord/sensord differ diff --git a/selfdrive/thermald.py b/selfdrive/thermald.py index 97e63b143892c0..97eefc3f5575bd 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.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 @@ def set_eon_fan(val): # 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 6948a492f546c0..bc53a6a41f7dd6 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 1a4867061edaad..89ad0fcdff3a6a 100755 Binary files a/selfdrive/visiond/visiond and b/selfdrive/visiond/visiond differ