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