carmaker/matlab联合仿真(三) 生成车道线点，拟合多项式方程carmaker/matlab联合仿真(三) 生成车道线点，拟合多项式方程

96 阅读 0 评论 64 点赞

我是靠谱客的博主直率烤鸡，这篇文章主要介绍carmaker/matlab联合仿真(三) 生成车道线点，拟合多项式方程carmaker/matlab联合仿真(三) 生成车道线点，拟合多项式方程，现在分享给大家，希望可以做个参考。

carmaker/matlab联合仿真(三) 生成车道线点，拟合多项式方程

win10
matlab 2020b
carmaker 10.2.2

在carmaker/matlab仿真中，carmaker提供了摄像头、雷达等传感器模型，能够将场景中在传感器覆盖范围内的目标传送到simulink中供给算法模块利用。但是对于车道线信息，Line Sensor只能将车道类型、距离等属性信息，缺少对于车道线本身点的位置信息。而对于有横向控制的算法功能如LKA等就需要提供车道线的点或者车道线拟合成的多项式方程。对于这种情况，可以通过carmaker的拓展功能将当前车道左右侧车道线的点以UAQ的形式传送到simulink中，再做进一步的拟合，以便后续功能模块使用。
还是以前本系列前两篇内容中生成的工程文件为基础做演示。
1 由于在第一篇中创建工程时只勾选了Carmaker for Simulink Extras的选项，文件夹中缺少拓展的模块。所以需要重新在创建一次工程以补充缺少的模块。在工程文件夹下打开的simulink模型中通过点击GUI图标打开carmaker主界面。点击File -> Project Folder -> Create Project，将工程文件夹切换到之前建立的文件夹处，然后勾选所有五个选项，点击OK。警告框中选择Continue。此时在项目文件夹下会多出来一个src文件夹。
在这里插入图片描述
2 打开工程文件夹下的src文件夹，发现这是一个visual studio的工程文件。使用visual studio打开CarMaker.sln文件。选择项目->重定目标解决方案。在跳出的重定向目标中点击确定。点击生成->生成解决方案，在输出框中会出现生成成功的字样。
在这里插入图片描述

在这里插入图片描述

3 将下面三个文件放到src文件夹下，替换原来的文件。
User.c

复制代码

/*
******************************************************************************
**  CarMaker - Version 10.2.2
**  Vehicle Dynamics Simulation Toolkit
**
**  Copyright (C)   IPG Automotive GmbH
**                  Bannwaldallee 60             Phone  +49.721.98520.0
**                  76185 Karlsruhe              Fax    +49.721.98520.99
**                  Germany                      WWW    www.ipg-automotive.com
******************************************************************************
**
** Functions
** ---------
**
** Initialization
**
**	User_Init_First ()
**	User_PrintUsage ()
**	User_ScanCmdLine ()
**
**	User_AppLogFilter ()
**
**	User_Init ()
**	User_Register ()
**	User_DeclQuants ()
**
**	User_Param_Add ()
**	User_Param_Get ()
**
**
** Main TestRun Start/End:
**
**	User_TestRun_Start_atBegin ()
**	User_TestRun_Start_atEnd ()
**	User_TestRun_Start_StaticCond_Calc ()
**	User_TestRun_Start_Finalize ()
**	User_TestRun_RampUp ()
**
**	User_TestRun_End_First ()
**	User_TestRun_End ()
**
**
** Main Cycle:
**
**	User_In ()
**
**	User_DrivMan_Calc ()
** 	User_Traffic_Calc ()
**	User_VehicleControl_Calc ()
**	User_Brake_Calc ()           in Vhcl_Calc ()
**	User_Calc ()
**	User_Check_IsIdle ()
**
**	User_Out ()
**
**
** APO Communication:
**
**	User_ApoMsg_Eval ()
**	User_ApoMsg_Send ()
**
**	User_ShutDown ()
**	User_End ()
**	User_Cleanup ()
**
**
******************************************************************************
*/

#include <Global.h>

#if defined(WIN32)
# include <windows.h>
#endif

#include <stdlib.h>
#include <string.h>
#include <math.h>

#if defined(XENO)
# include <mio.h>
#endif
# include <ioconf.h>

#include <CarMaker.h>
#include <Car/Vehicle_Car.h>

#include <ADASRP.h>

#include <rbs.h>

#include "IOVec.h"
#include "User.h"

/* @@PLUGIN-BEGIN-INCLUDE@@ - Automatically generated code - don't edit! */
/* @@PLUGIN-END@@ */
// Add the following inclusion at the beginning of the User.c file
#include "Lineds.h"

int UserCalcCalledByAppTestRunCalc = 0;

tUser	User;
//Add the following declaration at the beginning of the User.c file
tLds Lds;

/*
** User_Init_First ()
**
** First, low level initialization of the User module
**
** Call:
** - one times at start of program
** - no realtime conditions
**
*/

int
User_Init_First (void)
{
    memset (&User, 0, sizeof(User));

return 0;
}

/*
** User_PrintUsage ()
**
** Print the user/application specific programm arguments
*/

void
User_PrintUsage (const char *Pgm)
{
    /* REMARK: 1 log statement for each usage line, no line breaks */
    LogUsage("n");
    LogUsage("Usage: %s [options] [testrun]n", Pgm);
    LogUsage("Options:n");

#if defined(CM_HIL)
    {
	const tIOConfig *cf;
	const char *defio = IO_GetDefault();
	LogUsage(" -io %-12s Default I/O configuration (%s)n", "default",
	    (defio!=NULL && strcmp(defio, "none")!=0) ? defio : "minimal I/O");
	for (cf=IO_GetConfigurations(); cf->Name!=NULL; cf++)
	    LogUsage(" -io %-12s %sn", cf->Name, cf->Description);
    }
#endif
}

/*
** User_ScanCmdLine ()
**
** Scan application specific command line arguments
**
** Return:
** - argv: last unscanned argument
** - NULL: error or unknown argument
*/

char **
User_ScanCmdLine (int argc, char **argv)
{
    const char *Pgm = argv[0];

/* I/O configuration to be used in case no configuration was
       specified on the command line. */
    IO_SelectDefault("default" /* or "demoapp", "demorbs,demofr" etc. */);

while (*++argv) {
	if (strcmp(*argv, "-io") == 0 && argv[1] != NULL) {
	    if (IO_Select(*++argv) != 0)
		return NULL;
	} else if (strcmp(*argv, "-h") == 0 || strcmp(*argv, "-help") == 0) {
	    User_PrintUsage(Pgm);
	    SimCore_PrintUsage(Pgm); /* Possible exit(), depending on CM-platform! */
	    return  NULL;
	} else if ((*argv)[0] == '-') {
	    LogErrF(EC_General, "Unknown option '%s'", *argv);
	    return NULL;
	} else {
	    break;
	}
    }

return argv;
}

/*
** User_Init ()
**
** Basic initialization of the module User.o
**
** Call:
** - once at program start
** - no realtime conditions
*/

int
User_Init (void)
{
    return 0;
}

int
User_Register (void)
{

/* @@PLUGIN-BEGIN-REGISTER@@ - Automatically generated code - don't edit! */
    /* @@PLUGIN-END@@ */

return 0;
}

/*
** User_DeclQuants ()
**
** Add user specific quantities to the dictionary
**
** Call:
** - once at program start
** - no realtime conditions
*/

void
User_DeclQuants (void)
{
    int i;
    // in the function User_DeclQuants (void), add the following lines:

Lineds_DeclQuants();
    for (i=0; i<N_USEROUTPUT; i++) {
	char sbuf[32];
	sprintf (sbuf, "UserOut_%02d", i);
	DDefDouble (NULL, sbuf, "", &User.Out[i], DVA_IO_Out);
    }
    RBS_DeclQuants();
}

/*
** User_Param_Add ()
**
** Update all modified application specific parameters in the test stand
** parameter file (ECUParameters).
**
** If the variable SimCore.TestRig.ECUParam.Modified set to 1 somewhere else
** CarMaker calls this function to let the user add or change all necessary
** entries before the file is written.
** So, if writing the ECUParam file is necessary, set ECUParam.Modified to 1.
** The next TestRun start or end, CarMaker calls this function and writes
** the file to the harddisk.
**
** Call:
** - in a separate thread (no realtime contitions)
** - when starting a new test run
*/

int
User_Param_Add (void)
{
#if defined(CM_HIL)
    /* ECU parameters */
    if (SimCore.TestRig.ECUParam.Inf == NULL)
	return -1;
#endif

return 0;
}

/*
** User_Param_Get ()
**
** Update all modified application specific parameters from the test stand
** parameter file (ECUParameters).
**
** Call:
** - in a separate thread (no realtime conditions)
** - if User_Param_Get() wasn't called
** - when starting a new test run, if
**   - the files SimParameters and/or
**   - ECUParameters
**   are modified since last reading
**
** return values:
**  0	ok
** -1	no testrig parameter file
** -2	testrig parameter error
** -3	i/o configuration specific error
** -4	no simulation parameters
** -5	simulation parameters error
** -6	FailSafeTester parameter/init error
*/

int
User_Param_Get (void)
{
    int rv = 0;

#if defined(CM_HIL)
    /*** testrig / ECU parameters */
    if (SimCore.TestRig.ECUParam.Inf == NULL)
	return -1;

if (IO_Param_Get(SimCore.TestRig.ECUParam.Inf) != 0)
	rv = -2;
#endif

/*** simulation parameters */
    if (SimCore.TestRig.SimParam.Inf == NULL)
	return -4;

return rv;
}

/*
** User_TestRun_Start_atBegin ()
**
** Special things before a new simulation starts like
** - reset user variables to their default values
** - reset counters
** - ...
**
** Call:
** - in separate thread (no realtime conditions)
** - when starting a new test run
** - after (standard) infofiles are read in
** - before reading parameters for Environment, DrivMan, Car, ...
**   the models are NOT in the simulation-can-start-now state
**   (after Start(), before StaticCond())
*/

int
User_TestRun_Start_atBegin (void)
{
    int rv = 0;
    int i;

for (i=0; i<N_USEROUTPUT; i++)
	User.Out[i] = 0.0;

if (IO_None)
	return rv;

#if defined(CM_HIL)
    if (FST_New(SimCore.TestRig.ECUParam.Inf) != 0)
	rv = -6;
#endif

return rv;
}

/*
** User_TestRun_Start_atEnd ()
**
** Special things before a new simulation starts like
** - reset user variables to there default values
** - reset counters
** - ...
**
** Call:
** - in separate thread (no realtime conditions)
** - when starting a new test run
** - at the end, behind reading parameters for Environment, DrivMan,
**   Car, ...
**   the models are NOT in the simulation-can-start-now state
**   (after Start(), before StaticCond())
*/

int
User_TestRun_Start_atEnd (void)
{
    int rv = 0;

rv = Lineds_TestRun_Start_atEnd();

#if defined(XENO)
    IOConf_MapQuants();
#endif
    return rv;
}

/*
** User_TestRun_Start_StaticCond_Calc ()
**
** called in non RT context
*/

int
User_TestRun_Start_StaticCond_Calc (void)
{
    return 0;
}

/*
** User_TestRun_Start_Finalize ()
**
** called in RT context
*/

int
User_TestRun_Start_Finalize (void)
{
    return 0;
}

/*
** User_TestRun_RampUp ()
**
** Perform a smooth transition of variables (e.g. I/O)
** from their current state  to the new testrun.
** This function is called repeatedly, once during each cycle, until
** it returns true (or issues an error message), so the function should
** return true if transitioning is done, false otherwise.
**
** In case of an error the function should issue an apropriate
** error message and return false;
**
** Called in RT context, in state SCState_StartSim,
** after preprocessing is done, before starting the engine.
** Please note, that in this early initialization state no calculation
** of the vehicle model takes place.
*/

int
User_TestRun_RampUp (double dt)
{
    int IsReady = 1;

return IsReady;
}

/*
** User_TestRun_End_First ()
**
** Invoked immediately after the end of a simulation is initiated,
** but before data storage ends and before transitioning into SCState_Idle.
** - Send Scratchpad-note
** - ...
**
** Call:
** - in main task, in the main loop (real-time conditions!)
** - when a test run is finished (SimCore.State is SCState_End)
*/

int
User_TestRun_End_First (void)
{
    return 0;
}

/*
** User_TestRun_End ()
**
** Special things after the end of a simulation like
** - switch off an air compressor
** - Write something to a file
** - ...
**
** Call:
** - in separate thread (no realtime conditions)
** - when a test run is finished (SimCore.State is SCState_End<xyz>)
*/

int
User_TestRun_End (void)
{
    return 0;
}

/*
** User_In ()
**
** Assign quantities of the i/o vector to model variables
**
** Call:
** - in the main loop
** - pay attention to realtime condition
** - just after IO_In()
*/

void
User_In (const unsigned CycleNo)
{
    if (SimCore.State != SCState_Simulate)
	return;
}

/*
** User_DrivMan_Calc ()
**
** called
** - in RT context
** - after DrivMan_Calc()
*/

int
User_DrivMan_Calc (double dt)
{
    /* Rely on the Vehicle Operator within DrivMan module to get
       the vehicle in driving state using the IPG's
       PowerTrain Control model 'Generic' or similar */
    if (Vehicle.OperationState != OperState_Driving)
	return 0;

return 0;
}

/*
** User_VehicleControl_Calc ()
**
** called
** - in RT context
** - after VehicleControl_Calc()
*/

int
User_VehicleControl_Calc (double dt)
{
    /* Rely on the Vehicle Operator within DrivMan module to get
       the vehicle in driving state using the IPG's
       PowerTrain Control model 'Generic' or similar */
    if (Vehicle.OperationState != OperState_Driving)
	return 0;

return 0;
}

/*
** User_Brake_Calc ()
**
** called
** - in RT context
** - after Brake_Calc() in Vhcl_Calc()
*/

int
User_Brake_Calc (double dt)
{
    /* Modify the total brake torque from the brake system model Brake.Trq_tot[]
       or the target drive source torque from the brake control unit
       Brake.HydBrakeCU_IF.Trq_DriveSrc_trg[]
    */

return 0;
}

/*
** User_Traffic_Calc ()
**
** called
** - in RT context
** - after Traffic_Calc()
*/

int
User_Traffic_Calc (double dt)
{
    if (SimCore.State != SCState_Simulate)
	return 0;

return 0;
}

/*
** User_Calc ()
**
** called in RT context
*/

int
User_Calc (double dt)
{
    /* Starting with CM 6.0 User_Calc() will be invoked in EVERY simulation
       state. Uncomment the following line in order to restore the behaviour
       of CM 5.1 and earlier. */
    /*if (!UserCalcCalledByAppTestRunCalc) return 0;*/

int rv;
    rv = Lineds_Calc(dt);

return rv;
}

/*
** User_Check_IsIdle ()
**
** Checking, if the simulation model is in idle conditions (stand still,
** steeringwheel angle zero, cluch pedal pressed, ...).
** If reached idle state, the calculation of vehicle model and driving
** manoevers is stopped.
** Ready for start new simulation.
**
** Return:
** 1  idle state reached
** 0  else
**
** Call:
** - in main task, in the main loop
** - pay attention to realtime condition
** - while SimCore.State==SCState_EndIdleGet
*/

int
User_Check_IsIdle (int IsIdle)
{
    double val;

/*** ECU / carmodel signals */

/* vehicle and wheels: stand still */
    val = 0.5*kmh2ms;
    if (Vehicle.v > val
     || fabs(Vehicle.Wheel[0]->vBelt) > val || fabs(Vehicle.Wheel[1]->vBelt) > val
     || fabs(Vehicle.Wheel[2]->vBelt) > val || fabs(Vehicle.Wheel[3]->vBelt) > val) {
	IsIdle = 0;
    }

/* SteerAngle: drive  straight forward position */
    val = 1.0*deg2rad;
    if (Vehicle.Steering.Ang > val || Vehicle.Steering.Ang < -val)
	IsIdle = 0;

return IsIdle;
}

/*
** User_Out ()
**
** Assigns model quantities to variables of the i/o vector
**
** call:
** - in the main loop
** - pay attention to realtime condition
** - just before IO_Out();
*/

void
User_Out (const unsigned CycleNo)
{
    RBS_OutMap(CycleNo);

if (SimCore.State != SCState_Simulate)
	return;
}

/*
** User_ApoMsg_Eval ()
**
** Communication between the application and connected GUIs.
** Evaluate messages from GUIs
**
** Call:
** - in the main loop
** - pay attention to realtime condition
** - near the end of the main loop, if the function SimCore_ApoMsg_Eval()
**    skips the message
**
** Return:
**   0 : message evaluated
**  -1 : message not handled
*/

int
User_ApoMsg_Eval (int Ch, char *Msg, int len, int who)
{
#if defined(CM_HIL)
    /*** FailSafeTester */
    if (Ch == ApoCh_CarMaker) {
	if (FST_ApoMsgEval(Ch, Msg, len) <= 0)
	    return 0;
    }
#endif
    return -1;
}

/*
** User_ApoMsg_Send ()
**
** Communication between the application and connected GUIs.
** Sends messages to GUIs
**
** Call:
** - near the end of the main loop, in MainThread_FinishCycle()
** - pay attention to realtime condition
*/

void
User_ApoMsg_Send (double T, const unsigned CycleNo)
{
}

/*
** User_ShutDown ()
**
** Prepare application for shut down
**
** Call:
** - at end of program
** - no realtime conditions
*/

int
User_ShutDown (int ShutDownForced)
{
    int IsDown = 0;

/* Prepare application for shutdown and return that
       shutdown conditions are reached */
    if (1) {
	IsDown = 1;
    }

return IsDown;
}

/*
** User_End ()
**
** End all models of the user module
**
** Call:
** - one times at end of program
** - no realtime conditions
*/

int
User_End (void)
{
    return 0;
}

/*
** User_Cleanup ()
**
** Cleanup function of the User module
**
** Call:
** - one times at end of program, just before exit
** - no realtime conditions
*/

void
User_Cleanup (void)
{
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
/*
******************************************************************************
**  CarMaker - Version 10.2.2
**  Vehicle Dynamics Simulation Toolkit
**
**  Copyright (C)   IPG Automotive GmbH
**                  Bannwaldallee 60             Phone  +49.721.98520.0
**                  76185 Karlsruhe              Fax    +49.721.98520.99
**                  Germany                      WWW    www.ipg-automotive.com
******************************************************************************
**
** Functions
** ---------
**
** Initialization
**
**	User_Init_First ()
**	User_PrintUsage ()
**	User_ScanCmdLine ()
**
**	User_AppLogFilter ()
**
**	User_Init ()
**	User_Register ()
**	User_DeclQuants ()
**
**	User_Param_Add ()
**	User_Param_Get ()
**
**
** Main TestRun Start/End:
**
**	User_TestRun_Start_atBegin ()
**	User_TestRun_Start_atEnd ()
**	User_TestRun_Start_StaticCond_Calc ()
**	User_TestRun_Start_Finalize ()
**	User_TestRun_RampUp ()
**
**	User_TestRun_End_First ()
**	User_TestRun_End ()
**
**
** Main Cycle:
**
**	User_In ()
**
**	User_DrivMan_Calc ()
** 	User_Traffic_Calc ()
**	User_VehicleControl_Calc ()
**	User_Brake_Calc ()           in Vhcl_Calc ()
**	User_Calc ()
**	User_Check_IsIdle ()
**
**	User_Out ()
**
**
** APO Communication:
**
**	User_ApoMsg_Eval ()
**	User_ApoMsg_Send ()
**
**	User_ShutDown ()
**	User_End ()
**	User_Cleanup ()
**
**
******************************************************************************
*/

#include <Global.h>

#if defined(WIN32)
# include <windows.h>
#endif

#include <stdlib.h>
#include <string.h>
#include <math.h>

#if defined(XENO)
# include <mio.h>
#endif
# include <ioconf.h>

#include <CarMaker.h>
#include <Car/Vehicle_Car.h>

#include <ADASRP.h>

#include <rbs.h>

#include "IOVec.h"
#include "User.h"

/* @@PLUGIN-BEGIN-INCLUDE@@ - Automatically generated code - don't edit! */
/* @@PLUGIN-END@@ */
// Add the following inclusion at the beginning of the User.c file
#include "Lineds.h"

int UserCalcCalledByAppTestRunCalc = 0;


tUser	User;
//Add the following declaration at the beginning of the User.c file
tLds Lds;


/*
** User_Init_First ()
**
** First, low level initialization of the User module
**
** Call:
** - one times at start of program
** - no realtime conditions
**
*/

int
User_Init_First (void)
{
    memset (&User, 0, sizeof(User));

    return 0;
}



/*
** User_PrintUsage ()
**
** Print the user/application specific programm arguments
*/

void
User_PrintUsage (const char *Pgm)
{
    /* REMARK: 1 log statement for each usage line, no line breaks */
    LogUsage("n");
    LogUsage("Usage: %s [options] [testrun]n", Pgm);
    LogUsage("Options:n");

#if defined(CM_HIL)
    {
	const tIOConfig *cf;
	const char *defio = IO_GetDefault();
	LogUsage(" -io %-12s Default I/O configuration (%s)n", "default",
	    (defio!=NULL && strcmp(defio, "none")!=0) ? defio : "minimal I/O");
	for (cf=IO_GetConfigurations(); cf->Name!=NULL; cf++)
	    LogUsage(" -io %-12s %sn", cf->Name, cf->Description);
    }
#endif
}



/*
** User_ScanCmdLine ()
**
** Scan application specific command line arguments
**
** Return:
** - argv: last unscanned argument
** - NULL: error or unknown argument
*/

char **
User_ScanCmdLine (int argc, char **argv)
{
    const char *Pgm = argv[0];

    /* I/O configuration to be used in case no configuration was
       specified on the command line. */
    IO_SelectDefault("default" /* or "demoapp", "demorbs,demofr" etc. */);

    while (*++argv) {
	if (strcmp(*argv, "-io") == 0 && argv[1] != NULL) {
	    if (IO_Select(*++argv) != 0)
		return NULL;
	} else if (strcmp(*argv, "-h") == 0 || strcmp(*argv, "-help") == 0) {
	    User_PrintUsage(Pgm);
	    SimCore_PrintUsage(Pgm); /* Possible exit(), depending on CM-platform! */
	    return  NULL;
	} else if ((*argv)[0] == '-') {
	    LogErrF(EC_General, "Unknown option '%s'", *argv);
	    return NULL;
	} else {
	    break;
	}
    }

    return argv;
}



/*
** User_Init ()
**
** Basic initialization of the module User.o
**
** Call:
** - once at program start
** - no realtime conditions
*/

int
User_Init (void)
{
    return 0;
}



int
User_Register (void)
{

    /* @@PLUGIN-BEGIN-REGISTER@@ - Automatically generated code - don't edit! */
    /* @@PLUGIN-END@@ */

    return 0;
}



/*
** User_DeclQuants ()
**
** Add user specific quantities to the dictionary
**
** Call:
** - once at program start
** - no realtime conditions
*/

void
User_DeclQuants (void)
{
    int i;
    // in the function User_DeclQuants (void), add the following lines:

    Lineds_DeclQuants();
    for (i=0; i<N_USEROUTPUT; i++) {
	char sbuf[32];
	sprintf (sbuf, "UserOut_%02d", i);
	DDefDouble (NULL, sbuf, "", &User.Out[i], DVA_IO_Out);
    }
    RBS_DeclQuants();
}


/*
** User_Param_Add ()
**
** Update all modified application specific parameters in the test stand
** parameter file (ECUParameters).
**
** If the variable SimCore.TestRig.ECUParam.Modified set to 1 somewhere else
** CarMaker calls this function to let the user add or change all necessary
** entries before the file is written.
** So, if writing the ECUParam file is necessary, set ECUParam.Modified to 1.
** The next TestRun start or end, CarMaker calls this function and writes
** the file to the harddisk.
**
** Call:
** - in a separate thread (no realtime contitions)
** - when starting a new test run
*/

int
User_Param_Add (void)
{
#if defined(CM_HIL)
    /* ECU parameters */
    if (SimCore.TestRig.ECUParam.Inf == NULL)
	return -1;
#endif

    return 0;
}



/*
** User_Param_Get ()
**
** Update all modified application specific parameters from the test stand
** parameter file (ECUParameters).
**
** Call:
** - in a separate thread (no realtime conditions)
** - if User_Param_Get() wasn't called
** - when starting a new test run, if
**   - the files SimParameters and/or
**   - ECUParameters
**   are modified since last reading
**
** return values:
**  0	ok
** -1	no testrig parameter file
** -2	testrig parameter error
** -3	i/o configuration specific error
** -4	no simulation parameters
** -5	simulation parameters error
** -6	FailSafeTester parameter/init error
*/

int
User_Param_Get (void)
{
    int rv = 0;

#if defined(CM_HIL)
    /*** testrig / ECU parameters */
    if (SimCore.TestRig.ECUParam.Inf == NULL)
	return -1;

    if (IO_Param_Get(SimCore.TestRig.ECUParam.Inf) != 0)
	rv = -2;
#endif

    /*** simulation parameters */
    if (SimCore.TestRig.SimParam.Inf == NULL)
	return -4;

    return rv;
}



/*
** User_TestRun_Start_atBegin ()
**
** Special things before a new simulation starts like
** - reset user variables to their default values
** - reset counters
** - ...
**
** Call:
** - in separate thread (no realtime conditions)
** - when starting a new test run
** - after (standard) infofiles are read in
** - before reading parameters for Environment, DrivMan, Car, ...
**   the models are NOT in the simulation-can-start-now state
**   (after Start(), before StaticCond())
*/

int
User_TestRun_Start_atBegin (void)
{
    int rv = 0;
    int i;

    for (i=0; i<N_USEROUTPUT; i++)
	User.Out[i] = 0.0;


    if (IO_None)
	return rv;

#if defined(CM_HIL)
    if (FST_New(SimCore.TestRig.ECUParam.Inf) != 0)
	rv = -6;
#endif

    return rv;
}




/*
** User_TestRun_Start_atEnd ()
**
** Special things before a new simulation starts like
** - reset user variables to there default values
** - reset counters
** - ...
**
** Call:
** - in separate thread (no realtime conditions)
** - when starting a new test run
** - at the end, behind reading parameters for Environment, DrivMan,
**   Car, ...
**   the models are NOT in the simulation-can-start-now state
**   (after Start(), before StaticCond())
*/

int
User_TestRun_Start_atEnd (void)
{
    int rv = 0;

    rv = Lineds_TestRun_Start_atEnd();

    
#if defined(XENO)
    IOConf_MapQuants();
#endif
    return rv;
}



/*
** User_TestRun_Start_StaticCond_Calc ()
**
** called in non RT context
*/

int
User_TestRun_Start_StaticCond_Calc (void)
{
    return 0;
}



/*
** User_TestRun_Start_Finalize ()
**
** called in RT context
*/

int
User_TestRun_Start_Finalize (void)
{
    return 0;
}



/*
** User_TestRun_RampUp ()
**
** Perform a smooth transition of variables (e.g. I/O)
** from their current state  to the new testrun.
** This function is called repeatedly, once during each cycle, until
** it returns true (or issues an error message), so the function should
** return true if transitioning is done, false otherwise.
**
** In case of an error the function should issue an apropriate
** error message and return false;
**
** Called in RT context, in state SCState_StartSim,
** after preprocessing is done, before starting the engine.
** Please note, that in this early initialization state no calculation
** of the vehicle model takes place.
*/

int
User_TestRun_RampUp (double dt)
{
    int IsReady = 1;

    return IsReady;
}



/*
** User_TestRun_End_First ()
**
** Invoked immediately after the end of a simulation is initiated,
** but before data storage ends and before transitioning into SCState_Idle.
** - Send Scratchpad-note
** - ...
**
** Call:
** - in main task, in the main loop (real-time conditions!)
** - when a test run is finished (SimCore.State is SCState_End)
*/

int
User_TestRun_End_First (void)
{
    return 0;
}



/*
** User_TestRun_End ()
**
** Special things after the end of a simulation like
** - switch off an air compressor
** - Write something to a file
** - ...
**
** Call:
** - in separate thread (no realtime conditions)
** - when a test run is finished (SimCore.State is SCState_End<xyz>)
*/

int
User_TestRun_End (void)
{
    return 0;
}



/*
** User_In ()
**
** Assign quantities of the i/o vector to model variables
**
** Call:
** - in the main loop
** - pay attention to realtime condition
** - just after IO_In()
*/

void
User_In (const unsigned CycleNo)
{
    if (SimCore.State != SCState_Simulate)
	return;
}



/*
** User_DrivMan_Calc ()
**
** called
** - in RT context
** - after DrivMan_Calc()
*/

int
User_DrivMan_Calc (double dt)
{
    /* Rely on the Vehicle Operator within DrivMan module to get
       the vehicle in driving state using the IPG's
       PowerTrain Control model 'Generic' or similar */
    if (Vehicle.OperationState != OperState_Driving)
	return 0;

    return 0;
}


/*
** User_VehicleControl_Calc ()
**
** called
** - in RT context
** - after VehicleControl_Calc()
*/

int
User_VehicleControl_Calc (double dt)
{
    /* Rely on the Vehicle Operator within DrivMan module to get
       the vehicle in driving state using the IPG's
       PowerTrain Control model 'Generic' or similar */
    if (Vehicle.OperationState != OperState_Driving)
	return 0;

    return 0;
}



/*
** User_Brake_Calc ()
**
** called
** - in RT context
** - after Brake_Calc() in Vhcl_Calc()
*/

int
User_Brake_Calc (double dt)
{
    /* Modify the total brake torque from the brake system model Brake.Trq_tot[]
       or the target drive source torque from the brake control unit
       Brake.HydBrakeCU_IF.Trq_DriveSrc_trg[]
    */

    return 0;
}



/*
** User_Traffic_Calc ()
**
** called
** - in RT context
** - after Traffic_Calc()
*/

int
User_Traffic_Calc (double dt)
{
    if (SimCore.State != SCState_Simulate)
	return 0;

    return 0;
}



/*
** User_Calc ()
**
** called in RT context
*/

int
User_Calc (double dt)
{
    /* Starting with CM 6.0 User_Calc() will be invoked in EVERY simulation
       state. Uncomment the following line in order to restore the behaviour
       of CM 5.1 and earlier. */
    /*if (!UserCalcCalledByAppTestRunCalc) return 0;*/

    int rv;
    rv = Lineds_Calc(dt);

    return rv;
}



/*
** User_Check_IsIdle ()
**
** Checking, if the simulation model is in idle conditions (stand still,
** steeringwheel angle zero, cluch pedal pressed, ...).
** If reached idle state, the calculation of vehicle model and driving
** manoevers is stopped.
** Ready for start new simulation.
**
** Return:
** 1  idle state reached
** 0  else
**
** Call:
** - in main task, in the main loop
** - pay attention to realtime condition
** - while SimCore.State==SCState_EndIdleGet
*/

int
User_Check_IsIdle (int IsIdle)
{
    double val;

    /*** ECU / carmodel signals */

    /* vehicle and wheels: stand still */
    val = 0.5*kmh2ms;
    if (Vehicle.v > val
     || fabs(Vehicle.Wheel[0]->vBelt) > val || fabs(Vehicle.Wheel[1]->vBelt) > val
     || fabs(Vehicle.Wheel[2]->vBelt) > val || fabs(Vehicle.Wheel[3]->vBelt) > val) {
	IsIdle = 0;
    }

    /* SteerAngle: drive  straight forward position */
    val = 1.0*deg2rad;
    if (Vehicle.Steering.Ang > val || Vehicle.Steering.Ang < -val)
	IsIdle = 0;

    return IsIdle;
}



/*
** User_Out ()
**
** Assigns model quantities to variables of the i/o vector
**
** call:
** - in the main loop
** - pay attention to realtime condition
** - just before IO_Out();
*/

void
User_Out (const unsigned CycleNo)
{
    RBS_OutMap(CycleNo);

    if (SimCore.State != SCState_Simulate)
	return;
}



/*
** User_ApoMsg_Eval ()
**
** Communication between the application and connected GUIs.
** Evaluate messages from GUIs
**
** Call:
** - in the main loop
** - pay attention to realtime condition
** - near the end of the main loop, if the function SimCore_ApoMsg_Eval()
**    skips the message
**
** Return:
**   0 : message evaluated
**  -1 : message not handled
*/

int
User_ApoMsg_Eval (int Ch, char *Msg, int len, int who)
{
#if defined(CM_HIL)
    /*** FailSafeTester */
    if (Ch == ApoCh_CarMaker) {
	if (FST_ApoMsgEval(Ch, Msg, len) <= 0)
	    return 0;
    }
#endif
    return -1;
}



/*
** User_ApoMsg_Send ()
**
** Communication between the application and connected GUIs.
** Sends messages to GUIs
**
** Call:
** - near the end of the main loop, in MainThread_FinishCycle()
** - pay attention to realtime condition
*/

void
User_ApoMsg_Send (double T, const unsigned CycleNo)
{
}



/*
** User_ShutDown ()
**
** Prepare application for shut down
**
** Call:
** - at end of program
** - no realtime conditions
*/

int
User_ShutDown (int ShutDownForced)
{
    int IsDown = 0;

    /* Prepare application for shutdown and return that
       shutdown conditions are reached */
    if (1) {
	IsDown = 1;
    }

    return IsDown;
}



/*
** User_End ()
**
** End all models of the user module
**
** Call:
** - one times at end of program
** - no realtime conditions
*/

int
User_End (void)
{
    return 0;
}



/*
** User_Cleanup ()
**
** Cleanup function of the User module
**
** Call:
** - one times at end of program, just before exit
** - no realtime conditions
*/

void
User_Cleanup (void)
{
}

Lineds.h

复制代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

typedef struct tLds tLds;

struct tLds {

    struct {
        struct {
            double x;
            double y;
            double z;
        } p1, p2,p3,p4,p5,p6,p7,p8,p9,p10, p11, p12, p13, p14, p15, p16, p17, p18, p19, p20;
    } L1, L2, R1, R2;

};

extern tLds Lds;

int		Lineds_TestRun_Start_atEnd	(void);
void	Lineds_DeclQuants			(void);
int		Lineds_Calc					(double dt);

Lineds.c

复制代码

#include "Lineds.h"
#include <CarMaker.h>
#include "Vehicle/Sensor_Line.h"

/*  
** call in User_DeclQuants ()
*/

void
Lineds_DeclQuants (void)
{

DDefDouble4  (NULL, "LineL1_p1_x",	"m", &Lds.L1.p1.x, DVA_None);
    DDefDouble4  (NULL, "LineL1_p1_y",	"m", &Lds.L1.p1.y, DVA_None);
    DDefDouble4  (NULL, "LineL1_p1_z",	"m", &Lds.L1.p1.z, DVA_None);
    DDefDouble4  (NULL, "LineL1_p2_x",	"m", &Lds.L1.p2.x, DVA_None);
    DDefDouble4  (NULL, "LineL1_p2_y",	"m", &Lds.L1.p2.y, DVA_None);
    DDefDouble4  (NULL, "LineL1_p2_z",	"m", &Lds.L1.p2.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p3_x", "m", &Lds.L1.p3.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p3_y", "m", &Lds.L1.p3.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p3_z", "m", &Lds.L1.p3.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p4_x", "m", &Lds.L1.p4.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p4_y", "m", &Lds.L1.p4.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p4_z", "m", &Lds.L1.p4.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p5_x", "m", &Lds.L1.p5.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p5_y", "m", &Lds.L1.p5.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p5_z", "m", &Lds.L1.p5.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p6_x", "m", &Lds.L1.p6.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p6_y", "m", &Lds.L1.p6.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p6_z", "m", &Lds.L1.p6.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p7_x", "m", &Lds.L1.p7.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p7_y", "m", &Lds.L1.p7.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p7_z", "m", &Lds.L1.p7.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p8_x", "m", &Lds.L1.p8.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p8_y", "m", &Lds.L1.p8.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p8_z", "m", &Lds.L1.p8.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p9_x", "m", &Lds.L1.p9.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p9_y", "m", &Lds.L1.p9.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p9_z", "m", &Lds.L1.p9.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p10_x", "m", &Lds.L1.p10.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p10_y", "m", &Lds.L1.p10.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p10_z", "m", &Lds.L1.p10.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p11_x", "m", &Lds.L1.p11.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p11_y", "m", &Lds.L1.p11.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p11_z", "m", &Lds.L1.p11.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p12_x", "m", &Lds.L1.p12.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p12_y", "m", &Lds.L1.p12.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p12_z", "m", &Lds.L1.p12.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p13_x", "m", &Lds.L1.p13.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p13_y", "m", &Lds.L1.p13.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p13_z", "m", &Lds.L1.p13.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p14_x", "m", &Lds.L1.p14.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p14_y", "m", &Lds.L1.p14.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p14_z", "m", &Lds.L1.p14.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p15_x", "m", &Lds.L1.p15.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p15_y", "m", &Lds.L1.p15.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p15_z", "m", &Lds.L1.p15.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p16_x", "m", &Lds.L1.p16.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p16_y", "m", &Lds.L1.p16.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p16_z", "m", &Lds.L1.p16.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p17_x", "m", &Lds.L1.p17.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p17_y", "m", &Lds.L1.p17.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p17_z", "m", &Lds.L1.p17.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p18_x", "m", &Lds.L1.p18.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p18_y", "m", &Lds.L1.p18.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p18_z", "m", &Lds.L1.p18.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p19_x", "m", &Lds.L1.p19.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p19_y", "m", &Lds.L1.p19.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p19_z", "m", &Lds.L1.p19.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p20_x", "m", &Lds.L1.p20.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p20_y", "m", &Lds.L1.p20.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p20_z", "m", &Lds.L1.p20.z, DVA_None);

DDefDouble4  (NULL, "LineL2_p1_x",	"m", &Lds.L2.p1.x, DVA_None);
    DDefDouble4  (NULL, "LineL2_p1_y",	"m", &Lds.L2.p1.y, DVA_None);
    DDefDouble4  (NULL, "LineL2_p1_z",	"m", &Lds.L2.p1.z, DVA_None);
    DDefDouble4  (NULL, "LineL2_p2_x",	"m", &Lds.L2.p2.x, DVA_None);
    DDefDouble4  (NULL, "LineL2_p2_y",	"m", &Lds.L2.p2.y, DVA_None);
    DDefDouble4  (NULL, "LineL2_p2_z",	"m", &Lds.L2.p2.z, DVA_None);

DDefDouble4  (NULL, "LineR1_p1_x",	"m", &Lds.R1.p1.x, DVA_None);
    DDefDouble4  (NULL, "LineR1_p1_y",	"m", &Lds.R1.p1.y, DVA_None);
    DDefDouble4  (NULL, "LineR1_p1_z",	"m", &Lds.R1.p1.z, DVA_None);
    DDefDouble4  (NULL, "LineR1_p2_x",	"m", &Lds.R1.p2.x, DVA_None);
    DDefDouble4  (NULL, "LineR1_p2_y",	"m", &Lds.R1.p2.y, DVA_None);
    DDefDouble4  (NULL, "LineR1_p2_z",	"m", &Lds.R1.p2.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p3_x", "m", &Lds.R1.p3.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p3_y", "m", &Lds.R1.p3.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p3_z", "m", &Lds.R1.p3.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p4_x", "m", &Lds.R1.p4.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p4_y", "m", &Lds.R1.p4.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p4_z", "m", &Lds.R1.p4.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p5_x", "m", &Lds.R1.p5.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p5_y", "m", &Lds.R1.p5.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p5_z", "m", &Lds.R1.p5.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p6_x", "m", &Lds.R1.p6.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p6_y", "m", &Lds.R1.p6.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p6_z", "m", &Lds.R1.p6.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p7_x", "m", &Lds.R1.p7.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p7_y", "m", &Lds.R1.p7.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p7_z", "m", &Lds.R1.p7.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p8_x", "m", &Lds.R1.p8.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p8_y", "m", &Lds.R1.p8.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p8_z", "m", &Lds.R1.p8.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p9_x", "m", &Lds.R1.p9.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p9_y", "m", &Lds.R1.p9.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p9_z", "m", &Lds.R1.p9.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p10_x", "m", &Lds.R1.p10.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p10_y", "m", &Lds.R1.p10.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p10_z", "m", &Lds.R1.p10.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p11_x", "m", &Lds.R1.p11.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p11_y", "m", &Lds.R1.p11.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p11_z", "m", &Lds.R1.p11.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p12_x", "m", &Lds.R1.p12.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p12_y", "m", &Lds.R1.p12.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p12_z", "m", &Lds.R1.p12.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p13_x", "m", &Lds.R1.p13.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p13_y", "m", &Lds.R1.p13.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p13_z", "m", &Lds.R1.p13.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p14_x", "m", &Lds.R1.p14.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p14_y", "m", &Lds.R1.p14.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p14_z", "m", &Lds.R1.p14.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p15_x", "m", &Lds.R1.p15.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p15_y", "m", &Lds.R1.p15.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p15_z", "m", &Lds.R1.p15.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p16_x", "m", &Lds.R1.p16.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p16_y", "m", &Lds.R1.p16.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p16_z", "m", &Lds.R1.p16.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p17_x", "m", &Lds.R1.p17.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p17_y", "m", &Lds.R1.p17.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p17_z", "m", &Lds.R1.p17.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p18_x", "m", &Lds.R1.p18.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p18_y", "m", &Lds.R1.p18.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p18_z", "m", &Lds.R1.p18.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p19_x", "m", &Lds.R1.p19.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p19_y", "m", &Lds.R1.p19.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p19_z", "m", &Lds.R1.p19.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p20_x", "m", &Lds.R1.p20.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p20_y", "m", &Lds.R1.p20.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p20_z", "m", &Lds.R1.p20.z, DVA_None);

DDefDouble4  (NULL, "LineR2_p1_x",	"m", &Lds.R2.p1.x, DVA_None);
    DDefDouble4  (NULL, "LineR2_p1_y",	"m", &Lds.R2.p1.y, DVA_None);
    DDefDouble4  (NULL, "LineR2_p1_z",	"m", &Lds.R2.p1.z, DVA_None);
    DDefDouble4  (NULL, "LineR2_p2_x",	"m", &Lds.R2.p2.x, DVA_None);
    DDefDouble4  (NULL, "LineR2_p2_y",	"m", &Lds.R2.p2.y, DVA_None);
    DDefDouble4  (NULL, "LineR2_p2_z",	"m", &Lds.R2.p2.z, DVA_None);
}

/*
** Lineds_TestRun_Start_atEnd ()
**
** initialises struct at the start of a new TestRun
**
** call in User_TestRun_Start_atEnd ()
*/
int
Lineds_TestRun_Start_atEnd (void)
{
     Lds.L1.p1.x = 0; 
     Lds.L1.p1.y = 0; 
     Lds.L1.p1.z = 0;
     Lds.L1.p2.x = 0;
     Lds.L1.p2.y = 0;
     Lds.L1.p2.z = 0;
	 Lds.L1.p3.x = 0;
	 Lds.L1.p3.y = 0;
	 Lds.L1.p3.z = 0;
	 Lds.L1.p4.x = 0;
	 Lds.L1.p4.y = 0;
	 Lds.L1.p4.z = 0;
	 Lds.L1.p5.x = 0;
	 Lds.L1.p5.y = 0;
	 Lds.L1.p5.z = 0;
	 Lds.L1.p6.x = 0;
	 Lds.L1.p6.y = 0;
	 Lds.L1.p6.z = 0;
	 Lds.L1.p7.x = 0;
	 Lds.L1.p7.y = 0;
	 Lds.L1.p7.z = 0;
	 Lds.L1.p8.x = 0;
	 Lds.L1.p8.y = 0;
	 Lds.L1.p8.z = 0;
	 Lds.L1.p9.x = 0;
	 Lds.L1.p9.y = 0;
	 Lds.L1.p9.z = 0;
	 Lds.L1.p10.x = 0;
	 Lds.L1.p10.y = 0;
	 Lds.L1.p10.z = 0;
	 Lds.L1.p11.x = 0;
	 Lds.L1.p11.y = 0;
	 Lds.L1.p11.z = 0;
	 Lds.L1.p12.x = 0;
	 Lds.L1.p12.y = 0;
	 Lds.L1.p12.z = 0;
	 Lds.L1.p13.x = 0;
	 Lds.L1.p13.y = 0;
	 Lds.L1.p13.z = 0;
	 Lds.L1.p14.x = 0;
	 Lds.L1.p14.y = 0;
	 Lds.L1.p14.z = 0;
	 Lds.L1.p15.x = 0;
	 Lds.L1.p15.y = 0;
	 Lds.L1.p15.z = 0;
	 Lds.L1.p16.x = 0;
	 Lds.L1.p16.y = 0;
	 Lds.L1.p16.z = 0;
	 Lds.L1.p17.x = 0;
	 Lds.L1.p17.y = 0;
	 Lds.L1.p17.z = 0;
	 Lds.L1.p18.x = 0;
	 Lds.L1.p18.y = 0;
	 Lds.L1.p18.z = 0;
	 Lds.L1.p19.x = 0;
	 Lds.L1.p19.y = 0;
	 Lds.L1.p19.z = 0;
	 Lds.L1.p20.x = 0;
	 Lds.L1.p20.y = 0;
	 Lds.L1.p20.z = 0;

Lds.L2.p1.x = 0;
     Lds.L2.p1.y = 0;
     Lds.L2.p1.z = 0;
     Lds.L2.p2.x = 0; 
     Lds.L2.p2.y = 0; 
     Lds.L2.p2.z = 0;

Lds.R1.p1.x = 0;
     Lds.R1.p1.y = 0;
     Lds.R1.p1.z = 0;
     Lds.R1.p2.x = 0;
     Lds.R1.p2.y = 0;
     Lds.R1.p2.z = 0;
	 Lds.R1.p3.x = 0;
	 Lds.R1.p3.y = 0;
	 Lds.R1.p3.z = 0;
	 Lds.R1.p4.x = 0;
	 Lds.R1.p4.y = 0;
	 Lds.R1.p4.z = 0;
	 Lds.R1.p5.x = 0;
	 Lds.R1.p5.y = 0;
	 Lds.R1.p5.z = 0;
	 Lds.R1.p6.x = 0;
	 Lds.R1.p6.y = 0;
	 Lds.R1.p6.z = 0;
	 Lds.R1.p7.x = 0;
	 Lds.R1.p7.y = 0;
	 Lds.R1.p7.z = 0;
	 Lds.R1.p8.x = 0;
	 Lds.R1.p8.y = 0;
	 Lds.R1.p8.z = 0;
	 Lds.R1.p9.x = 0;
	 Lds.R1.p9.y = 0;
	 Lds.R1.p9.z = 0;
	 Lds.R1.p10.x = 0;
	 Lds.R1.p10.y = 0;
	 Lds.R1.p10.z = 0;
	 Lds.R1.p11.x = 0;
	 Lds.R1.p11.y = 0;
	 Lds.R1.p11.z = 0;
	 Lds.R1.p12.x = 0;
	 Lds.R1.p12.y = 0;
	 Lds.R1.p12.z = 0;
	 Lds.R1.p13.x = 0;
	 Lds.R1.p13.y = 0;
	 Lds.R1.p13.z = 0;
	 Lds.R1.p14.x = 0;
	 Lds.R1.p14.y = 0;
	 Lds.R1.p14.z = 0;
	 Lds.R1.p15.x = 0;
	 Lds.R1.p15.y = 0;
	 Lds.R1.p15.z = 0;
	 Lds.R1.p16.x = 0;
	 Lds.R1.p16.y = 0;
	 Lds.R1.p16.z = 0;
	 Lds.R1.p17.x = 0;
	 Lds.R1.p17.y = 0;
	 Lds.R1.p17.z = 0;
	 Lds.R1.p18.x = 0;
	 Lds.R1.p18.y = 0;
	 Lds.R1.p18.z = 0;
	 Lds.R1.p19.x = 0;
	 Lds.R1.p19.y = 0;
	 Lds.R1.p19.z = 0;
	 Lds.R1.p20.x = 0;
	 Lds.R1.p20.y = 0;
	 Lds.R1.p20.z = 0;

Lds.R2.p1.x = 0; 
     Lds.R2.p1.y = 0; 
     Lds.R2.p1.z = 0;
     Lds.R2.p2.x = 0;
     Lds.R2.p2.y = 0;
     Lds.R2.p2.z = 0;
     
    return 0;
}    
     
     
/*  
** call in User_Calc ()
*/   
     
int  
Lineds_Calc (double dt)
{    
     
    /*Execute only during simulation*/
    if (SimCore.State != SCState_Simulate) return 0;
     
    /*Execute only if at least 1 line is detected on the left or  right side*/
    if (LineSensor[0].RLines.nLine == 0 || LineSensor[0].LLines.nLine == 0) {
       Lds.L1.p1.x  = 0; 
       Lds.L1.p1.y  = 0; 
       Lds.L1.p1.z  = 0;
       Lds.L1.p2.x  = 0;
       Lds.L1.p2.y  = 0;
       Lds.L1.p2.z  = 0;
	   Lds.L1.p3.x = 0;
	   Lds.L1.p3.y = 0;
	   Lds.L1.p3.z = 0;
	   Lds.L1.p4.x = 0;
	   Lds.L1.p4.y = 0;
	   Lds.L1.p4.z = 0;
	   Lds.L1.p5.x = 0;
	   Lds.L1.p5.y = 0;
	   Lds.L1.p5.z = 0;
	   Lds.L1.p6.x = 0;
	   Lds.L1.p6.y = 0;
	   Lds.L1.p6.z = 0;
	   Lds.L1.p7.x = 0;
	   Lds.L1.p7.y = 0;
	   Lds.L1.p7.z = 0;
	   Lds.L1.p8.x = 0;
	   Lds.L1.p8.y = 0;
	   Lds.L1.p8.z = 0;
	   Lds.L1.p9.x = 0;
	   Lds.L1.p9.y = 0;
	   Lds.L1.p9.z = 0;
	   Lds.L1.p10.x = 0;
	   Lds.L1.p10.y = 0;
	   Lds.L1.p10.z = 0;
	   Lds.L1.p11.x = 0;
	   Lds.L1.p11.y = 0;
	   Lds.L1.p11.z = 0;
	   Lds.L1.p12.x = 0;
	   Lds.L1.p12.y = 0;
	   Lds.L1.p12.z = 0;
	   Lds.L1.p13.x = 0;
	   Lds.L1.p13.y = 0;
	   Lds.L1.p13.z = 0;
	   Lds.L1.p14.x = 0;
	   Lds.L1.p14.y = 0;
	   Lds.L1.p14.z = 0;
	   Lds.L1.p15.x = 0;
	   Lds.L1.p15.y = 0;
	   Lds.L1.p15.z = 0;
	   Lds.L1.p16.x = 0;
	   Lds.L1.p16.y = 0;
	   Lds.L1.p16.z = 0;
	   Lds.L1.p17.x = 0;
	   Lds.L1.p17.y = 0;
	   Lds.L1.p17.z = 0;
	   Lds.L1.p18.x = 0;
	   Lds.L1.p18.y = 0;
	   Lds.L1.p18.z = 0;
	   Lds.L1.p19.x = 0;
	   Lds.L1.p19.y = 0;
	   Lds.L1.p19.z = 0;
	   Lds.L1.p20.x = 0;
	   Lds.L1.p20.y = 0;
	   Lds.L1.p20.z = 0;

Lds.L2.p1.x  = 0;
       Lds.L2.p1.y  = 0;
       Lds.L2.p1.z  = 0;
       Lds.L2.p2.x  = 0; 
       Lds.L2.p2.y  = 0; 
       Lds.L2.p2.z  = 0;

Lds.R1.p1.x  = 0;
       Lds.R1.p1.y  = 0;
       Lds.R1.p1.z  = 0;
       Lds.R1.p2.x  = 0;
       Lds.R1.p2.y  = 0;
       Lds.R1.p2.z  = 0;
	   Lds.R1.p3.x = 0;
	   Lds.R1.p3.y = 0;
	   Lds.R1.p3.z = 0;
	   Lds.R1.p4.x = 0;
	   Lds.R1.p4.y = 0;
	   Lds.R1.p4.z = 0;
	   Lds.R1.p5.x = 0;
	   Lds.R1.p5.y = 0;
	   Lds.R1.p5.z = 0;
	   Lds.R1.p6.x = 0;
	   Lds.R1.p6.y = 0;
	   Lds.R1.p6.z = 0;
	   Lds.R1.p7.x = 0;
	   Lds.R1.p7.y = 0;
	   Lds.R1.p7.z = 0;
	   Lds.R1.p8.x = 0;
	   Lds.R1.p8.y = 0;
	   Lds.R1.p8.z = 0;
	   Lds.R1.p9.x = 0;
	   Lds.R1.p9.y = 0;
	   Lds.R1.p9.z = 0;
	   Lds.R1.p10.x = 0;
	   Lds.R1.p10.y = 0;
	   Lds.R1.p10.z = 0;
	   Lds.R1.p11.x = 0;
	   Lds.R1.p11.y = 0;
	   Lds.R1.p11.z = 0;
	   Lds.R1.p12.x = 0;
	   Lds.R1.p12.y = 0;
	   Lds.R1.p12.z = 0;
	   Lds.R1.p13.x = 0;
	   Lds.R1.p13.y = 0;
	   Lds.R1.p13.z = 0;
	   Lds.R1.p14.x = 0;
	   Lds.R1.p14.y = 0;
	   Lds.R1.p14.z = 0;
	   Lds.R1.p15.x = 0;
	   Lds.R1.p15.y = 0;
	   Lds.R1.p15.z = 0;
	   Lds.R1.p16.x = 0;
	   Lds.R1.p16.y = 0;
	   Lds.R1.p16.z = 0;
	   Lds.R1.p17.x = 0;
	   Lds.R1.p17.y = 0;
	   Lds.R1.p17.z = 0;
	   Lds.R1.p18.x = 0;
	   Lds.R1.p18.y = 0;
	   Lds.R1.p18.z = 0;
	   Lds.R1.p19.x = 0;
	   Lds.R1.p19.y = 0;
	   Lds.R1.p19.z = 0;
	   Lds.R1.p20.x = 0;
	   Lds.R1.p20.y = 0;
	   Lds.R1.p20.z = 0;

Lds.R2.p1.x  = 0; 
       Lds.R2.p1.y  = 0; 
       Lds.R2.p1.z  = 0;
       Lds.R2.p2.x  = 0;
       Lds.R2.p2.y  = 0;
       Lds.R2.p2.z  = 0;

return 0;
    };

Lds.L1.p1.x = LineSensor[0].LLines.L[0].ds[0][0];
        Lds.L1.p1.y = LineSensor[0].LLines.L[0].ds[0][1];
        Lds.L1.p1.z = LineSensor[0].LLines.L[0].ds[0][2];
        Lds.L1.p2.x = LineSensor[0].LLines.L[0].ds[1][0];
        Lds.L1.p2.y = LineSensor[0].LLines.L[0].ds[1][1];
        Lds.L1.p2.z = LineSensor[0].LLines.L[0].ds[1][2];
		Lds.L1.p3.x = LineSensor[0].LLines.L[0].ds[2][0];
		Lds.L1.p3.y = LineSensor[0].LLines.L[0].ds[2][1];
		Lds.L1.p3.z = LineSensor[0].LLines.L[0].ds[2][2];
		Lds.L1.p4.x = LineSensor[0].LLines.L[0].ds[3][0];
		Lds.L1.p4.y = LineSensor[0].LLines.L[0].ds[3][1];
		Lds.L1.p4.z = LineSensor[0].LLines.L[0].ds[3][2];
		Lds.L1.p5.x = LineSensor[0].LLines.L[0].ds[4][0];
		Lds.L1.p5.y = LineSensor[0].LLines.L[0].ds[4][1];
		Lds.L1.p5.z = LineSensor[0].LLines.L[0].ds[4][2];
		Lds.L1.p6.x = LineSensor[0].LLines.L[0].ds[5][0];
		Lds.L1.p6.y = LineSensor[0].LLines.L[0].ds[5][1];
		Lds.L1.p6.z = LineSensor[0].LLines.L[0].ds[5][2];
		Lds.L1.p7.x = LineSensor[0].LLines.L[0].ds[6][0];
		Lds.L1.p7.y = LineSensor[0].LLines.L[0].ds[6][1];
		Lds.L1.p7.z = LineSensor[0].LLines.L[0].ds[6][2];
		Lds.L1.p8.x = LineSensor[0].LLines.L[0].ds[7][0];
		Lds.L1.p8.y = LineSensor[0].LLines.L[0].ds[7][1];
		Lds.L1.p8.z = LineSensor[0].LLines.L[0].ds[7][2];
		Lds.L1.p9.x = LineSensor[0].LLines.L[0].ds[8][0];
		Lds.L1.p9.y = LineSensor[0].LLines.L[0].ds[8][1];
		Lds.L1.p9.z = LineSensor[0].LLines.L[0].ds[8][2];
		Lds.L1.p10.x = LineSensor[0].LLines.L[0].ds[9][0];
		Lds.L1.p10.y = LineSensor[0].LLines.L[0].ds[9][1];
		Lds.L1.p10.z = LineSensor[0].LLines.L[0].ds[9][2];
		Lds.L1.p11.x = LineSensor[0].LLines.L[0].ds[10][0];
		Lds.L1.p11.y = LineSensor[0].LLines.L[0].ds[10][1];
		Lds.L1.p11.z = LineSensor[0].LLines.L[0].ds[10][2];
		Lds.L1.p12.x = LineSensor[0].LLines.L[0].ds[11][0];
		Lds.L1.p12.y = LineSensor[0].LLines.L[0].ds[11][1];
		Lds.L1.p12.z = LineSensor[0].LLines.L[0].ds[11][2];
		Lds.L1.p13.x = LineSensor[0].LLines.L[0].ds[12][0];
		Lds.L1.p13.y = LineSensor[0].LLines.L[0].ds[12][1];
		Lds.L1.p13.z = LineSensor[0].LLines.L[0].ds[12][2];
		Lds.L1.p14.x = LineSensor[0].LLines.L[0].ds[13][0];
		Lds.L1.p14.y = LineSensor[0].LLines.L[0].ds[13][1];
		Lds.L1.p14.z = LineSensor[0].LLines.L[0].ds[13][2];
		Lds.L1.p15.x = LineSensor[0].LLines.L[0].ds[14][0];
		Lds.L1.p15.y = LineSensor[0].LLines.L[0].ds[14][1];
		Lds.L1.p15.z = LineSensor[0].LLines.L[0].ds[14][2];
		Lds.L1.p16.x = LineSensor[0].LLines.L[0].ds[15][0];
		Lds.L1.p16.y = LineSensor[0].LLines.L[0].ds[15][1];
		Lds.L1.p16.z = LineSensor[0].LLines.L[0].ds[15][2];
		Lds.L1.p17.x = LineSensor[0].LLines.L[0].ds[16][0];
		Lds.L1.p17.y = LineSensor[0].LLines.L[0].ds[16][1];
		Lds.L1.p17.z = LineSensor[0].LLines.L[0].ds[16][2];
		Lds.L1.p18.x = LineSensor[0].LLines.L[0].ds[17][0];
		Lds.L1.p18.y = LineSensor[0].LLines.L[0].ds[17][1];
		Lds.L1.p18.z = LineSensor[0].LLines.L[0].ds[17][2];
		Lds.L1.p19.x = LineSensor[0].LLines.L[0].ds[18][0];
		Lds.L1.p19.y = LineSensor[0].LLines.L[0].ds[18][1];
		Lds.L1.p19.z = LineSensor[0].LLines.L[0].ds[18][2];
		Lds.L1.p20.x = LineSensor[0].LLines.L[0].ds[19][0];
		Lds.L1.p20.y = LineSensor[0].LLines.L[0].ds[19][1];
		Lds.L1.p20.z = LineSensor[0].LLines.L[0].ds[19][2];

Lds.L2.p1.x = LineSensor[0].LLines.L[1].ds[0][0];
        Lds.L2.p1.y = LineSensor[0].LLines.L[1].ds[0][1];
        Lds.L2.p1.z = LineSensor[0].LLines.L[1].ds[0][2];
        Lds.L2.p2.x = LineSensor[0].LLines.L[1].ds[1][0];
        Lds.L2.p2.y = LineSensor[0].LLines.L[1].ds[1][1];
        Lds.L2.p2.z = LineSensor[0].LLines.L[1].ds[1][2];

Lds.R1.p1.x = LineSensor[0].RLines.L[0].ds[0][0];
        Lds.R1.p1.y = LineSensor[0].RLines.L[0].ds[0][1]; 
        Lds.R1.p1.z = LineSensor[0].RLines.L[0].ds[0][2];  
        Lds.R1.p2.x = LineSensor[0].RLines.L[0].ds[1][0];  
        Lds.R1.p2.y = LineSensor[0].RLines.L[0].ds[1][1];
        Lds.R1.p2.z = LineSensor[0].RLines.L[0].ds[1][2];
		Lds.R1.p3.x = LineSensor[0].RLines.L[0].ds[2][0];
		Lds.R1.p3.y = LineSensor[0].RLines.L[0].ds[2][1];
		Lds.R1.p3.z = LineSensor[0].RLines.L[0].ds[2][2];
		Lds.R1.p4.x = LineSensor[0].RLines.L[0].ds[3][0];
		Lds.R1.p4.y = LineSensor[0].RLines.L[0].ds[3][1];
		Lds.R1.p4.z = LineSensor[0].RLines.L[0].ds[3][2];
		Lds.R1.p5.x = LineSensor[0].RLines.L[0].ds[4][0];
		Lds.R1.p5.y = LineSensor[0].RLines.L[0].ds[4][1];
		Lds.R1.p5.z = LineSensor[0].RLines.L[0].ds[4][2];
		Lds.R1.p6.x = LineSensor[0].RLines.L[0].ds[5][0];
		Lds.R1.p6.y = LineSensor[0].RLines.L[0].ds[5][1];
		Lds.R1.p6.z = LineSensor[0].RLines.L[0].ds[5][2];
		Lds.R1.p7.x = LineSensor[0].RLines.L[0].ds[6][0];
		Lds.R1.p7.y = LineSensor[0].RLines.L[0].ds[6][1];
		Lds.R1.p7.z = LineSensor[0].RLines.L[0].ds[6][2];
		Lds.R1.p8.x = LineSensor[0].RLines.L[0].ds[7][0];
		Lds.R1.p8.y = LineSensor[0].RLines.L[0].ds[7][1];
		Lds.R1.p8.z = LineSensor[0].RLines.L[0].ds[7][2];
		Lds.R1.p9.x = LineSensor[0].RLines.L[0].ds[8][0];
		Lds.R1.p9.y = LineSensor[0].RLines.L[0].ds[8][1];
		Lds.R1.p9.z = LineSensor[0].RLines.L[0].ds[8][2];
		Lds.R1.p10.x = LineSensor[0].RLines.L[0].ds[9][0];
		Lds.R1.p10.y = LineSensor[0].RLines.L[0].ds[9][1];
		Lds.R1.p10.z = LineSensor[0].RLines.L[0].ds[9][2];
		Lds.R1.p11.x = LineSensor[0].RLines.L[0].ds[10][0];
		Lds.R1.p11.y = LineSensor[0].RLines.L[0].ds[10][1];
		Lds.R1.p11.z = LineSensor[0].RLines.L[0].ds[10][2];
		Lds.R1.p12.x = LineSensor[0].RLines.L[0].ds[11][0];
		Lds.R1.p12.y = LineSensor[0].RLines.L[0].ds[11][1];
		Lds.R1.p12.z = LineSensor[0].RLines.L[0].ds[11][2];
		Lds.R1.p13.x = LineSensor[0].RLines.L[0].ds[12][0];
		Lds.R1.p13.y = LineSensor[0].RLines.L[0].ds[12][1];
		Lds.R1.p13.z = LineSensor[0].RLines.L[0].ds[12][2];
		Lds.R1.p14.x = LineSensor[0].RLines.L[0].ds[13][0];
		Lds.R1.p14.y = LineSensor[0].RLines.L[0].ds[13][1];
		Lds.R1.p14.z = LineSensor[0].RLines.L[0].ds[13][2];
		Lds.R1.p15.x = LineSensor[0].RLines.L[0].ds[14][0];
		Lds.R1.p15.y = LineSensor[0].RLines.L[0].ds[14][1];
		Lds.R1.p15.z = LineSensor[0].RLines.L[0].ds[14][2];
		Lds.R1.p16.x = LineSensor[0].RLines.L[0].ds[15][0];
		Lds.R1.p16.y = LineSensor[0].RLines.L[0].ds[15][1];
		Lds.R1.p16.z = LineSensor[0].RLines.L[0].ds[15][2];
		Lds.R1.p17.x = LineSensor[0].RLines.L[0].ds[16][0];
		Lds.R1.p17.y = LineSensor[0].RLines.L[0].ds[16][1];
		Lds.R1.p17.z = LineSensor[0].RLines.L[0].ds[16][2];
		Lds.R1.p18.x = LineSensor[0].RLines.L[0].ds[17][0];
		Lds.R1.p18.y = LineSensor[0].RLines.L[0].ds[17][1];
		Lds.R1.p18.z = LineSensor[0].RLines.L[0].ds[17][2];
		Lds.R1.p19.x = LineSensor[0].RLines.L[0].ds[18][0];
		Lds.R1.p19.y = LineSensor[0].RLines.L[0].ds[18][1];
		Lds.R1.p19.z = LineSensor[0].RLines.L[0].ds[18][2];
		Lds.R1.p20.x = LineSensor[0].RLines.L[0].ds[19][0];
		Lds.R1.p20.y = LineSensor[0].RLines.L[0].ds[19][1];
		Lds.R1.p20.z = LineSensor[0].RLines.L[0].ds[19][2];

Lds.R2.p1.x = LineSensor[0].RLines.L[1].ds[0][0]; 
        Lds.R2.p1.y = LineSensor[0].RLines.L[1].ds[0][1]; 
        Lds.R2.p1.z = LineSensor[0].RLines.L[1].ds[0][2]; 
        Lds.R2.p2.x = LineSensor[0].RLines.L[1].ds[1][0]; 
        Lds.R2.p2.y = LineSensor[0].RLines.L[1].ds[1][1];  
        Lds.R2.p2.z = LineSensor[0].RLines.L[1].ds[1][2];

return 0;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
#include "Lineds.h"
#include <CarMaker.h>
#include "Vehicle/Sensor_Line.h"

/*  
** call in User_DeclQuants ()
*/   

void
Lineds_DeclQuants (void)
{

    DDefDouble4  (NULL, "LineL1_p1_x",	"m", &Lds.L1.p1.x, DVA_None);
    DDefDouble4  (NULL, "LineL1_p1_y",	"m", &Lds.L1.p1.y, DVA_None);
    DDefDouble4  (NULL, "LineL1_p1_z",	"m", &Lds.L1.p1.z, DVA_None);
    DDefDouble4  (NULL, "LineL1_p2_x",	"m", &Lds.L1.p2.x, DVA_None);
    DDefDouble4  (NULL, "LineL1_p2_y",	"m", &Lds.L1.p2.y, DVA_None);
    DDefDouble4  (NULL, "LineL1_p2_z",	"m", &Lds.L1.p2.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p3_x", "m", &Lds.L1.p3.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p3_y", "m", &Lds.L1.p3.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p3_z", "m", &Lds.L1.p3.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p4_x", "m", &Lds.L1.p4.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p4_y", "m", &Lds.L1.p4.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p4_z", "m", &Lds.L1.p4.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p5_x", "m", &Lds.L1.p5.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p5_y", "m", &Lds.L1.p5.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p5_z", "m", &Lds.L1.p5.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p6_x", "m", &Lds.L1.p6.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p6_y", "m", &Lds.L1.p6.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p6_z", "m", &Lds.L1.p6.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p7_x", "m", &Lds.L1.p7.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p7_y", "m", &Lds.L1.p7.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p7_z", "m", &Lds.L1.p7.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p8_x", "m", &Lds.L1.p8.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p8_y", "m", &Lds.L1.p8.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p8_z", "m", &Lds.L1.p8.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p9_x", "m", &Lds.L1.p9.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p9_y", "m", &Lds.L1.p9.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p9_z", "m", &Lds.L1.p9.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p10_x", "m", &Lds.L1.p10.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p10_y", "m", &Lds.L1.p10.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p10_z", "m", &Lds.L1.p10.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p11_x", "m", &Lds.L1.p11.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p11_y", "m", &Lds.L1.p11.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p11_z", "m", &Lds.L1.p11.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p12_x", "m", &Lds.L1.p12.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p12_y", "m", &Lds.L1.p12.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p12_z", "m", &Lds.L1.p12.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p13_x", "m", &Lds.L1.p13.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p13_y", "m", &Lds.L1.p13.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p13_z", "m", &Lds.L1.p13.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p14_x", "m", &Lds.L1.p14.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p14_y", "m", &Lds.L1.p14.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p14_z", "m", &Lds.L1.p14.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p15_x", "m", &Lds.L1.p15.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p15_y", "m", &Lds.L1.p15.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p15_z", "m", &Lds.L1.p15.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p16_x", "m", &Lds.L1.p16.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p16_y", "m", &Lds.L1.p16.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p16_z", "m", &Lds.L1.p16.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p17_x", "m", &Lds.L1.p17.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p17_y", "m", &Lds.L1.p17.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p17_z", "m", &Lds.L1.p17.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p18_x", "m", &Lds.L1.p18.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p18_y", "m", &Lds.L1.p18.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p18_z", "m", &Lds.L1.p18.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p19_x", "m", &Lds.L1.p19.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p19_y", "m", &Lds.L1.p19.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p19_z", "m", &Lds.L1.p19.z, DVA_None);
	DDefDouble4(NULL, "LineL1_p20_x", "m", &Lds.L1.p20.x, DVA_None);
	DDefDouble4(NULL, "LineL1_p20_y", "m", &Lds.L1.p20.y, DVA_None);
	DDefDouble4(NULL, "LineL1_p20_z", "m", &Lds.L1.p20.z, DVA_None);

    DDefDouble4  (NULL, "LineL2_p1_x",	"m", &Lds.L2.p1.x, DVA_None);
    DDefDouble4  (NULL, "LineL2_p1_y",	"m", &Lds.L2.p1.y, DVA_None);
    DDefDouble4  (NULL, "LineL2_p1_z",	"m", &Lds.L2.p1.z, DVA_None);
    DDefDouble4  (NULL, "LineL2_p2_x",	"m", &Lds.L2.p2.x, DVA_None);
    DDefDouble4  (NULL, "LineL2_p2_y",	"m", &Lds.L2.p2.y, DVA_None);
    DDefDouble4  (NULL, "LineL2_p2_z",	"m", &Lds.L2.p2.z, DVA_None);

    DDefDouble4  (NULL, "LineR1_p1_x",	"m", &Lds.R1.p1.x, DVA_None);
    DDefDouble4  (NULL, "LineR1_p1_y",	"m", &Lds.R1.p1.y, DVA_None);
    DDefDouble4  (NULL, "LineR1_p1_z",	"m", &Lds.R1.p1.z, DVA_None);
    DDefDouble4  (NULL, "LineR1_p2_x",	"m", &Lds.R1.p2.x, DVA_None);
    DDefDouble4  (NULL, "LineR1_p2_y",	"m", &Lds.R1.p2.y, DVA_None);
    DDefDouble4  (NULL, "LineR1_p2_z",	"m", &Lds.R1.p2.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p3_x", "m", &Lds.R1.p3.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p3_y", "m", &Lds.R1.p3.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p3_z", "m", &Lds.R1.p3.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p4_x", "m", &Lds.R1.p4.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p4_y", "m", &Lds.R1.p4.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p4_z", "m", &Lds.R1.p4.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p5_x", "m", &Lds.R1.p5.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p5_y", "m", &Lds.R1.p5.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p5_z", "m", &Lds.R1.p5.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p6_x", "m", &Lds.R1.p6.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p6_y", "m", &Lds.R1.p6.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p6_z", "m", &Lds.R1.p6.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p7_x", "m", &Lds.R1.p7.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p7_y", "m", &Lds.R1.p7.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p7_z", "m", &Lds.R1.p7.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p8_x", "m", &Lds.R1.p8.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p8_y", "m", &Lds.R1.p8.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p8_z", "m", &Lds.R1.p8.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p9_x", "m", &Lds.R1.p9.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p9_y", "m", &Lds.R1.p9.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p9_z", "m", &Lds.R1.p9.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p10_x", "m", &Lds.R1.p10.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p10_y", "m", &Lds.R1.p10.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p10_z", "m", &Lds.R1.p10.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p11_x", "m", &Lds.R1.p11.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p11_y", "m", &Lds.R1.p11.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p11_z", "m", &Lds.R1.p11.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p12_x", "m", &Lds.R1.p12.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p12_y", "m", &Lds.R1.p12.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p12_z", "m", &Lds.R1.p12.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p13_x", "m", &Lds.R1.p13.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p13_y", "m", &Lds.R1.p13.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p13_z", "m", &Lds.R1.p13.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p14_x", "m", &Lds.R1.p14.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p14_y", "m", &Lds.R1.p14.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p14_z", "m", &Lds.R1.p14.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p15_x", "m", &Lds.R1.p15.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p15_y", "m", &Lds.R1.p15.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p15_z", "m", &Lds.R1.p15.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p16_x", "m", &Lds.R1.p16.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p16_y", "m", &Lds.R1.p16.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p16_z", "m", &Lds.R1.p16.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p17_x", "m", &Lds.R1.p17.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p17_y", "m", &Lds.R1.p17.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p17_z", "m", &Lds.R1.p17.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p18_x", "m", &Lds.R1.p18.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p18_y", "m", &Lds.R1.p18.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p18_z", "m", &Lds.R1.p18.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p19_x", "m", &Lds.R1.p19.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p19_y", "m", &Lds.R1.p19.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p19_z", "m", &Lds.R1.p19.z, DVA_None);
	DDefDouble4(NULL, "LineR1_p20_x", "m", &Lds.R1.p20.x, DVA_None);
	DDefDouble4(NULL, "LineR1_p20_y", "m", &Lds.R1.p20.y, DVA_None);
	DDefDouble4(NULL, "LineR1_p20_z", "m", &Lds.R1.p20.z, DVA_None);

    DDefDouble4  (NULL, "LineR2_p1_x",	"m", &Lds.R2.p1.x, DVA_None);
    DDefDouble4  (NULL, "LineR2_p1_y",	"m", &Lds.R2.p1.y, DVA_None);
    DDefDouble4  (NULL, "LineR2_p1_z",	"m", &Lds.R2.p1.z, DVA_None);
    DDefDouble4  (NULL, "LineR2_p2_x",	"m", &Lds.R2.p2.x, DVA_None);
    DDefDouble4  (NULL, "LineR2_p2_y",	"m", &Lds.R2.p2.y, DVA_None);
    DDefDouble4  (NULL, "LineR2_p2_z",	"m", &Lds.R2.p2.z, DVA_None);
}


/*
** Lineds_TestRun_Start_atEnd ()
**
** initialises struct at the start of a new TestRun
**
** call in User_TestRun_Start_atEnd ()
*/
int
Lineds_TestRun_Start_atEnd (void)
{
     Lds.L1.p1.x = 0; 
     Lds.L1.p1.y = 0; 
     Lds.L1.p1.z = 0;
     Lds.L1.p2.x = 0;
     Lds.L1.p2.y = 0;
     Lds.L1.p2.z = 0;
	 Lds.L1.p3.x = 0;
	 Lds.L1.p3.y = 0;
	 Lds.L1.p3.z = 0;
	 Lds.L1.p4.x = 0;
	 Lds.L1.p4.y = 0;
	 Lds.L1.p4.z = 0;
	 Lds.L1.p5.x = 0;
	 Lds.L1.p5.y = 0;
	 Lds.L1.p5.z = 0;
	 Lds.L1.p6.x = 0;
	 Lds.L1.p6.y = 0;
	 Lds.L1.p6.z = 0;
	 Lds.L1.p7.x = 0;
	 Lds.L1.p7.y = 0;
	 Lds.L1.p7.z = 0;
	 Lds.L1.p8.x = 0;
	 Lds.L1.p8.y = 0;
	 Lds.L1.p8.z = 0;
	 Lds.L1.p9.x = 0;
	 Lds.L1.p9.y = 0;
	 Lds.L1.p9.z = 0;
	 Lds.L1.p10.x = 0;
	 Lds.L1.p10.y = 0;
	 Lds.L1.p10.z = 0;
	 Lds.L1.p11.x = 0;
	 Lds.L1.p11.y = 0;
	 Lds.L1.p11.z = 0;
	 Lds.L1.p12.x = 0;
	 Lds.L1.p12.y = 0;
	 Lds.L1.p12.z = 0;
	 Lds.L1.p13.x = 0;
	 Lds.L1.p13.y = 0;
	 Lds.L1.p13.z = 0;
	 Lds.L1.p14.x = 0;
	 Lds.L1.p14.y = 0;
	 Lds.L1.p14.z = 0;
	 Lds.L1.p15.x = 0;
	 Lds.L1.p15.y = 0;
	 Lds.L1.p15.z = 0;
	 Lds.L1.p16.x = 0;
	 Lds.L1.p16.y = 0;
	 Lds.L1.p16.z = 0;
	 Lds.L1.p17.x = 0;
	 Lds.L1.p17.y = 0;
	 Lds.L1.p17.z = 0;
	 Lds.L1.p18.x = 0;
	 Lds.L1.p18.y = 0;
	 Lds.L1.p18.z = 0;
	 Lds.L1.p19.x = 0;
	 Lds.L1.p19.y = 0;
	 Lds.L1.p19.z = 0;
	 Lds.L1.p20.x = 0;
	 Lds.L1.p20.y = 0;
	 Lds.L1.p20.z = 0;

     Lds.L2.p1.x = 0;
     Lds.L2.p1.y = 0;
     Lds.L2.p1.z = 0;
     Lds.L2.p2.x = 0; 
     Lds.L2.p2.y = 0; 
     Lds.L2.p2.z = 0;

     Lds.R1.p1.x = 0;
     Lds.R1.p1.y = 0;
     Lds.R1.p1.z = 0;
     Lds.R1.p2.x = 0;
     Lds.R1.p2.y = 0;
     Lds.R1.p2.z = 0;
	 Lds.R1.p3.x = 0;
	 Lds.R1.p3.y = 0;
	 Lds.R1.p3.z = 0;
	 Lds.R1.p4.x = 0;
	 Lds.R1.p4.y = 0;
	 Lds.R1.p4.z = 0;
	 Lds.R1.p5.x = 0;
	 Lds.R1.p5.y = 0;
	 Lds.R1.p5.z = 0;
	 Lds.R1.p6.x = 0;
	 Lds.R1.p6.y = 0;
	 Lds.R1.p6.z = 0;
	 Lds.R1.p7.x = 0;
	 Lds.R1.p7.y = 0;
	 Lds.R1.p7.z = 0;
	 Lds.R1.p8.x = 0;
	 Lds.R1.p8.y = 0;
	 Lds.R1.p8.z = 0;
	 Lds.R1.p9.x = 0;
	 Lds.R1.p9.y = 0;
	 Lds.R1.p9.z = 0;
	 Lds.R1.p10.x = 0;
	 Lds.R1.p10.y = 0;
	 Lds.R1.p10.z = 0;
	 Lds.R1.p11.x = 0;
	 Lds.R1.p11.y = 0;
	 Lds.R1.p11.z = 0;
	 Lds.R1.p12.x = 0;
	 Lds.R1.p12.y = 0;
	 Lds.R1.p12.z = 0;
	 Lds.R1.p13.x = 0;
	 Lds.R1.p13.y = 0;
	 Lds.R1.p13.z = 0;
	 Lds.R1.p14.x = 0;
	 Lds.R1.p14.y = 0;
	 Lds.R1.p14.z = 0;
	 Lds.R1.p15.x = 0;
	 Lds.R1.p15.y = 0;
	 Lds.R1.p15.z = 0;
	 Lds.R1.p16.x = 0;
	 Lds.R1.p16.y = 0;
	 Lds.R1.p16.z = 0;
	 Lds.R1.p17.x = 0;
	 Lds.R1.p17.y = 0;
	 Lds.R1.p17.z = 0;
	 Lds.R1.p18.x = 0;
	 Lds.R1.p18.y = 0;
	 Lds.R1.p18.z = 0;
	 Lds.R1.p19.x = 0;
	 Lds.R1.p19.y = 0;
	 Lds.R1.p19.z = 0;
	 Lds.R1.p20.x = 0;
	 Lds.R1.p20.y = 0;
	 Lds.R1.p20.z = 0;

     Lds.R2.p1.x = 0; 
     Lds.R2.p1.y = 0; 
     Lds.R2.p1.z = 0;
     Lds.R2.p2.x = 0;
     Lds.R2.p2.y = 0;
     Lds.R2.p2.z = 0;
     
    return 0;
}    
     
     
/*  
** call in User_Calc ()
*/   
     
int  
Lineds_Calc (double dt)
{    
     
    /*Execute only during simulation*/
    if (SimCore.State != SCState_Simulate) return 0;
     
    /*Execute only if at least 1 line is detected on the left or  right side*/
    if (LineSensor[0].RLines.nLine == 0 || LineSensor[0].LLines.nLine == 0) {
       Lds.L1.p1.x  = 0; 
       Lds.L1.p1.y  = 0; 
       Lds.L1.p1.z  = 0;
       Lds.L1.p2.x  = 0;
       Lds.L1.p2.y  = 0;
       Lds.L1.p2.z  = 0;
	   Lds.L1.p3.x = 0;
	   Lds.L1.p3.y = 0;
	   Lds.L1.p3.z = 0;
	   Lds.L1.p4.x = 0;
	   Lds.L1.p4.y = 0;
	   Lds.L1.p4.z = 0;
	   Lds.L1.p5.x = 0;
	   Lds.L1.p5.y = 0;
	   Lds.L1.p5.z = 0;
	   Lds.L1.p6.x = 0;
	   Lds.L1.p6.y = 0;
	   Lds.L1.p6.z = 0;
	   Lds.L1.p7.x = 0;
	   Lds.L1.p7.y = 0;
	   Lds.L1.p7.z = 0;
	   Lds.L1.p8.x = 0;
	   Lds.L1.p8.y = 0;
	   Lds.L1.p8.z = 0;
	   Lds.L1.p9.x = 0;
	   Lds.L1.p9.y = 0;
	   Lds.L1.p9.z = 0;
	   Lds.L1.p10.x = 0;
	   Lds.L1.p10.y = 0;
	   Lds.L1.p10.z = 0;
	   Lds.L1.p11.x = 0;
	   Lds.L1.p11.y = 0;
	   Lds.L1.p11.z = 0;
	   Lds.L1.p12.x = 0;
	   Lds.L1.p12.y = 0;
	   Lds.L1.p12.z = 0;
	   Lds.L1.p13.x = 0;
	   Lds.L1.p13.y = 0;
	   Lds.L1.p13.z = 0;
	   Lds.L1.p14.x = 0;
	   Lds.L1.p14.y = 0;
	   Lds.L1.p14.z = 0;
	   Lds.L1.p15.x = 0;
	   Lds.L1.p15.y = 0;
	   Lds.L1.p15.z = 0;
	   Lds.L1.p16.x = 0;
	   Lds.L1.p16.y = 0;
	   Lds.L1.p16.z = 0;
	   Lds.L1.p17.x = 0;
	   Lds.L1.p17.y = 0;
	   Lds.L1.p17.z = 0;
	   Lds.L1.p18.x = 0;
	   Lds.L1.p18.y = 0;
	   Lds.L1.p18.z = 0;
	   Lds.L1.p19.x = 0;
	   Lds.L1.p19.y = 0;
	   Lds.L1.p19.z = 0;
	   Lds.L1.p20.x = 0;
	   Lds.L1.p20.y = 0;
	   Lds.L1.p20.z = 0;

       Lds.L2.p1.x  = 0;
       Lds.L2.p1.y  = 0;
       Lds.L2.p1.z  = 0;
       Lds.L2.p2.x  = 0; 
       Lds.L2.p2.y  = 0; 
       Lds.L2.p2.z  = 0;

       Lds.R1.p1.x  = 0;
       Lds.R1.p1.y  = 0;
       Lds.R1.p1.z  = 0;
       Lds.R1.p2.x  = 0;
       Lds.R1.p2.y  = 0;
       Lds.R1.p2.z  = 0;
	   Lds.R1.p3.x = 0;
	   Lds.R1.p3.y = 0;
	   Lds.R1.p3.z = 0;
	   Lds.R1.p4.x = 0;
	   Lds.R1.p4.y = 0;
	   Lds.R1.p4.z = 0;
	   Lds.R1.p5.x = 0;
	   Lds.R1.p5.y = 0;
	   Lds.R1.p5.z = 0;
	   Lds.R1.p6.x = 0;
	   Lds.R1.p6.y = 0;
	   Lds.R1.p6.z = 0;
	   Lds.R1.p7.x = 0;
	   Lds.R1.p7.y = 0;
	   Lds.R1.p7.z = 0;
	   Lds.R1.p8.x = 0;
	   Lds.R1.p8.y = 0;
	   Lds.R1.p8.z = 0;
	   Lds.R1.p9.x = 0;
	   Lds.R1.p9.y = 0;
	   Lds.R1.p9.z = 0;
	   Lds.R1.p10.x = 0;
	   Lds.R1.p10.y = 0;
	   Lds.R1.p10.z = 0;
	   Lds.R1.p11.x = 0;
	   Lds.R1.p11.y = 0;
	   Lds.R1.p11.z = 0;
	   Lds.R1.p12.x = 0;
	   Lds.R1.p12.y = 0;
	   Lds.R1.p12.z = 0;
	   Lds.R1.p13.x = 0;
	   Lds.R1.p13.y = 0;
	   Lds.R1.p13.z = 0;
	   Lds.R1.p14.x = 0;
	   Lds.R1.p14.y = 0;
	   Lds.R1.p14.z = 0;
	   Lds.R1.p15.x = 0;
	   Lds.R1.p15.y = 0;
	   Lds.R1.p15.z = 0;
	   Lds.R1.p16.x = 0;
	   Lds.R1.p16.y = 0;
	   Lds.R1.p16.z = 0;
	   Lds.R1.p17.x = 0;
	   Lds.R1.p17.y = 0;
	   Lds.R1.p17.z = 0;
	   Lds.R1.p18.x = 0;
	   Lds.R1.p18.y = 0;
	   Lds.R1.p18.z = 0;
	   Lds.R1.p19.x = 0;
	   Lds.R1.p19.y = 0;
	   Lds.R1.p19.z = 0;
	   Lds.R1.p20.x = 0;
	   Lds.R1.p20.y = 0;
	   Lds.R1.p20.z = 0;

       Lds.R2.p1.x  = 0; 
       Lds.R2.p1.y  = 0; 
       Lds.R2.p1.z  = 0;
       Lds.R2.p2.x  = 0;
       Lds.R2.p2.y  = 0;
       Lds.R2.p2.z  = 0;

        return 0;
    };

        Lds.L1.p1.x = LineSensor[0].LLines.L[0].ds[0][0];
        Lds.L1.p1.y = LineSensor[0].LLines.L[0].ds[0][1];
        Lds.L1.p1.z = LineSensor[0].LLines.L[0].ds[0][2];
        Lds.L1.p2.x = LineSensor[0].LLines.L[0].ds[1][0];
        Lds.L1.p2.y = LineSensor[0].LLines.L[0].ds[1][1];
        Lds.L1.p2.z = LineSensor[0].LLines.L[0].ds[1][2];
		Lds.L1.p3.x = LineSensor[0].LLines.L[0].ds[2][0];
		Lds.L1.p3.y = LineSensor[0].LLines.L[0].ds[2][1];
		Lds.L1.p3.z = LineSensor[0].LLines.L[0].ds[2][2];
		Lds.L1.p4.x = LineSensor[0].LLines.L[0].ds[3][0];
		Lds.L1.p4.y = LineSensor[0].LLines.L[0].ds[3][1];
		Lds.L1.p4.z = LineSensor[0].LLines.L[0].ds[3][2];
		Lds.L1.p5.x = LineSensor[0].LLines.L[0].ds[4][0];
		Lds.L1.p5.y = LineSensor[0].LLines.L[0].ds[4][1];
		Lds.L1.p5.z = LineSensor[0].LLines.L[0].ds[4][2];
		Lds.L1.p6.x = LineSensor[0].LLines.L[0].ds[5][0];
		Lds.L1.p6.y = LineSensor[0].LLines.L[0].ds[5][1];
		Lds.L1.p6.z = LineSensor[0].LLines.L[0].ds[5][2];
		Lds.L1.p7.x = LineSensor[0].LLines.L[0].ds[6][0];
		Lds.L1.p7.y = LineSensor[0].LLines.L[0].ds[6][1];
		Lds.L1.p7.z = LineSensor[0].LLines.L[0].ds[6][2];
		Lds.L1.p8.x = LineSensor[0].LLines.L[0].ds[7][0];
		Lds.L1.p8.y = LineSensor[0].LLines.L[0].ds[7][1];
		Lds.L1.p8.z = LineSensor[0].LLines.L[0].ds[7][2];
		Lds.L1.p9.x = LineSensor[0].LLines.L[0].ds[8][0];
		Lds.L1.p9.y = LineSensor[0].LLines.L[0].ds[8][1];
		Lds.L1.p9.z = LineSensor[0].LLines.L[0].ds[8][2];
		Lds.L1.p10.x = LineSensor[0].LLines.L[0].ds[9][0];
		Lds.L1.p10.y = LineSensor[0].LLines.L[0].ds[9][1];
		Lds.L1.p10.z = LineSensor[0].LLines.L[0].ds[9][2];
		Lds.L1.p11.x = LineSensor[0].LLines.L[0].ds[10][0];
		Lds.L1.p11.y = LineSensor[0].LLines.L[0].ds[10][1];
		Lds.L1.p11.z = LineSensor[0].LLines.L[0].ds[10][2];
		Lds.L1.p12.x = LineSensor[0].LLines.L[0].ds[11][0];
		Lds.L1.p12.y = LineSensor[0].LLines.L[0].ds[11][1];
		Lds.L1.p12.z = LineSensor[0].LLines.L[0].ds[11][2];
		Lds.L1.p13.x = LineSensor[0].LLines.L[0].ds[12][0];
		Lds.L1.p13.y = LineSensor[0].LLines.L[0].ds[12][1];
		Lds.L1.p13.z = LineSensor[0].LLines.L[0].ds[12][2];
		Lds.L1.p14.x = LineSensor[0].LLines.L[0].ds[13][0];
		Lds.L1.p14.y = LineSensor[0].LLines.L[0].ds[13][1];
		Lds.L1.p14.z = LineSensor[0].LLines.L[0].ds[13][2];
		Lds.L1.p15.x = LineSensor[0].LLines.L[0].ds[14][0];
		Lds.L1.p15.y = LineSensor[0].LLines.L[0].ds[14][1];
		Lds.L1.p15.z = LineSensor[0].LLines.L[0].ds[14][2];
		Lds.L1.p16.x = LineSensor[0].LLines.L[0].ds[15][0];
		Lds.L1.p16.y = LineSensor[0].LLines.L[0].ds[15][1];
		Lds.L1.p16.z = LineSensor[0].LLines.L[0].ds[15][2];
		Lds.L1.p17.x = LineSensor[0].LLines.L[0].ds[16][0];
		Lds.L1.p17.y = LineSensor[0].LLines.L[0].ds[16][1];
		Lds.L1.p17.z = LineSensor[0].LLines.L[0].ds[16][2];
		Lds.L1.p18.x = LineSensor[0].LLines.L[0].ds[17][0];
		Lds.L1.p18.y = LineSensor[0].LLines.L[0].ds[17][1];
		Lds.L1.p18.z = LineSensor[0].LLines.L[0].ds[17][2];
		Lds.L1.p19.x = LineSensor[0].LLines.L[0].ds[18][0];
		Lds.L1.p19.y = LineSensor[0].LLines.L[0].ds[18][1];
		Lds.L1.p19.z = LineSensor[0].LLines.L[0].ds[18][2];
		Lds.L1.p20.x = LineSensor[0].LLines.L[0].ds[19][0];
		Lds.L1.p20.y = LineSensor[0].LLines.L[0].ds[19][1];
		Lds.L1.p20.z = LineSensor[0].LLines.L[0].ds[19][2];

        Lds.L2.p1.x = LineSensor[0].LLines.L[1].ds[0][0];
        Lds.L2.p1.y = LineSensor[0].LLines.L[1].ds[0][1];
        Lds.L2.p1.z = LineSensor[0].LLines.L[1].ds[0][2];
        Lds.L2.p2.x = LineSensor[0].LLines.L[1].ds[1][0];
        Lds.L2.p2.y = LineSensor[0].LLines.L[1].ds[1][1];
        Lds.L2.p2.z = LineSensor[0].LLines.L[1].ds[1][2];

        Lds.R1.p1.x = LineSensor[0].RLines.L[0].ds[0][0];
        Lds.R1.p1.y = LineSensor[0].RLines.L[0].ds[0][1]; 
        Lds.R1.p1.z = LineSensor[0].RLines.L[0].ds[0][2];  
        Lds.R1.p2.x = LineSensor[0].RLines.L[0].ds[1][0];  
        Lds.R1.p2.y = LineSensor[0].RLines.L[0].ds[1][1];
        Lds.R1.p2.z = LineSensor[0].RLines.L[0].ds[1][2];
		Lds.R1.p3.x = LineSensor[0].RLines.L[0].ds[2][0];
		Lds.R1.p3.y = LineSensor[0].RLines.L[0].ds[2][1];
		Lds.R1.p3.z = LineSensor[0].RLines.L[0].ds[2][2];
		Lds.R1.p4.x = LineSensor[0].RLines.L[0].ds[3][0];
		Lds.R1.p4.y = LineSensor[0].RLines.L[0].ds[3][1];
		Lds.R1.p4.z = LineSensor[0].RLines.L[0].ds[3][2];
		Lds.R1.p5.x = LineSensor[0].RLines.L[0].ds[4][0];
		Lds.R1.p5.y = LineSensor[0].RLines.L[0].ds[4][1];
		Lds.R1.p5.z = LineSensor[0].RLines.L[0].ds[4][2];
		Lds.R1.p6.x = LineSensor[0].RLines.L[0].ds[5][0];
		Lds.R1.p6.y = LineSensor[0].RLines.L[0].ds[5][1];
		Lds.R1.p6.z = LineSensor[0].RLines.L[0].ds[5][2];
		Lds.R1.p7.x = LineSensor[0].RLines.L[0].ds[6][0];
		Lds.R1.p7.y = LineSensor[0].RLines.L[0].ds[6][1];
		Lds.R1.p7.z = LineSensor[0].RLines.L[0].ds[6][2];
		Lds.R1.p8.x = LineSensor[0].RLines.L[0].ds[7][0];
		Lds.R1.p8.y = LineSensor[0].RLines.L[0].ds[7][1];
		Lds.R1.p8.z = LineSensor[0].RLines.L[0].ds[7][2];
		Lds.R1.p9.x = LineSensor[0].RLines.L[0].ds[8][0];
		Lds.R1.p9.y = LineSensor[0].RLines.L[0].ds[8][1];
		Lds.R1.p9.z = LineSensor[0].RLines.L[0].ds[8][2];
		Lds.R1.p10.x = LineSensor[0].RLines.L[0].ds[9][0];
		Lds.R1.p10.y = LineSensor[0].RLines.L[0].ds[9][1];
		Lds.R1.p10.z = LineSensor[0].RLines.L[0].ds[9][2];
		Lds.R1.p11.x = LineSensor[0].RLines.L[0].ds[10][0];
		Lds.R1.p11.y = LineSensor[0].RLines.L[0].ds[10][1];
		Lds.R1.p11.z = LineSensor[0].RLines.L[0].ds[10][2];
		Lds.R1.p12.x = LineSensor[0].RLines.L[0].ds[11][0];
		Lds.R1.p12.y = LineSensor[0].RLines.L[0].ds[11][1];
		Lds.R1.p12.z = LineSensor[0].RLines.L[0].ds[11][2];
		Lds.R1.p13.x = LineSensor[0].RLines.L[0].ds[12][0];
		Lds.R1.p13.y = LineSensor[0].RLines.L[0].ds[12][1];
		Lds.R1.p13.z = LineSensor[0].RLines.L[0].ds[12][2];
		Lds.R1.p14.x = LineSensor[0].RLines.L[0].ds[13][0];
		Lds.R1.p14.y = LineSensor[0].RLines.L[0].ds[13][1];
		Lds.R1.p14.z = LineSensor[0].RLines.L[0].ds[13][2];
		Lds.R1.p15.x = LineSensor[0].RLines.L[0].ds[14][0];
		Lds.R1.p15.y = LineSensor[0].RLines.L[0].ds[14][1];
		Lds.R1.p15.z = LineSensor[0].RLines.L[0].ds[14][2];
		Lds.R1.p16.x = LineSensor[0].RLines.L[0].ds[15][0];
		Lds.R1.p16.y = LineSensor[0].RLines.L[0].ds[15][1];
		Lds.R1.p16.z = LineSensor[0].RLines.L[0].ds[15][2];
		Lds.R1.p17.x = LineSensor[0].RLines.L[0].ds[16][0];
		Lds.R1.p17.y = LineSensor[0].RLines.L[0].ds[16][1];
		Lds.R1.p17.z = LineSensor[0].RLines.L[0].ds[16][2];
		Lds.R1.p18.x = LineSensor[0].RLines.L[0].ds[17][0];
		Lds.R1.p18.y = LineSensor[0].RLines.L[0].ds[17][1];
		Lds.R1.p18.z = LineSensor[0].RLines.L[0].ds[17][2];
		Lds.R1.p19.x = LineSensor[0].RLines.L[0].ds[18][0];
		Lds.R1.p19.y = LineSensor[0].RLines.L[0].ds[18][1];
		Lds.R1.p19.z = LineSensor[0].RLines.L[0].ds[18][2];
		Lds.R1.p20.x = LineSensor[0].RLines.L[0].ds[19][0];
		Lds.R1.p20.y = LineSensor[0].RLines.L[0].ds[19][1];
		Lds.R1.p20.z = LineSensor[0].RLines.L[0].ds[19][2];

        Lds.R2.p1.x = LineSensor[0].RLines.L[1].ds[0][0]; 
        Lds.R2.p1.y = LineSensor[0].RLines.L[1].ds[0][1]; 
        Lds.R2.p1.z = LineSensor[0].RLines.L[1].ds[0][2]; 
        Lds.R2.p2.x = LineSensor[0].RLines.L[1].ds[1][0]; 
        Lds.R2.p2.y = LineSensor[0].RLines.L[1].ds[1][1];  
        Lds.R2.p2.z = LineSensor[0].RLines.L[1].ds[1][2];  

        return 0;
}

4 打开src文件夹下的Makefile文件，将

复制代码

1
2
OBJS =			CM_Main.o CM_Vehicle.o User.o

改为

复制代码

1
2
OBJS = CM_Main.o CM_Vehicle.o User.o Lineds.o

5 打开工程文件CarMaker.sln，将Lineds.c加入到工程源文件列表中，点击生成->重新生成解决方案，成功的话会生成CarMaker.win64.exe文件。
在这里插入图片描述
6 通过快捷方式或者开始菜单打开carmaker,点击Application->Configuration/Status,在Command(executable)下面的框中选择生成的CarMaker.win64.exe文件，点击start&connect,然后点击close

7 回到主界面后，点击Application -> Direct variable access,按住Quantity下的向下箭头就能看到L1L2R1R2四条车道线的x,y,z的坐标点，可以选取几个进行观察。关闭carmaker。
在这里插入图片描述
8 通过simulink模型中的CM图标打开carmaker，点击File->Open打开之前我们新建的testrun。

9 打开IPGMovie观察动画，打开Direct Variable Access观察变量。点击Start开始运行仿真。可以看到已经能够读取车道线的坐标点。
在这里插入图片描述
10 在simulink模型中接收车道线点的数据。simulink模型中集成控制算法的位置为generic/CarMaker/VehicleControl/CreateBus VhclCtrl，所以在这里接受车道线的数据，通过Read CM Dict模块读取carmaker变量。

11 拟合左右车道线数据，生成车道线方程
在这里插入图片描述

使用matlab自带的拟合函数，一般采用三次拟合的方式，即可输出车道线方程。

复制代码

1
2
3
4
5
6
function [l1c,r1c] = fcn(l1x,l1y,r1x,r1y)

l1c = polyfit(l1x,l1y,3);
r1c = polyfit(r1x,r1y,3);
end

运行效果如图

在这里插入图片描述

最后

以上就是直率烤鸡最近收集整理的关于carmaker/matlab联合仿真(三) 生成车道线点，拟合多项式方程carmaker/matlab联合仿真(三) 生成车道线点，拟合多项式方程的全部内容，更多相关carmaker/matlab联合仿真(三)内容请搜索靠谱客的其他文章。

本图文内容来源于网友提供，作为学习参考使用，或来自网络收集整理，版权属于原作者所有。

本文分类：自动驾驶仿真
浏览次数：96 次浏览
发布日期：2023-05-26 06:14:01
本文链接：https://www.kaopuke.com/article/k-p-k_14_ujocf4_13__23__18_z.html

carmaker/matlab联合仿真(三) 生成车道线点，拟合多项式方程carmaker/matlab联合仿真(三) 生成车道线点，拟合多项式方程

carmaker/matlab联合仿真(三) 生成车道线点，拟合多项式方程

最后

评论列表共有 0 条评论

发表评论取消回复

carmaker/matlab联合仿真(三) 生成车道线点，拟合多项式方程carmaker/matlab联合仿真(三) 生成车道线点，拟合多项式方程

carmaker/matlab联合仿真(三) 生成车道线点，拟合多项式方程

最后

相关文章

评论列表共有 0 条评论

发表评论 取消回复

微信扫一扫：分享

发表评论取消回复