Skip to content

Commit 5ccbc5b

Browse files
JcZouJcZou
committed
[Boat-Controller] Add THROTTLE2_SCALE parameter
1 parent 9640ea8 commit 5ccbc5b

File tree

8 files changed

+103
-98
lines changed

8 files changed

+103
-98
lines changed

‎src/model/control/boat_controller/control_interface.c‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,7 @@ static param_t __param_list[] = {
4848
PARAM_FLOAT(BW_PSI_RATE_D, 0.05, false),
4949
PARAM_FLOAT(BW_PSI_RATE_FF, 0.0, false),
5050
PARAM_FLOAT(THROTTLE_SCALE, 400, false),
51+
PARAM_FLOAT(THROTTLE2_SCALE, 400, false),
5152
PARAM_UINT16(THROTTLE_BIAS, 1500, false),
5253
PARAM_UINT16(THROTTLE2_BIAS, 1500, false),
5354
PARAM_FLOAT(SERVO_SCALE, 250, false),
@@ -110,6 +111,7 @@ static void init_parameter(void)
110111
FMT_CHECK(param_link_variable(PARAM_GET(CONTROL, BW_PSI_RATE_D), &CONTROL_PARAM.BW_PSI_RATE_D));
111112
FMT_CHECK(param_link_variable(PARAM_GET(CONTROL, BW_PSI_RATE_FF), &CONTROL_PARAM.BW_PSI_RATE_FF));
112113
FMT_CHECK(param_link_variable(PARAM_GET(CONTROL, THROTTLE_SCALE), &CONTROL_PARAM.THROTTLE_SCALE));
114+
FMT_CHECK(param_link_variable(PARAM_GET(CONTROL, THROTTLE2_SCALE), &CONTROL_PARAM.THROTTLE2_SCALE));
113115
FMT_CHECK(param_link_variable(PARAM_GET(CONTROL, THROTTLE_BIAS), &CONTROL_PARAM.THROTTLE_BIAS));
114116
FMT_CHECK(param_link_variable(PARAM_GET(CONTROL, THROTTLE2_BIAS), &CONTROL_PARAM.THROTTLE2_BIAS));
115117
FMT_CHECK(param_link_variable(PARAM_GET(CONTROL, SERVO_SCALE), &CONTROL_PARAM.SERVO_SCALE));

‎src/model/control/boat_controller/lib/Controller.c‎

Lines changed: 83 additions & 82 deletions
Original file line numberDiff line numberDiff line change
@@ -3,9 +3,9 @@
33
*
44
* Code generated for Simulink model 'Controller'.
55
*
6-
* Model version : 1.1176
6+
* Model version : 1.1185
77
* Simulink Coder version : 9.0 (R2018b) 24-May-2018
8-
* C/C++ source code generated on : Fri May 16 14:46:58 2025
8+
* C/C++ source code generated on : Tue May 20 17:00:11 2025
99
*
1010
* Target selection: ert.tlc
1111
* Embedded hardware selection: ARM Compatible->ARM Cortex
@@ -25,18 +25,18 @@ const Control_Out_Bus Controller_rtZControl_Out_Bus = {
2525
} ; /* Control_Out_Bus ground */
2626

2727
/* Exported block parameters */
28-
struct_Zm37EZYmzJbgCrEnB3vf4C CONTROL_PARAM = {
28+
struct_KPrsn48OrR8nBbHqB69JGB CONTROL_PARAM = {
2929
0.5F,
3030
0.1F,
3131
0.0F,
32-
0.2F,
33-
-0.2F,
32+
0.6F,
33+
-0.6F,
3434
0.1F,
3535
-0.1F,
3636
0.0F,
3737
0.2F,
3838
0.0F,
39-
3.0F,
39+
2.0F,
4040
2.0F,
4141
0.2F,
4242
0.05F,
@@ -45,6 +45,7 @@ struct_Zm37EZYmzJbgCrEnB3vf4C CONTROL_PARAM = {
4545
-1.0F,
4646
500.0F,
4747
1500U,
48+
500.0F,
4849
1500U,
4950
250.0F,
5051
1500U,
@@ -62,6 +63,7 @@ struct_Zm37EZYmzJbgCrEnB3vf4C CONTROL_PARAM = {
6263
* '<S8>/Bias1'
6364
* '<S8>/Gain'
6465
* '<S8>/Gain1'
66+
* '<S8>/Gain2'
6567
* '<S8>/Saturation1'
6668
* '<S8>/Saturation2'
6769
* '<S15>/BW_FF'
@@ -124,7 +126,7 @@ void Controller_step(void)
124126
real32_T rtb_Gain;
125127
real32_T rtb_Sum;
126128
real32_T rtb_Gain_h;
127-
real32_T rtb_Gain_i;
129+
real32_T rtb_Gain_hj;
128130
int32_T i;
129131
real32_T rtb_VectorConcatenate_0[3];
130132

@@ -237,17 +239,17 @@ void Controller_step(void)
237239
* Gain: '<S39>/Gain1'
238240
*/
239241
if (Controller_U.FMS_Out.reset > 0) {
240-
rtb_Gain_i = 0.0F;
242+
rtb_Gain_hj = 0.0F;
241243
} else {
242-
rtb_Gain_i = rtb_Gain;
244+
rtb_Gain_hj = rtb_Gain;
243245
}
244246

245247
/* End of Switch: '<S39>/Switch' */
246248

247249
/* Product: '<S36>/Multiply' incorporates:
248250
* Constant: '<S36>/gain1'
249251
*/
250-
rtb_Saturation_g = CONTROL_PARAM.VEL_D * rtb_Gain_i;
252+
rtb_Saturation_g = CONTROL_PARAM.VEL_D * rtb_Gain_hj;
251253

252254
/* Saturate: '<S36>/Saturation' */
253255
if (rtb_Saturation_g > CONTROL_PARAM.VEL_D_MAX) {
@@ -374,24 +376,24 @@ void Controller_step(void)
374376
* Gain: '<S28>/Gain1'
375377
*/
376378
if (Controller_U.FMS_Out.reset > 0) {
377-
rtb_Gain_i = 0.0F;
379+
rtb_Gain_hj = 0.0F;
378380
} else {
379-
rtb_Gain_i = rtb_Gain_h;
381+
rtb_Gain_hj = rtb_Gain_h;
380382
}
381383

382384
/* End of Switch: '<S28>/Switch' */
383385

384386
/* Product: '<S25>/Multiply' incorporates:
385387
* Constant: '<S25>/gain1'
386388
*/
387-
rtb_Gain_i *= CONTROL_PARAM.FW_PSI_RATE_D;
389+
rtb_Gain_hj *= CONTROL_PARAM.FW_PSI_RATE_D;
388390

389391
/* Saturate: '<S25>/Saturation' */
390-
if (rtb_Gain_i > 0.2F) {
391-
rtb_Gain_i = 0.2F;
392+
if (rtb_Gain_hj > 0.2F) {
393+
rtb_Gain_hj = 0.2F;
392394
} else {
393-
if (rtb_Gain_i < -0.2F) {
394-
rtb_Gain_i = -0.2F;
395+
if (rtb_Gain_hj < -0.2F) {
396+
rtb_Gain_hj = -0.2F;
395397
}
396398
}
397399

@@ -404,7 +406,7 @@ void Controller_step(void)
404406
*/
405407
Controller_B.Merge = (CONTROL_PARAM.FW_PSI_RATE_P * rtb_Sum +
406408
Controller_DW.DiscreteTimeIntegrator_DSTATE_f) +
407-
rtb_Gain_i;
409+
rtb_Gain_hj;
408410

409411
/* Update for DiscreteIntegrator: '<S26>/Discrete-Time Integrator' incorporates:
410412
* Constant: '<S26>/gain1'
@@ -508,7 +510,7 @@ void Controller_step(void)
508510
* DiscreteIntegrator: '<S24>/Discrete-Time Integrator1'
509511
* Sum: '<S24>/Sum5'
510512
*/
511-
rtb_Gain_i = (rtb_Sum - Controller_DW.DiscreteTimeIntegrator1_DSTAT_p) *
513+
rtb_Gain_hj = (rtb_Sum - Controller_DW.DiscreteTimeIntegrator1_DSTAT_p) *
512514
188.49556F;
513515

514516
/* Switch: '<S24>/Switch' incorporates:
@@ -517,7 +519,7 @@ void Controller_step(void)
517519
if (rtb_Sum > 0.0F) {
518520
rtb_Gain_h = 0.0F;
519521
} else {
520-
rtb_Gain_h = rtb_Gain_i;
522+
rtb_Gain_h = rtb_Gain_hj;
521523
}
522524

523525
/* End of Switch: '<S24>/Switch' */
@@ -587,7 +589,7 @@ void Controller_step(void)
587589

588590
/* Update for DiscreteIntegrator: '<S24>/Discrete-Time Integrator1' */
589591
Controller_DW.DiscreteTimeIntegrator1_IC_LO_m = 0U;
590-
Controller_DW.DiscreteTimeIntegrator1_DSTAT_p += 0.01F * rtb_Gain_i;
592+
Controller_DW.DiscreteTimeIntegrator1_DSTAT_p += 0.01F * rtb_Gain_hj;
591593

592594
/* End of Outputs for SubSystem: '<S16>/Backward' */
593595
}
@@ -627,15 +629,15 @@ void Controller_step(void)
627629

628630
/* Saturate: '<S16>/Saturation' */
629631
if (Controller_B.Merge > 1.0F) {
630-
rtb_Gain_i = 1.0F;
632+
rtb_Gain_hj = 1.0F;
631633
} else if (Controller_B.Merge < -1.0F) {
632-
rtb_Gain_i = -1.0F;
634+
rtb_Gain_hj = -1.0F;
633635
} else {
634-
rtb_Gain_i = Controller_B.Merge;
636+
rtb_Gain_hj = Controller_B.Merge;
635637
}
636638

637639
/* End of Saturate: '<S16>/Saturation' */
638-
rtb_Saturation_g = rtb_Sum + rtb_Gain_i;
640+
rtb_Saturation_g = rtb_Sum + rtb_Gain_hj;
639641
} else {
640642
rtb_Saturation_g = Controller_U.FMS_Out.psi_rate_cmd;
641643
}
@@ -658,9 +660,10 @@ void Controller_step(void)
658660

659661
/* Output and update for atomic system: '<S4>/Boat_1' */
660662
{
663+
real32_T rtb_thruster;
661664
int32_T i_j;
665+
real32_T tmp_j;
662666
uint16_T u0_j;
663-
real32_T u0_c;
664667

665668
/* MultiPortSwitch: '<S9>/Multiport Switch' incorporates:
666669
* Inport: '<Root>/FMS_Out'
@@ -687,11 +690,11 @@ void Controller_step(void)
687690
case 3:
688691
/* Saturate: '<S7>/Saturation1' */
689692
if (rtb_Saturation > CONTROL_PARAM.THROTTLE_MAX) {
690-
u0_c = CONTROL_PARAM.THROTTLE_MAX;
693+
rtb_thruster = CONTROL_PARAM.THROTTLE_MAX;
691694
} else if (rtb_Saturation < CONTROL_PARAM.THROTTLE_MIN) {
692-
u0_c = CONTROL_PARAM.THROTTLE_MIN;
695+
rtb_thruster = CONTROL_PARAM.THROTTLE_MIN;
693696
} else {
694-
u0_c = rtb_Saturation;
697+
rtb_thruster = rtb_Saturation;
695698
}
696699

697700
/* End of Saturate: '<S7>/Saturation1' */
@@ -700,15 +703,15 @@ void Controller_step(void)
700703
* Bias: '<S7>/Bias'
701704
* Gain: '<S7>/Gain'
702705
*/
703-
u0_c = fmodf(floorf(CONTROL_PARAM.THROTTLE_SCALE * u0_c + (real32_T)
704-
CONTROL_PARAM.THROTTLE_BIAS), 65536.0F);
705-
u0_j = (uint16_T)(u0_c < 0.0F ? (int32_T)(uint16_T)-(int16_T)(uint16_T)
706-
-u0_c : (int32_T)(uint16_T)u0_c);
706+
tmp_j = fmodf(floorf(CONTROL_PARAM.THROTTLE_SCALE * rtb_thruster +
707+
(real32_T)CONTROL_PARAM.THROTTLE_BIAS), 65536.0F);
708+
u0_j = (uint16_T)(tmp_j < 0.0F ? (int32_T)(uint16_T)-(int16_T)(uint16_T)
709+
-tmp_j : (int32_T)(uint16_T)tmp_j);
707710

708711
/* Saturate: '<S7>/Saturation' */
709-
if (u0_j > 1950) {
712+
if (u0_j > 2000) {
710713
/* Reshape: '<S9>/Reshape' */
711-
rtb_VariantMergeForOutportactua[0] = 1950U;
714+
rtb_VariantMergeForOutportactua[0] = 2000U;
712715
} else if (u0_j < 1000) {
713716
/* Reshape: '<S9>/Reshape' */
714717
rtb_VariantMergeForOutportactua[0] = 1000U;
@@ -717,27 +720,26 @@ void Controller_step(void)
717720
rtb_VariantMergeForOutportactua[0] = u0_j;
718721
}
719722

720-
/* Switch: '<S7>/Switch' incorporates:
721-
* Constant: '<S7>/Constant'
722-
* Constant: '<S7>/Constant1'
723-
*/
724-
if (rtb_Saturation >= 0.0F) {
725-
i_j = 1;
723+
/* Signum: '<S7>/Sign' */
724+
if (rtb_thruster < 0.0F) {
725+
rtb_thruster = -1.0F;
726726
} else {
727-
i_j = -1;
727+
if (rtb_thruster > 0.0F) {
728+
rtb_thruster = 1.0F;
729+
}
728730
}
729731

730-
/* End of Switch: '<S7>/Switch' */
732+
/* End of Signum: '<S7>/Sign' */
731733

732734
/* Product: '<S7>/Multiply' */
733-
u0_c = (real32_T)i_j * rtb_Saturation_g;
735+
rtb_thruster *= rtb_Saturation_g;
734736

735737
/* Saturate: '<S7>/Saturation2' */
736-
if (u0_c > CONTROL_PARAM.SERVO_MAX) {
737-
u0_c = CONTROL_PARAM.SERVO_MAX;
738+
if (rtb_thruster > CONTROL_PARAM.SERVO_MAX) {
739+
rtb_thruster = CONTROL_PARAM.SERVO_MAX;
738740
} else {
739-
if (u0_c < CONTROL_PARAM.SERVO_MIN) {
740-
u0_c = CONTROL_PARAM.SERVO_MIN;
741+
if (rtb_thruster < CONTROL_PARAM.SERVO_MIN) {
742+
rtb_thruster = CONTROL_PARAM.SERVO_MIN;
741743
}
742744
}
743745

@@ -747,15 +749,15 @@ void Controller_step(void)
747749
* Bias: '<S7>/Bias1'
748750
* Gain: '<S7>/Gain1'
749751
*/
750-
u0_c = fmodf(floorf(CONTROL_PARAM.SERVO_SCALE * u0_c + (real32_T)
751-
CONTROL_PARAM.SERVO_BIAS), 65536.0F);
752-
u0_j = (uint16_T)(u0_c < 0.0F ? (int32_T)(uint16_T)-(int16_T)(uint16_T)
753-
-u0_c : (int32_T)(uint16_T)u0_c);
752+
tmp_j = fmodf(floorf(CONTROL_PARAM.SERVO_SCALE * rtb_thruster + (real32_T)
753+
CONTROL_PARAM.SERVO_BIAS), 65536.0F);
754+
u0_j = (uint16_T)(tmp_j < 0.0F ? (int32_T)(uint16_T)-(int16_T)(uint16_T)
755+
-tmp_j : (int32_T)(uint16_T)tmp_j);
754756

755757
/* Saturate: '<S7>/Saturation' */
756-
if (u0_j > 1950) {
758+
if (u0_j > 2000) {
757759
/* Reshape: '<S9>/Reshape' */
758-
rtb_VariantMergeForOutportactua[1] = 1950U;
760+
rtb_VariantMergeForOutportactua[1] = 2000U;
759761
} else if (u0_j < 1000) {
760762
/* Reshape: '<S9>/Reshape' */
761763
rtb_VariantMergeForOutportactua[1] = 1000U;
@@ -787,7 +789,7 @@ void Controller_step(void)
787789

788790
/* Output and update for atomic system: '<S4>/Boat_2' */
789791
{
790-
real32_T rtb_Gain1_c4;
792+
real32_T rtb_Saturation1;
791793
real32_T rtb_thruster1;
792794
int32_T i_e;
793795
real32_T u0_e;
@@ -815,6 +817,17 @@ void Controller_step(void)
815817
break;
816818

817819
case 3:
820+
/* Saturate: '<S8>/Saturation1' */
821+
if (rtb_Saturation > CONTROL_PARAM.THROTTLE_MAX) {
822+
rtb_Saturation1 = CONTROL_PARAM.THROTTLE_MAX;
823+
} else if (rtb_Saturation < CONTROL_PARAM.THROTTLE_MIN) {
824+
rtb_Saturation1 = CONTROL_PARAM.THROTTLE_MIN;
825+
} else {
826+
rtb_Saturation1 = rtb_Saturation;
827+
}
828+
829+
/* End of Saturate: '<S8>/Saturation1' */
830+
818831
/* Saturate: '<S8>/Saturation2' */
819832
if (rtb_Saturation_g > CONTROL_PARAM.SERVO_MAX) {
820833
rtb_thruster1 = CONTROL_PARAM.SERVO_MAX;
@@ -827,34 +840,21 @@ void Controller_step(void)
827840
/* End of Saturate: '<S8>/Saturation2' */
828841

829842
/* Gain: '<S8>/Gain1' */
830-
rtb_Gain1_c4 = CONTROL_PARAM.SERVO_SCALE * rtb_thruster1;
831-
832-
/* Saturate: '<S8>/Saturation1' */
833-
if (rtb_Saturation > CONTROL_PARAM.THROTTLE_MAX) {
834-
rtb_thruster1 = CONTROL_PARAM.THROTTLE_MAX;
835-
} else if (rtb_Saturation < CONTROL_PARAM.THROTTLE_MIN) {
836-
rtb_thruster1 = CONTROL_PARAM.THROTTLE_MIN;
837-
} else {
838-
rtb_thruster1 = rtb_Saturation;
839-
}
840-
841-
/* End of Saturate: '<S8>/Saturation1' */
842-
843-
/* Gain: '<S8>/Gain' */
844-
rtb_thruster1 *= CONTROL_PARAM.THROTTLE_SCALE;
843+
rtb_thruster1 *= CONTROL_PARAM.SERVO_SCALE;
845844

846845
/* Bias: '<S8>/Bias' incorporates:
846+
* Gain: '<S8>/Gain'
847847
* Sum: '<S8>/Add'
848848
*/
849-
u0_e = (rtb_thruster1 + rtb_Gain1_c4) + (real32_T)
850-
CONTROL_PARAM.THROTTLE_BIAS;
849+
u0_e = (CONTROL_PARAM.THROTTLE_SCALE * rtb_Saturation1 + rtb_thruster1) +
850+
(real32_T)CONTROL_PARAM.THROTTLE_BIAS;
851851

852852
/* Saturate: '<S8>/Saturation' */
853-
if (u0_e > 1900.0F) {
854-
u0_e = 1900.0F;
853+
if (u0_e > 2000.0F) {
854+
u0_e = 2000.0F;
855855
} else {
856-
if (u0_e < 1100.0F) {
857-
u0_e = 1100.0F;
856+
if (u0_e < 1000.0F) {
857+
u0_e = 1000.0F;
858858
}
859859
}
860860

@@ -865,17 +865,18 @@ void Controller_step(void)
865865
65536.0F);
866866

867867
/* Bias: '<S8>/Bias1' incorporates:
868+
* Gain: '<S8>/Gain2'
868869
* Sum: '<S8>/Add1'
869870
*/
870-
u0_e = (rtb_thruster1 - rtb_Gain1_c4) + (real32_T)
871-
CONTROL_PARAM.THROTTLE2_BIAS;
871+
u0_e = (CONTROL_PARAM.THROTTLE2_SCALE * rtb_Saturation1 - rtb_thruster1) +
872+
(real32_T)CONTROL_PARAM.THROTTLE2_BIAS;
872873

873874
/* Saturate: '<S8>/Saturation' */
874-
if (u0_e > 1900.0F) {
875-
u0_e = 1900.0F;
875+
if (u0_e > 2000.0F) {
876+
u0_e = 2000.0F;
876877
} else {
877-
if (u0_e < 1100.0F) {
878-
u0_e = 1100.0F;
878+
if (u0_e < 1000.0F) {
879+
u0_e = 1000.0F;
879880
}
880881
}
881882

0 commit comments

Comments
 (0)