Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2001-2024 German Aerospace Center (DLR) and others.
4 : // This program and the accompanying materials are made available under the
5 : // terms of the Eclipse Public License 2.0 which is available at
6 : // https://www.eclipse.org/legal/epl-2.0/
7 : // This Source Code may also be made available under the following Secondary
8 : // Licenses when the conditions for such availability set forth in the Eclipse
9 : // Public License 2.0 are satisfied: GNU General Public License, version 2
10 : // or later which is available at
11 : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 : /****************************************************************************/
14 : /// @file MSCFModel_CACC.cpp
15 : /// @author Kallirroi Porfyri
16 : /// @author Karagounis Vasilios
17 : /// @date Nov 2018
18 : ///
19 : // CACC car-following model based on [1], [2].
20 : // [1] Milanes, V., and S. E. Shladover. Handling Cut-In Vehicles in Strings
21 : // of Cooperative Adaptive Cruise Control Vehicles. Journal of Intelligent
22 : // Transportation Systems, Vol. 20, No. 2, 2015, pp. 178-191.
23 : // [2] Xiao, L., M. Wang and B. van Arem. Realistic Car-Following Models for
24 : // Microscopic Simulation of Adaptive and Cooperative Adaptive Cruise
25 : // Control Vehicles. Transportation Research Record: Journal of the
26 : // Transportation Research Board, No. 2623, 2017. (DOI: 10.3141/2623-01).
27 : /****************************************************************************/
28 : #include <config.h>
29 :
30 : #include <stdio.h>
31 : #include <iostream>
32 :
33 : #include "MSCFModel_CACC.h"
34 : #include <microsim/MSVehicle.h>
35 : #include <microsim/MSLane.h>
36 : #include <utils/common/RandHelper.h>
37 : #include <utils/common/SUMOTime.h>
38 : #include <utils/common/StringUtils.h>
39 : #include <microsim/lcmodels/MSAbstractLaneChangeModel.h>
40 : #include <math.h>
41 : #include <microsim/MSNet.h>
42 :
43 : // ===========================================================================
44 : // debug flags
45 : // ===========================================================================
46 : #define DEBUG_CACC 0
47 : #define DEBUG_CACC_INSERTION_FOLLOW_SPEED 0
48 : #define DEBUG_CACC_SECURE_GAP 0
49 : #define DEBUG_COND (veh->isSelected())
50 : //#define DEBUG_COND (veh->getID() == "flow.0")
51 : //#define DEBUG_COND (veh->getID() == "CVflowToC2.11")
52 :
53 :
54 : // ===========================================================================
55 : // defaults
56 : // ===========================================================================
57 : #define DEFAULT_SC_GAIN_CACC -0.4
58 : #define DEFAULT_GCC_GAIN_GAP_CACC 0.005
59 : #define DEFAULT_GCC_GAIN_GAP_DOT_CACC 0.05
60 : #define DEFAULT_GC_GAIN_GAP_CACC 0.45
61 : #define DEFAULT_GC_GAIN_GAP_DOT_CACC 0.0125
62 : #define DEFAULT_CA_GAIN_GAP_CACC 0.45
63 : #define DEFAULT_CA_GAIN_GAP_DOT_CACC 0.05
64 : #define DEFAULT_HEADWAYTIME_ACC 1.0
65 : #define DEFAULT_SC_MIN_GAP 1.66
66 :
67 : // override followSpeed when deemed unsafe by the given margin (the value was selected to reduce the number of necessary interventions)
68 : #define DEFAULT_EMERGENCY_OVERRIDE_THRESHOLD 2.0
69 :
70 : std::map<std::string, MSCFModel_CACC::CommunicationsOverrideMode> MSCFModel_CACC::CommunicationsOverrideModeMap = {
71 : {"0", CACC_NO_OVERRIDE},
72 : {"1", CACC_MODE_NO_LEADER},
73 : {"2", CACC_MODE_LEADER_NO_CAV},
74 : {"3", CACC_MODE_LEADER_CAV}
75 : };
76 :
77 : std::map<MSCFModel_CACC::VehicleMode, std::string> MSCFModel_CACC::VehicleModeNames = {
78 : {CC_MODE, "CC"},
79 : {ACC_MODE, "ACC"},
80 : {CACC_GAP_CLOSING_MODE, "CACC_GAP_CL"},
81 : {CACC_GAP_MODE, "CACC_GAP"},
82 : {CACC_COLLISION_AVOIDANCE_MODE, "CACC_CA"}
83 : };
84 :
85 : // ===========================================================================
86 : // method definitions
87 : // ===========================================================================
88 437 : MSCFModel_CACC::MSCFModel_CACC(const MSVehicleType* vtype) :
89 437 : MSCFModel(vtype), acc_CFM(MSCFModel_ACC(vtype)),
90 437 : mySpeedControlGain(vtype->getParameter().getCFParam(SUMO_ATTR_SC_GAIN_CACC, DEFAULT_SC_GAIN_CACC)),
91 437 : myGapClosingControlGainGap(vtype->getParameter().getCFParam(SUMO_ATTR_GCC_GAIN_GAP_CACC, DEFAULT_GCC_GAIN_GAP_CACC)),
92 437 : myGapClosingControlGainGapDot(vtype->getParameter().getCFParam(SUMO_ATTR_GCC_GAIN_GAP_DOT_CACC, DEFAULT_GCC_GAIN_GAP_DOT_CACC)),
93 437 : myGapControlGainGap(vtype->getParameter().getCFParam(SUMO_ATTR_GC_GAIN_GAP_CACC, DEFAULT_GC_GAIN_GAP_CACC)),
94 437 : myGapControlGainGapDot(vtype->getParameter().getCFParam(SUMO_ATTR_GC_GAIN_GAP_DOT_CACC, DEFAULT_GC_GAIN_GAP_DOT_CACC)),
95 437 : myCollisionAvoidanceGainGap(vtype->getParameter().getCFParam(SUMO_ATTR_CA_GAIN_GAP_CACC, DEFAULT_CA_GAIN_GAP_CACC)),
96 437 : myCollisionAvoidanceGainGapDot(vtype->getParameter().getCFParam(SUMO_ATTR_CA_GAIN_GAP_DOT_CACC, DEFAULT_CA_GAIN_GAP_DOT_CACC)),
97 437 : myHeadwayTimeACC(vtype->getParameter().getCFParam(SUMO_ATTR_HEADWAY_TIME_CACC_TO_ACC, DEFAULT_HEADWAYTIME_ACC)),
98 437 : myApplyDriverstate(vtype->getParameter().getCFParam(SUMO_ATTR_APPLYDRIVERSTATE, 0)),
99 437 : myEmergencyThreshold(vtype->getParameter().getCFParam(SUMO_ATTR_CA_OVERRIDE, DEFAULT_EMERGENCY_OVERRIDE_THRESHOLD)),
100 874 : mySpeedControlMinGap(vtype->getParameter().getCFParam(SUMO_ATTR_SC_MIN_GAP, DEFAULT_SC_MIN_GAP)) {
101 437 : myCollisionMinGapFactor = vtype->getParameter().getCFParam(SUMO_ATTR_COLLISION_MINGAP_FACTOR, 0.1);
102 437 : acc_CFM.setHeadwayTime(myHeadwayTimeACC);
103 437 : }
104 :
105 874 : MSCFModel_CACC::~MSCFModel_CACC() {}
106 :
107 : double
108 8135881 : MSCFModel_CACC::freeSpeed(const MSVehicle* const veh, double speed, double seen, double maxSpeed, const bool onInsertion, const CalcReason usage) const {
109 8135881 : if (!MSGlobals::gComputeLC && usage == CalcReason::CURRENT) {
110 : CACCVehicleVariables* vars = (CACCVehicleVariables*)veh->getCarFollowVariables();
111 8118855 : if (vars->lastUpdateTime != MSNet::getInstance()->getCurrentTimeStep()) {
112 : // _v was not called in this step
113 935794 : const_cast<SUMOVehicleParameter&>(veh->getParameter()).setParameter("caccVehicleMode", VehicleModeNames[CC_MODE]);
114 : }
115 : }
116 8135881 : return MSCFModel::freeSpeed(veh, speed, seen, maxSpeed, onInsertion, usage);
117 : }
118 :
119 : double
120 25691740 : MSCFModel_CACC::followSpeed(const MSVehicle* const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle* const pred, const CalcReason usage) const {
121 25691740 : if (myApplyDriverstate) {
122 51045 : applyHeadwayAndSpeedDifferencePerceptionErrors(veh, speed, gap2pred, predSpeed, predMaxDecel, pred);
123 : }
124 :
125 25691740 : const double desSpeed = veh->getLane()->getVehicleMaxSpeed(veh);
126 25691740 : const double vCACC = _v(veh, pred, gap2pred, speed, predSpeed, desSpeed, true, usage);
127 : // using onInsertion=true disables emergencyOverides emergency deceleration smoothing
128 25691740 : const double vSafe = maximumSafeFollowSpeed(gap2pred, speed, predSpeed, predMaxDecel, true);
129 :
130 : #if DEBUG_CACC == 1
131 : if (DEBUG_COND) {
132 : std::cout << SIMTIME << " veh=" << veh->getID() << " pred=" << Named::getIDSecure(pred)
133 : << " v=" << speed << " vL=" << predSpeed << " gap=" << gap2pred
134 : << " predDecel=" << predMaxDecel << " vCACC=" << vCACC << " vSafe=" << vSafe << "\n";
135 : }
136 : #else
137 : UNUSED_PARAMETER(pred);
138 : #endif
139 25691740 : const double speedOverride = MIN2(myEmergencyThreshold, gap2pred);
140 25691740 : if (vSafe + speedOverride < vCACC) {
141 : #if DEBUG_CACC == 1
142 : if (DEBUG_COND) {
143 : std::cout << "Apply Safe speed, override=" << speedOverride << "\n";
144 : }
145 : #endif
146 : return MAX2(0.0, vSafe + speedOverride);
147 : }
148 : return vCACC;
149 : }
150 :
151 : double
152 10886270 : MSCFModel_CACC::stopSpeed(const MSVehicle* const veh, const double speed, double gap, double decel, const CalcReason /*usage*/) const {
153 10886270 : if (myApplyDriverstate) {
154 2728 : applyHeadwayPerceptionError(veh, speed, gap);
155 : }
156 :
157 : // NOTE: This allows return of smaller values than minNextSpeed().
158 : // Only relevant for the ballistic update: We give the argument headway=TS, to assure that
159 : // the stopping position is approached with a uniform deceleration also for tau!=TS.
160 10886270 : return MIN2(maximumSafeStopSpeed(gap, decel, speed, false, veh->getActionStepLengthSecs()), maxNextSpeed(speed, veh));
161 : }
162 :
163 : double
164 28260057 : MSCFModel_CACC::getSecureGap(const MSVehicle* const veh, const MSVehicle* const pred, const double speed, const double leaderSpeed, const double leaderMaxDecel) const {
165 : // Accel in gap mode should vanish:
166 : double desSpacing;
167 28260057 : if (pred->getCarFollowModel().getModelID() != SUMO_TAG_CF_CACC) {
168 : // 0 = myGapControlGainSpeed * (leaderSpeed - speed) + myGapControlGainSpace * (g - myHeadwayTime * speed);
169 : // <=> myGapControlGainSpace * g = - myGapControlGainSpeed * (leaderSpeed - speed) + myGapControlGainSpace * myHeadwayTime * speed;
170 : // <=> g = - myGapControlGainSpeed * (leaderSpeed - speed) / myGapControlGainSpace + myHeadwayTime * speed;
171 109442 : desSpacing = acc_CFM.myGapControlGainSpeed * (speed - leaderSpeed) / acc_CFM.myGapControlGainSpace + myHeadwayTimeACC * speed; // MSCFModel_ACC::accelGapControl
172 : } else {
173 28150615 : desSpacing = myHeadwayTime * speed; // speedGapControl
174 : }
175 28260057 : const double desSpacingDefault = MSCFModel::getSecureGap(veh, pred, speed, leaderSpeed, leaderMaxDecel);
176 : #if DEBUG_CACC_SECURE_GAP == 1
177 : std::cout << SIMTIME << "MSCFModel_ACC::getSecureGap speed=" << speed << " leaderSpeed=" << leaderSpeed
178 : << " desSpacing=" << desSpacing << " desSpacingDefault=" << desSpacingDefault << "\n";
179 : #endif
180 28260057 : return MAX2(desSpacing, desSpacingDefault);
181 : }
182 :
183 :
184 : double
185 322070 : MSCFModel_CACC::insertionFollowSpeed(const MSVehicle* const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle* const pred) const {
186 : #if DEBUG_CACC_INSERTION_FOLLOW_SPEED == 1
187 : if (DEBUG_COND) {
188 : std::cout << "MSCFModel_ACC::insertionFollowSpeed(), speed=" << speed << " gap2pred=" << gap2pred << " predSpeed=" << predSpeed << "\n";
189 : }
190 : #endif
191 : // iterate to find a stationary value for
192 : // speed = followSpeed(v, speed, gap2pred, predSpeed, predMaxDecel, nullptr, CalcReason::FUTURE)
193 : const int max_iter = 50;
194 : int n_iter = 0;
195 : const double tol = 0.1;
196 : double damping = 0.8;
197 :
198 : double res = speed;
199 3385873 : while (n_iter < max_iter) {
200 : // proposed acceleration
201 3362903 : const double vCACC = _v(veh, pred, gap2pred, res, predSpeed, speed, true);
202 3362903 : const double vSafe = maximumSafeFollowSpeed(gap2pred, res, predSpeed, predMaxDecel, true);
203 3362903 : const double a = MIN2(vCACC, vSafe) - res;
204 3362903 : res = res + damping * a;
205 : #if DEBUG_CACC_INSERTION_FOLLOW_SPEED == 1
206 : if (DEBUG_COND) {
207 : std::cout << " n_iter=" << n_iter << " vSafe=" << vSafe << " vCACC=" << vCACC << " a=" << a << " damping=" << damping << " res=" << res << std::endl;
208 : }
209 : #endif
210 3362903 : damping *= 0.9;
211 3362903 : if (fabs(a) < tol) {
212 : break;
213 : } else {
214 3063803 : n_iter++;
215 : }
216 : }
217 322070 : return res;
218 : }
219 :
220 :
221 :
222 :
223 : /// @todo update interactionGap logic
224 : double
225 0 : MSCFModel_CACC::interactionGap(const MSVehicle* const /* veh */, double /* vL */) const {
226 : /*maximum radar range is CACC is enabled*/
227 0 : return 250;
228 : }
229 :
230 :
231 : std::string
232 16 : MSCFModel_CACC::getParameter(const MSVehicle* veh, const std::string& key) const {
233 : CACCVehicleVariables* vars = (CACCVehicleVariables*) veh->getCarFollowVariables();
234 :
235 16 : if (key.compare("caccCommunicationsOverrideMode") == 0) {
236 16 : return toString(vars->CACC_CommunicationsOverrideMode);
237 : }
238 :
239 0 : return "";
240 : }
241 :
242 :
243 : void
244 16 : MSCFModel_CACC::setParameter(MSVehicle* veh, const std::string& key, const std::string& value) const {
245 : CACCVehicleVariables* vars = (CACCVehicleVariables*) veh->getCarFollowVariables();
246 :
247 : try {
248 16 : if (key.compare("caccCommunicationsOverrideMode") == 0) {
249 16 : vars->CACC_CommunicationsOverrideMode = CommunicationsOverrideModeMap[value];
250 : }
251 0 : } catch (NumberFormatException&) {
252 0 : throw InvalidArgument("Invalid value '" + value + "' for parameter '" + key + "' for vehicle '" + veh->getID() + "'");
253 0 : }
254 16 : }
255 :
256 :
257 : double
258 22651347 : MSCFModel_CACC::speedSpeedControl(const double speed, double vErr, VehicleMode& vehMode) const {
259 : // Speed control law
260 22651347 : vehMode = CC_MODE;
261 22651347 : double sclAccel = mySpeedControlGain * vErr;
262 22651347 : double newSpeed = speed + ACCEL2SPEED(sclAccel);
263 22651347 : return newSpeed;
264 : }
265 :
266 : double
267 7885808 : MSCFModel_CACC::speedGapControl(const MSVehicle* const veh, const double gap2pred,
268 : const double speed, const double predSpeed, const double desSpeed, double vErr,
269 : const MSVehicle* const pred, VehicleMode& vehMode) const {
270 : // Gap control law
271 : double newSpeed = 0.0;
272 :
273 7885808 : if (pred != nullptr) {
274 6401932 : if (pred->getCarFollowModel().getModelID() != SUMO_TAG_CF_CACC) {
275 51961 : vehMode = ACC_MODE;
276 51961 : newSpeed = acc_CFM._v(veh, gap2pred, speed, predSpeed, desSpeed, true);
277 : #if DEBUG_CACC == 1
278 : if (DEBUG_COND) {
279 : std::cout << " acc control mode" << std::endl;
280 : }
281 : #endif
282 : } else {
283 : #if DEBUG_CACC == 1
284 : if (DEBUG_COND) {
285 : std::cout << " CACC control mode" << std::endl;
286 : }
287 : #endif
288 6349971 : double desSpacing = myHeadwayTime * speed;
289 6349971 : double spacingErr = gap2pred - desSpacing;
290 6349971 : double accel = veh->getAcceleration();
291 6349971 : double speedErr = predSpeed - speed - myHeadwayTime * accel;
292 :
293 6349971 : if ((spacingErr > 0 && spacingErr < 0.2) && (vErr < 0.1)) {
294 : // gap mode
295 : //newSpeed = speed + 0.45 * spacingErr + 0.0125 *speedErr;
296 : #if DEBUG_CACC == 1
297 : if (DEBUG_COND) {
298 : std::cout << " applying gap control: err=" << spacingErr << " speedErr=" << speedErr << std::endl;
299 : }
300 : #endif
301 304042 : newSpeed = speed + myGapControlGainGap * spacingErr + myGapControlGainGapDot * speedErr;
302 :
303 304042 : vehMode = CACC_GAP_MODE;
304 2656852 : } else if (spacingErr < 0) {
305 : // collision avoidance mode
306 : //newSpeed = speed + 0.45 * spacingErr + 0.05 *speedErr;
307 : #if DEBUG_CACC == 1
308 : if (DEBUG_COND) {
309 : std::cout << " applying collision avoidance: err=" << spacingErr << " speedErr=" << speedErr << "\n";
310 : }
311 : #endif
312 2650140 : newSpeed = speed + myCollisionAvoidanceGainGap * spacingErr + myCollisionAvoidanceGainGapDot * speedErr;
313 2650140 : vehMode = CACC_COLLISION_AVOIDANCE_MODE;
314 : } else {
315 : // gap closing mode
316 : #if DEBUG_CACC == 1
317 : if (DEBUG_COND) {
318 : std::cout << " applying gap closing err=" << spacingErr << " speedErr=" << speedErr << " predSpeed=" << predSpeed << " speed=" << speed << " accel=" << accel << "\n";
319 : }
320 : #endif
321 3395789 : newSpeed = speed + myGapClosingControlGainGap * spacingErr + myGapClosingControlGainGapDot * speedErr;
322 :
323 3395789 : vehMode = CACC_GAP_CLOSING_MODE;
324 : }
325 : }
326 : } else {
327 : /* no leader */
328 : #if DEBUG_CACC == 1
329 : if (DEBUG_COND) {
330 : std::cout << " no leader" << std::endl;
331 : }
332 : #endif
333 1483876 : newSpeed = speedSpeedControl(speed, vErr, vehMode);
334 : }
335 :
336 7885808 : return newSpeed;
337 : }
338 :
339 : double
340 29054643 : MSCFModel_CACC::_v(const MSVehicle* const veh, const MSVehicle* const pred, const double gap2pred, const double speed,
341 : const double predSpeed, const double desSpeed, const bool /* respectMinGap */, const CalcReason usage) const {
342 : double newSpeed = 0.0;
343 29054643 : VehicleMode vehMode = CC_MODE;
344 :
345 : #if DEBUG_CACC == 1
346 : if (DEBUG_COND) {
347 : std::cout << SIMTIME << " MSCFModel_CACC::_v() for veh '" << veh->getID()
348 : << " gap=" << gap2pred << " speed=" << speed << " predSpeed=" << predSpeed
349 : << " desSpeed=" << desSpeed << std::endl;
350 : }
351 : #endif
352 :
353 : /* Velocity error */
354 29054643 : double vErr = speed - desSpeed;
355 : bool setControlMode = false;
356 : CACCVehicleVariables* vars = (CACCVehicleVariables*)veh->getCarFollowVariables();
357 29054643 : if (vars->lastUpdateTime != MSNet::getInstance()->getCurrentTimeStep()) {
358 4152282 : vars->lastUpdateTime = MSNet::getInstance()->getCurrentTimeStep();
359 : setControlMode = true;
360 : }
361 :
362 29054643 : CommunicationsOverrideMode commMode = vars->CACC_CommunicationsOverrideMode;
363 :
364 29054643 : if (commMode == CACC_NO_OVERRIDE) { // old CACC logic (rely on time gap from predecessor)
365 : // @note: using (gap2pred + minGap) here increases oscillations but may
366 : // actually be a good idea once the acceleration-spike-problem is fixed
367 29052599 : double time_gap = gap2pred / MAX2(NUMERICAL_EPS, speed);
368 29052599 : double spacingErr = gap2pred - myHeadwayTime * speed;
369 :
370 29052599 : if (time_gap > 2 && spacingErr > mySpeedControlMinGap) {
371 : #if DEBUG_CACC == 1
372 : if (DEBUG_COND) {
373 : std::cout << " applying speedControl" << " time_gap=" << time_gap << std::endl;
374 : }
375 : #endif
376 : // Find acceleration - Speed control law
377 16687731 : newSpeed = speedSpeedControl(speed, vErr, vehMode);
378 : // Set cl to vehicle parameters
379 16687731 : if (setControlMode) {
380 933884 : vars->CACC_ControlMode = 0;
381 : }
382 12364868 : } else if (time_gap < 1.5) {
383 : // Find acceleration - Gap control law
384 : #if DEBUG_CACC == 1
385 : if (DEBUG_COND) {
386 : std::cout << " speedGapControl" << " time_gap=" << time_gap << std::endl;
387 : }
388 : #endif
389 :
390 6188646 : newSpeed = speedGapControl(veh, gap2pred, speed, predSpeed, desSpeed, vErr, pred, vehMode);
391 : // Set cl to vehicle parameters
392 6188646 : if (setControlMode) {
393 1298038 : vars->CACC_ControlMode = 1;
394 : }
395 : } else {
396 : // Follow previous applied law
397 6176222 : int cm = vars->CACC_ControlMode;
398 6176222 : if (!cm) {
399 : // CACC_ControlMode = speed control
400 :
401 : #if DEBUG_CACC == 1
402 : if (DEBUG_COND) {
403 : std::cout << " applying speedControl (previous)" << " time_gap=" << time_gap << std::endl;
404 : }
405 : #endif
406 4479060 : newSpeed = speedSpeedControl(speed, vErr, vehMode);
407 : } else {
408 : // CACC_ControlMode = gap control
409 : #if DEBUG_CACC == 1
410 : if (DEBUG_COND) {
411 : std::cout << " previous speedGapControl (previous)" << " time_gap=" << time_gap << std::endl;
412 : }
413 : #endif
414 1697162 : newSpeed = speedGapControl(veh, gap2pred, speed, predSpeed, desSpeed, vErr, pred, vehMode);
415 : }
416 : }
417 2044 : } else if (commMode == CACC_MODE_NO_LEADER) {
418 680 : newSpeed = speedSpeedControl(speed, vErr, vehMode);
419 1364 : } else if (commMode == CACC_MODE_LEADER_NO_CAV) {
420 680 : newSpeed = acc_CFM._v(veh, gap2pred, speed, predSpeed, desSpeed, true);
421 680 : vehMode = ACC_MODE;
422 684 : } else if (commMode == CACC_MODE_LEADER_CAV) {
423 684 : double desSpacing = myHeadwayTime * speed;
424 684 : double spacingErr = gap2pred - desSpacing;
425 684 : double accel = veh->getAcceleration();
426 684 : double speedErr = predSpeed - speed - myHeadwayTime * accel;
427 :
428 684 : if ((spacingErr > 0 && spacingErr < 0.2) && (vErr < 0.1)) {
429 : // gap mode
430 : //newSpeed = speed + 0.45 * spacingErr + 0.0125 *speedErr;
431 336 : if (DEBUG_COND) {
432 : std::cout << " applying CACC_GAP_MODE " << std::endl;
433 : }
434 336 : newSpeed = speed + myGapControlGainGap * spacingErr + myGapControlGainGapDot * speedErr;
435 336 : vehMode = CACC_GAP_MODE;
436 32 : } else if (spacingErr < 0) {
437 : // collision avoidance mode
438 : //newSpeed = speed + 0.45 * spacingErr + 0.05 *speedErr;
439 32 : if (DEBUG_COND) {
440 : std::cout << " applying CACC_COLLISION_AVOIDANCE_MODE " << std::endl;
441 : }
442 32 : newSpeed = speed + myCollisionAvoidanceGainGap * spacingErr + myCollisionAvoidanceGainGapDot * speedErr;
443 32 : vehMode = CACC_COLLISION_AVOIDANCE_MODE;
444 : } else {
445 : // gap closing mode
446 316 : if (DEBUG_COND) {
447 : std::cout << " applying CACC_GAP_CLOSING_MODE " << std::endl;
448 : }
449 316 : newSpeed = speed + myGapClosingControlGainGap * spacingErr + myGapClosingControlGainGapDot * speedErr;
450 316 : vehMode = CACC_GAP_CLOSING_MODE;
451 : }
452 : }
453 :
454 8410188 : if (setControlMode && !MSGlobals::gComputeLC && usage == CalcReason::CURRENT) {
455 8288052 : const_cast<SUMOVehicleParameter&>(veh->getParameter()).setParameter("caccVehicleMode", VehicleModeNames[vehMode]);
456 : }
457 :
458 : //std::cout << veh->getID() << " commMode: " << commMode << ", caccVehicleMode: " << VehicleModeNames[vehMode]
459 : // << ", gap2pred: " << gap2pred << ", newSpeed: " << newSpeed << std::endl;
460 :
461 : // newSpeed is meant for step length 0.1 but this would cause extreme
462 : // accelerations at lower step length
463 : double newSpeedScaled = newSpeed;
464 29054643 : if (DELTA_T < 100) {
465 49788 : const double accel01 = (newSpeed - speed) * 10;
466 49788 : newSpeedScaled = speed + ACCEL2SPEED(accel01);
467 : }
468 :
469 : #if DEBUG_CACC == 1
470 : if (DEBUG_COND) {
471 : std::cout << " result: rawAccel=" << SPEED2ACCEL(newSpeed - speed) << " newSpeed=" << newSpeed << " newSpeedScaled=" << newSpeedScaled << "\n";
472 : }
473 : #endif
474 :
475 29054643 : return MAX2(0., newSpeedScaled);
476 : }
477 :
478 :
479 :
480 : MSCFModel*
481 0 : MSCFModel_CACC::duplicate(const MSVehicleType* vtype) const {
482 0 : return new MSCFModel_CACC(vtype);
483 : }
|
