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 443 : MSCFModel_CACC::MSCFModel_CACC(const MSVehicleType* vtype) :
89 443 : MSCFModel(vtype), acc_CFM(MSCFModel_ACC(vtype)),
90 443 : mySpeedControlGain(vtype->getParameter().getCFParam(SUMO_ATTR_SC_GAIN_CACC, DEFAULT_SC_GAIN_CACC)),
91 443 : myGapClosingControlGainGap(vtype->getParameter().getCFParam(SUMO_ATTR_GCC_GAIN_GAP_CACC, DEFAULT_GCC_GAIN_GAP_CACC)),
92 443 : myGapClosingControlGainGapDot(vtype->getParameter().getCFParam(SUMO_ATTR_GCC_GAIN_GAP_DOT_CACC, DEFAULT_GCC_GAIN_GAP_DOT_CACC)),
93 443 : myGapControlGainGap(vtype->getParameter().getCFParam(SUMO_ATTR_GC_GAIN_GAP_CACC, DEFAULT_GC_GAIN_GAP_CACC)),
94 443 : myGapControlGainGapDot(vtype->getParameter().getCFParam(SUMO_ATTR_GC_GAIN_GAP_DOT_CACC, DEFAULT_GC_GAIN_GAP_DOT_CACC)),
95 443 : myCollisionAvoidanceGainGap(vtype->getParameter().getCFParam(SUMO_ATTR_CA_GAIN_GAP_CACC, DEFAULT_CA_GAIN_GAP_CACC)),
96 443 : myCollisionAvoidanceGainGapDot(vtype->getParameter().getCFParam(SUMO_ATTR_CA_GAIN_GAP_DOT_CACC, DEFAULT_CA_GAIN_GAP_DOT_CACC)),
97 443 : myHeadwayTimeACC(vtype->getParameter().getCFParam(SUMO_ATTR_HEADWAY_TIME_CACC_TO_ACC, DEFAULT_HEADWAYTIME_ACC)),
98 443 : myApplyDriverstate(vtype->getParameter().getCFParam(SUMO_ATTR_APPLYDRIVERSTATE, 0)),
99 443 : myEmergencyThreshold(vtype->getParameter().getCFParam(SUMO_ATTR_CA_OVERRIDE, DEFAULT_EMERGENCY_OVERRIDE_THRESHOLD)),
100 886 : mySpeedControlMinGap(vtype->getParameter().getCFParam(SUMO_ATTR_SC_MIN_GAP, DEFAULT_SC_MIN_GAP)) {
101 443 : myCollisionMinGapFactor = vtype->getParameter().getCFParam(SUMO_ATTR_COLLISION_MINGAP_FACTOR, 0.1);
102 443 : acc_CFM.setHeadwayTime(myHeadwayTimeACC);
103 443 : }
104 :
105 886 : MSCFModel_CACC::~MSCFModel_CACC() {}
106 :
107 : double
108 8041697 : MSCFModel_CACC::freeSpeed(const MSVehicle* const veh, double speed, double seen, double maxSpeed, const bool onInsertion, const CalcReason usage) const {
109 : // set "caccVehicleMode" parameter to default value
110 8041697 : if (!MSGlobals::gComputeLC && usage == CalcReason::CURRENT) {
111 15626274 : const_cast<SUMOVehicleParameter&>(veh->getParameter()).setParameter("caccVehicleMode", VehicleModeNames[CC_MODE]);
112 : }
113 8041697 : return MSCFModel::freeSpeed(veh, speed, seen, maxSpeed, onInsertion, usage);
114 : }
115 :
116 : double
117 24437731 : MSCFModel_CACC::followSpeed(const MSVehicle* const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle* const pred, const CalcReason usage) const {
118 24437731 : if (myApplyDriverstate) {
119 51075 : applyHeadwayAndSpeedDifferencePerceptionErrors(veh, speed, gap2pred, predSpeed, predMaxDecel, pred);
120 : }
121 :
122 24437731 : const double desSpeed = veh->getLane()->getVehicleMaxSpeed(veh);
123 24437731 : const double vCACC = _v(veh, pred, gap2pred, speed, predSpeed, desSpeed, true, usage);
124 : // using onInsertion=true disables emergencyOverides emergency deceleration smoothing
125 24437731 : const double vSafe = maximumSafeFollowSpeed(gap2pred, speed, predSpeed, predMaxDecel, true);
126 :
127 : #if DEBUG_CACC == 1
128 : if (DEBUG_COND) {
129 : std::cout << SIMTIME << " veh=" << veh->getID() << " pred=" << Named::getIDSecure(pred)
130 : << " v=" << speed << " vL=" << predSpeed << " gap=" << gap2pred
131 : << " predDecel=" << predMaxDecel << " vCACC=" << vCACC << " vSafe=" << vSafe << "\n";
132 : }
133 : #else
134 : UNUSED_PARAMETER(pred);
135 : #endif
136 24437731 : const double speedOverride = MIN2(myEmergencyThreshold, gap2pred);
137 24437731 : if (vSafe + speedOverride < vCACC) {
138 : #if DEBUG_CACC == 1
139 : if (DEBUG_COND) {
140 : std::cout << "Apply Safe speed, override=" << speedOverride << "\n";
141 : }
142 : #endif
143 : return MAX2(0.0, vSafe + speedOverride);
144 : }
145 : return vCACC;
146 : }
147 :
148 : double
149 10412701 : MSCFModel_CACC::stopSpeed(const MSVehicle* const veh, const double speed, double gap, double decel, const CalcReason /*usage*/) const {
150 10412701 : if (myApplyDriverstate) {
151 2728 : applyHeadwayPerceptionError(veh, speed, gap);
152 : }
153 :
154 : // NOTE: This allows return of smaller values than minNextSpeed().
155 : // Only relevant for the ballistic update: We give the argument headway=TS, to assure that
156 : // the stopping position is approached with a uniform deceleration also for tau!=TS.
157 10412701 : return MIN2(maximumSafeStopSpeed(gap, decel, speed, false, veh->getActionStepLengthSecs()), maxNextSpeed(speed, veh));
158 : }
159 :
160 : double
161 26562159 : MSCFModel_CACC::getSecureGap(const MSVehicle* const veh, const MSVehicle* const pred, const double speed, const double leaderSpeed, const double leaderMaxDecel) const {
162 : // Accel in gap mode should vanish:
163 : double desSpacing;
164 26562159 : if (pred->getCarFollowModel().getModelID() != SUMO_TAG_CF_CACC) {
165 : // 0 = myGapControlGainSpeed * (leaderSpeed - speed) + myGapControlGainSpace * (g - myHeadwayTime * speed);
166 : // <=> myGapControlGainSpace * g = - myGapControlGainSpeed * (leaderSpeed - speed) + myGapControlGainSpace * myHeadwayTime * speed;
167 : // <=> g = - myGapControlGainSpeed * (leaderSpeed - speed) / myGapControlGainSpace + myHeadwayTime * speed;
168 109566 : desSpacing = acc_CFM.myGapControlGainSpeed * (speed - leaderSpeed) / acc_CFM.myGapControlGainSpace + myHeadwayTimeACC * speed; // MSCFModel_ACC::accelGapControl
169 : } else {
170 26452593 : desSpacing = myHeadwayTime * speed; // speedGapControl
171 : }
172 26562159 : const double desSpacingDefault = MSCFModel::getSecureGap(veh, pred, speed, leaderSpeed, leaderMaxDecel);
173 : #if DEBUG_CACC_SECURE_GAP == 1
174 : std::cout << SIMTIME << "MSCFModel_ACC::getSecureGap speed=" << speed << " leaderSpeed=" << leaderSpeed
175 : << " desSpacing=" << desSpacing << " desSpacingDefault=" << desSpacingDefault << "\n";
176 : #endif
177 26562159 : return MAX2(desSpacing, desSpacingDefault);
178 : }
179 :
180 :
181 : double
182 827829 : MSCFModel_CACC::insertionFollowSpeed(const MSVehicle* const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle* const pred) const {
183 : #if DEBUG_CACC_INSERTION_FOLLOW_SPEED == 1
184 : if (DEBUG_COND) {
185 : std::cout << "MSCFModel_ACC::insertionFollowSpeed(), speed=" << speed << " gap2pred=" << gap2pred << " predSpeed=" << predSpeed << "\n";
186 : }
187 : #endif
188 : // iterate to find a stationary value for
189 : // speed = followSpeed(v, speed, gap2pred, predSpeed, predMaxDecel, nullptr, CalcReason::FUTURE)
190 : const int max_iter = 50;
191 : int n_iter = 0;
192 : const double tol = 0.1;
193 : double damping = 0.8;
194 :
195 : double res = speed;
196 21649335 : while (n_iter < max_iter) {
197 : // proposed acceleration
198 21292867 : const double vCACC = _v(veh, pred, gap2pred, res, predSpeed, speed, true);
199 21292867 : const double vSafe = maximumSafeFollowSpeed(gap2pred, res, predSpeed, predMaxDecel, true);
200 21292867 : const double a = MIN2(vCACC, vSafe) - res;
201 21292867 : res = res + damping * a;
202 : #if DEBUG_CACC_INSERTION_FOLLOW_SPEED == 1
203 : if (DEBUG_COND) {
204 : std::cout << " n_iter=" << n_iter << " vSafe=" << vSafe << " vCACC=" << vCACC << " a=" << a << " damping=" << damping << " res=" << res << std::endl;
205 : }
206 : #endif
207 21292867 : damping *= 0.9;
208 21292867 : if (fabs(a) < tol) {
209 : break;
210 : } else {
211 20821506 : n_iter++;
212 : }
213 : }
214 827829 : return res;
215 : }
216 :
217 :
218 :
219 :
220 : /// @todo update interactionGap logic
221 : double
222 0 : MSCFModel_CACC::interactionGap(const MSVehicle* const /* veh */, double /* vL */) const {
223 : /*maximum radar range is CACC is enabled*/
224 0 : return 250;
225 : }
226 :
227 :
228 : std::string
229 20 : MSCFModel_CACC::getParameter(const MSVehicle* veh, const std::string& key) const {
230 : CACCVehicleVariables* vars = (CACCVehicleVariables*) veh->getCarFollowVariables();
231 :
232 20 : if (key.compare("caccCommunicationsOverrideMode") == 0) {
233 20 : return toString(vars->CACC_CommunicationsOverrideMode);
234 : }
235 :
236 0 : return "";
237 : }
238 :
239 :
240 : void
241 20 : MSCFModel_CACC::setParameter(MSVehicle* veh, const std::string& key, const std::string& value) const {
242 : CACCVehicleVariables* vars = (CACCVehicleVariables*) veh->getCarFollowVariables();
243 :
244 : try {
245 20 : if (key.compare("caccCommunicationsOverrideMode") == 0) {
246 20 : vars->CACC_CommunicationsOverrideMode = CommunicationsOverrideModeMap[value];
247 : }
248 0 : } catch (NumberFormatException&) {
249 0 : throw InvalidArgument("Invalid value '" + value + "' for parameter '" + key + "' for vehicle '" + veh->getID() + "'");
250 0 : }
251 20 : }
252 :
253 :
254 : double
255 22225683 : MSCFModel_CACC::speedSpeedControl(const double speed, double vErr, VehicleMode& vehMode) const {
256 : // Speed control law
257 22225683 : vehMode = CC_MODE;
258 22225683 : double sclAccel = mySpeedControlGain * vErr;
259 22225683 : double newSpeed = speed + ACCEL2SPEED(sclAccel);
260 22225683 : return newSpeed;
261 : }
262 :
263 : double
264 24766230 : MSCFModel_CACC::speedGapControl(const MSVehicle* const veh, const double gap2pred,
265 : const double speed, const double predSpeed, const double desSpeed, double vErr,
266 : const MSVehicle* const pred, VehicleMode& vehMode) const {
267 : // Gap control law
268 : double newSpeed = 0.0;
269 :
270 24766230 : if (pred != nullptr) {
271 23503210 : if (pred->getCarFollowModel().getModelID() != SUMO_TAG_CF_CACC) {
272 52038 : vehMode = ACC_MODE;
273 52038 : newSpeed = acc_CFM._v(veh, gap2pred, speed, predSpeed, desSpeed, true);
274 : #if DEBUG_CACC == 1
275 : if (DEBUG_COND) {
276 : std::cout << " acc control mode" << std::endl;
277 : }
278 : #endif
279 : } else {
280 : #if DEBUG_CACC == 1
281 : if (DEBUG_COND) {
282 : std::cout << " CACC control mode" << std::endl;
283 : }
284 : #endif
285 23451172 : double desSpacing = myHeadwayTime * speed;
286 23451172 : double spacingErr = gap2pred - desSpacing;
287 23451172 : double accel = veh->getAcceleration();
288 23451172 : double speedErr = predSpeed - speed + myHeadwayTime * accel;
289 :
290 23451172 : if ((spacingErr > 0 && spacingErr < 0.2) && (vErr < 0.1)) {
291 : // gap mode
292 : //newSpeed = speed + 0.45 * spacingErr + 0.0125 *speedErr;
293 : #if DEBUG_CACC == 1
294 : if (DEBUG_COND) {
295 : std::cout << " applying gap control: err=" << spacingErr << " speedErr=" << speedErr << std::endl;
296 : }
297 : #endif
298 670850 : newSpeed = speed + myGapControlGainGap * spacingErr + myGapControlGainGapDot * speedErr;
299 :
300 670850 : vehMode = CACC_GAP_MODE;
301 22780322 : } else if (spacingErr < 0) {
302 : // collision avoidance mode
303 : //newSpeed = speed + 0.45 * spacingErr + 0.05 *speedErr;
304 : #if DEBUG_CACC == 1
305 : if (DEBUG_COND) {
306 : std::cout << " applying collision avoidance: err=" << spacingErr << " speedErr=" << speedErr << "\n";
307 : }
308 : #endif
309 5115039 : newSpeed = speed + myCollisionAvoidanceGainGap * spacingErr + myCollisionAvoidanceGainGapDot * speedErr;
310 5115039 : vehMode = CACC_COLLISION_AVOIDANCE_MODE;
311 : } else {
312 : // gap closing mode
313 : #if DEBUG_CACC == 1
314 : if (DEBUG_COND) {
315 : std::cout << " applying gap closing err=" << spacingErr << " speedErr=" << speedErr << " predSpeed=" << predSpeed << " speed=" << speed << " accel=" << accel << "\n";
316 : }
317 : #endif
318 17665283 : newSpeed = speed + myGapClosingControlGainGap * spacingErr + myGapClosingControlGainGapDot * speedErr;
319 :
320 17665283 : vehMode = CACC_GAP_CLOSING_MODE;
321 : }
322 : }
323 : } else {
324 : /* no leader */
325 : #if DEBUG_CACC == 1
326 : if (DEBUG_COND) {
327 : std::cout << " no leader" << std::endl;
328 : }
329 : #endif
330 1263020 : newSpeed = speedSpeedControl(speed, vErr, vehMode);
331 : }
332 :
333 24766230 : return newSpeed;
334 : }
335 :
336 : double
337 45730598 : MSCFModel_CACC::_v(const MSVehicle* const veh, const MSVehicle* const pred, const double gap2pred, const double speed,
338 : const double predSpeed, const double desSpeed, const bool /* respectMinGap */, const CalcReason usage) const {
339 : double newSpeed = 0.0;
340 45730598 : VehicleMode vehMode = CC_MODE;
341 :
342 : #if DEBUG_CACC == 1
343 : if (DEBUG_COND) {
344 : std::cout << SIMTIME << " MSCFModel_CACC::_v() for veh '" << veh->getID()
345 : << " gap=" << gap2pred << " speed=" << speed << " predSpeed=" << predSpeed
346 : << " desSpeed=" << desSpeed << std::endl;
347 : }
348 : #endif
349 :
350 : /* Velocity error */
351 45730598 : double vErr = speed - desSpeed;
352 : bool setControlMode = false;
353 : CACCVehicleVariables* vars = (CACCVehicleVariables*)veh->getCarFollowVariables();
354 45730598 : if (vars->lastUpdateTime != MSNet::getInstance()->getCurrentTimeStep()) {
355 4109644 : vars->lastUpdateTime = MSNet::getInstance()->getCurrentTimeStep();
356 : setControlMode = true;
357 : }
358 :
359 45730598 : CommunicationsOverrideMode commMode = vars->CACC_CommunicationsOverrideMode;
360 :
361 45730598 : if (commMode == CACC_NO_OVERRIDE) { // old CACC logic (rely on time gap from predecessor)
362 : // @note: using (gap2pred + minGap) here increases oscillations but may
363 : // actually be a good idea once the acceleration-spike-problem is fixed
364 45728043 : double time_gap = gap2pred / MAX2(NUMERICAL_EPS, speed);
365 45728043 : double spacingErr = gap2pred - myHeadwayTime * speed;
366 :
367 45728043 : if (time_gap > 2 && spacingErr > mySpeedControlMinGap) {
368 : #if DEBUG_CACC == 1
369 : if (DEBUG_COND) {
370 : std::cout << " applying speedControl" << " time_gap=" << time_gap << std::endl;
371 : }
372 : #endif
373 : // Find acceleration - Speed control law
374 15873956 : newSpeed = speedSpeedControl(speed, vErr, vehMode);
375 : // Set cl to vehicle parameters
376 15873956 : if (setControlMode) {
377 826687 : vars->CACC_ControlMode = 0;
378 : }
379 29854087 : } else if (time_gap < 1.5) {
380 : // Find acceleration - Gap control law
381 : #if DEBUG_CACC == 1
382 : if (DEBUG_COND) {
383 : std::cout << " speedGapControl" << " time_gap=" << time_gap << std::endl;
384 : }
385 : #endif
386 :
387 21623399 : newSpeed = speedGapControl(veh, gap2pred, speed, predSpeed, desSpeed, vErr, pred, vehMode);
388 : // Set cl to vehicle parameters
389 21623399 : if (setControlMode) {
390 1374546 : vars->CACC_ControlMode = 1;
391 : }
392 : } else {
393 : // Follow previous applied law
394 8230688 : int cm = vars->CACC_ControlMode;
395 8230688 : if (!cm) {
396 : // CACC_ControlMode = speed control
397 :
398 : #if DEBUG_CACC == 1
399 : if (DEBUG_COND) {
400 : std::cout << " applying speedControl (previous)" << " time_gap=" << time_gap << std::endl;
401 : }
402 : #endif
403 5087857 : newSpeed = speedSpeedControl(speed, vErr, vehMode);
404 : } else {
405 : // CACC_ControlMode = gap control
406 : #if DEBUG_CACC == 1
407 : if (DEBUG_COND) {
408 : std::cout << " previous speedGapControl (previous)" << " time_gap=" << time_gap << std::endl;
409 : }
410 : #endif
411 3142831 : newSpeed = speedGapControl(veh, gap2pred, speed, predSpeed, desSpeed, vErr, pred, vehMode);
412 : }
413 : }
414 2555 : } else if (commMode == CACC_MODE_NO_LEADER) {
415 850 : newSpeed = speedSpeedControl(speed, vErr, vehMode);
416 1705 : } else if (commMode == CACC_MODE_LEADER_NO_CAV) {
417 850 : newSpeed = acc_CFM._v(veh, gap2pred, speed, predSpeed, desSpeed, true);
418 850 : vehMode = ACC_MODE;
419 855 : } else if (commMode == CACC_MODE_LEADER_CAV) {
420 855 : double desSpacing = myHeadwayTime * speed;
421 855 : double spacingErr = gap2pred - desSpacing;
422 855 : double accel = veh->getAcceleration();
423 855 : double speedErr = predSpeed - speed + myHeadwayTime * accel;
424 :
425 855 : if ((spacingErr > 0 && spacingErr < 0.2) && (vErr < 0.1)) {
426 : // gap mode
427 : //newSpeed = speed + 0.45 * spacingErr + 0.0125 *speedErr;
428 420 : if (DEBUG_COND) {
429 : std::cout << " applying CACC_GAP_MODE " << std::endl;
430 : }
431 420 : newSpeed = speed + myGapControlGainGap * spacingErr + myGapControlGainGapDot * speedErr;
432 420 : vehMode = CACC_GAP_MODE;
433 435 : } else if (spacingErr < 0) {
434 : // collision avoidance mode
435 : //newSpeed = speed + 0.45 * spacingErr + 0.05 *speedErr;
436 40 : if (DEBUG_COND) {
437 : std::cout << " applying CACC_COLLISION_AVOIDANCE_MODE " << std::endl;
438 : }
439 40 : newSpeed = speed + myCollisionAvoidanceGainGap * spacingErr + myCollisionAvoidanceGainGapDot * speedErr;
440 40 : vehMode = CACC_COLLISION_AVOIDANCE_MODE;
441 : } else {
442 : // gap closing mode
443 395 : if (DEBUG_COND) {
444 : std::cout << " applying CACC_GAP_CLOSING_MODE " << std::endl;
445 : }
446 395 : newSpeed = speed + myGapClosingControlGainGap * spacingErr + myGapClosingControlGainGapDot * speedErr;
447 395 : vehMode = CACC_GAP_CLOSING_MODE;
448 : }
449 : }
450 :
451 45730598 : if (setControlMode && !MSGlobals::gComputeLC && usage == CalcReason::CURRENT) {
452 8202870 : const_cast<SUMOVehicleParameter&>(veh->getParameter()).setParameter("caccVehicleMode", VehicleModeNames[vehMode]);
453 : }
454 :
455 : //std::cout << veh->getID() << " commMode: " << commMode << ", caccVehicleMode: " << VehicleModeNames[vehMode]
456 : // << ", gap2pred: " << gap2pred << ", newSpeed: " << newSpeed << std::endl;
457 :
458 : // newSpeed is meant for step length 0.1 but this would cause extreme
459 : // accelerations at lower step length
460 : double newSpeedScaled = newSpeed;
461 45730598 : if (DELTA_T < 100) {
462 49788 : const double accel01 = (newSpeed - speed) * 10;
463 49788 : newSpeedScaled = speed + ACCEL2SPEED(accel01);
464 : }
465 :
466 : #if DEBUG_CACC == 1
467 : if (DEBUG_COND) {
468 : std::cout << " result: rawAccel=" << SPEED2ACCEL(newSpeed - speed) << " newSpeed=" << newSpeed << " newSpeedScaled=" << newSpeedScaled << "\n";
469 : }
470 : #endif
471 :
472 45730598 : return MAX2(0., newSpeedScaled);
473 : }
474 :
475 :
476 :
477 : MSCFModel*
478 0 : MSCFModel_CACC::duplicate(const MSVehicleType* vtype) const {
479 0 : return new MSCFModel_CACC(vtype);
480 : }
|
