From de5aecb0f48936a4928d461609cb4b25eb31bc5a Mon Sep 17 00:00:00 2001 From: Josh Zou Date: Thu, 7 Nov 2024 11:18:28 +0800 Subject: [PATCH] Merge Car/Boat FMS which fix prearm issue --- src/model/fms/boat_fms/lib/FMS.c | 64 +++---------- src/model/fms/boat_fms/lib/FMS.h | 4 +- src/model/fms/boat_fms/lib/FMS_data.c | 4 +- src/model/fms/boat_fms/lib/FMS_private.h | 4 +- src/model/fms/boat_fms/lib/FMS_types.h | 4 +- .../fms/boat_fms/lib/builtin_typeid_types.h | 4 +- src/model/fms/boat_fms/lib/rtmodel.h | 4 +- src/model/fms/boat_fms/lib/rtwtypes.h | 4 +- .../fms/boat_fms/lib/zero_crossing_types.h | 4 +- src/model/fms/car_fms/lib/FMS.c | 95 +++++-------------- src/model/fms/car_fms/lib/FMS.h | 6 +- src/model/fms/car_fms/lib/FMS_data.c | 4 +- src/model/fms/car_fms/lib/FMS_private.h | 4 +- src/model/fms/car_fms/lib/FMS_types.h | 4 +- .../fms/car_fms/lib/builtin_typeid_types.h | 4 +- src/model/fms/car_fms/lib/rtmodel.h | 4 +- src/model/fms/car_fms/lib/rtwtypes.h | 4 +- .../fms/car_fms/lib/zero_crossing_types.h | 4 +- 18 files changed, 70 insertions(+), 155 deletions(-) diff --git a/src/model/fms/boat_fms/lib/FMS.c b/src/model/fms/boat_fms/lib/FMS.c index a0439319..db86f355 100644 --- a/src/model/fms/boat_fms/lib/FMS.c +++ b/src/model/fms/boat_fms/lib/FMS.c @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'FMS'. * - * Model version : 1.1984 + * Model version : 1.1985 * Simulink Coder version : 9.0 (R2018b) 24-May-2018 - * C/C++ source code generated on : Thu Jul 18 14:49:54 2024 + * C/C++ source code generated on : Thu Nov 7 11:14:41 2024 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex @@ -266,15 +266,6 @@ static boolean_T FMS_CheckCmdValid(FMS_Cmd cmd_in, PilotMode mode_in, uint32_T ins_flag) { boolean_T valid; - int32_T b_index; - int32_T j; - boolean_T y; - boolean_T x[2]; - static const PilotMode varargin_1[2] = { PilotMode_Position, - PilotMode_Offboard }; - - int32_T exitg1; - boolean_T exitg2; valid = false; if ((ins_flag & 1U) != 0U) { switch (cmd_in) { @@ -287,60 +278,27 @@ static boolean_T FMS_CheckCmdValid(FMS_Cmd cmd_in, PilotMode mode_in, uint32_T break; case FMS_Cmd_PreArm: - b_index = -1; - j = 0; - do { - exitg1 = 0; - if (j < 2) { - if (varargin_1[j] == mode_in) { - b_index = 0; - exitg1 = 1; - } else { - j++; - } - } else { - if (PilotMode_Mission == mode_in) { - b_index = 1; - } else { - x[0] = (PilotMode_Stabilize == mode_in); - x[1] = (PilotMode_Manual == mode_in); - y = true; - j = 0; - exitg2 = false; - while ((!exitg2) && (j < 2)) { - if (!x[j]) { - y = false; - exitg2 = true; - } else { - j++; - } - } - - if (y) { - b_index = 2; - } - } - - exitg1 = 1; - } - } while (exitg1 == 0); - - switch (b_index) { - case 0: + switch (mode_in) { + case PilotMode_Position: + case PilotMode_Offboard: if (((ins_flag & 8U) != 0U) && ((ins_flag & 16U) != 0U) && ((ins_flag & 64U) != 0U)) { valid = true; } break; - case 1: + case PilotMode_Mission: if (((ins_flag & 8U) != 0U) && ((ins_flag & 16U) != 0U) && ((ins_flag & 32U) != 0U) && ((ins_flag & 64U) != 0U)) { valid = true; } break; - case 2: + case PilotMode_Stabilize: + valid = true; + break; + + case PilotMode_Manual: valid = true; break; } diff --git a/src/model/fms/boat_fms/lib/FMS.h b/src/model/fms/boat_fms/lib/FMS.h index ae23dd09..9cf0e2b5 100644 --- a/src/model/fms/boat_fms/lib/FMS.h +++ b/src/model/fms/boat_fms/lib/FMS.h @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'FMS'. * - * Model version : 1.1984 + * Model version : 1.1985 * Simulink Coder version : 9.0 (R2018b) 24-May-2018 - * C/C++ source code generated on : Thu Jul 18 14:49:54 2024 + * C/C++ source code generated on : Thu Nov 7 11:14:41 2024 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex diff --git a/src/model/fms/boat_fms/lib/FMS_data.c b/src/model/fms/boat_fms/lib/FMS_data.c index 8a2ba3d6..6169bd51 100644 --- a/src/model/fms/boat_fms/lib/FMS_data.c +++ b/src/model/fms/boat_fms/lib/FMS_data.c @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'FMS'. * - * Model version : 1.1984 + * Model version : 1.1985 * Simulink Coder version : 9.0 (R2018b) 24-May-2018 - * C/C++ source code generated on : Thu Jul 18 14:49:54 2024 + * C/C++ source code generated on : Thu Nov 7 11:14:41 2024 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex diff --git a/src/model/fms/boat_fms/lib/FMS_private.h b/src/model/fms/boat_fms/lib/FMS_private.h index 60108638..4dc75ebd 100644 --- a/src/model/fms/boat_fms/lib/FMS_private.h +++ b/src/model/fms/boat_fms/lib/FMS_private.h @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'FMS'. * - * Model version : 1.1984 + * Model version : 1.1985 * Simulink Coder version : 9.0 (R2018b) 24-May-2018 - * C/C++ source code generated on : Thu Jul 18 14:49:54 2024 + * C/C++ source code generated on : Thu Nov 7 11:14:41 2024 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex diff --git a/src/model/fms/boat_fms/lib/FMS_types.h b/src/model/fms/boat_fms/lib/FMS_types.h index dd470223..217e7580 100644 --- a/src/model/fms/boat_fms/lib/FMS_types.h +++ b/src/model/fms/boat_fms/lib/FMS_types.h @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'FMS'. * - * Model version : 1.1984 + * Model version : 1.1985 * Simulink Coder version : 9.0 (R2018b) 24-May-2018 - * C/C++ source code generated on : Thu Jul 18 14:49:54 2024 + * C/C++ source code generated on : Thu Nov 7 11:14:41 2024 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex diff --git a/src/model/fms/boat_fms/lib/builtin_typeid_types.h b/src/model/fms/boat_fms/lib/builtin_typeid_types.h index f78afb06..e5ae590c 100644 --- a/src/model/fms/boat_fms/lib/builtin_typeid_types.h +++ b/src/model/fms/boat_fms/lib/builtin_typeid_types.h @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'FMS'. * - * Model version : 1.1984 + * Model version : 1.1985 * Simulink Coder version : 9.0 (R2018b) 24-May-2018 - * C/C++ source code generated on : Thu Jul 18 14:49:54 2024 + * C/C++ source code generated on : Thu Nov 7 11:14:41 2024 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex diff --git a/src/model/fms/boat_fms/lib/rtmodel.h b/src/model/fms/boat_fms/lib/rtmodel.h index c6d16c3d..fdf9bd1d 100644 --- a/src/model/fms/boat_fms/lib/rtmodel.h +++ b/src/model/fms/boat_fms/lib/rtmodel.h @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'FMS'. * - * Model version : 1.1984 + * Model version : 1.1985 * Simulink Coder version : 9.0 (R2018b) 24-May-2018 - * C/C++ source code generated on : Thu Jul 18 14:49:54 2024 + * C/C++ source code generated on : Thu Nov 7 11:14:41 2024 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex diff --git a/src/model/fms/boat_fms/lib/rtwtypes.h b/src/model/fms/boat_fms/lib/rtwtypes.h index 69db6bee..558b00b7 100644 --- a/src/model/fms/boat_fms/lib/rtwtypes.h +++ b/src/model/fms/boat_fms/lib/rtwtypes.h @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'FMS'. * - * Model version : 1.1984 + * Model version : 1.1985 * Simulink Coder version : 9.0 (R2018b) 24-May-2018 - * C/C++ source code generated on : Thu Jul 18 14:49:54 2024 + * C/C++ source code generated on : Thu Nov 7 11:14:41 2024 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex diff --git a/src/model/fms/boat_fms/lib/zero_crossing_types.h b/src/model/fms/boat_fms/lib/zero_crossing_types.h index 0d0ba5b1..311e350e 100644 --- a/src/model/fms/boat_fms/lib/zero_crossing_types.h +++ b/src/model/fms/boat_fms/lib/zero_crossing_types.h @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'FMS'. * - * Model version : 1.1984 + * Model version : 1.1985 * Simulink Coder version : 9.0 (R2018b) 24-May-2018 - * C/C++ source code generated on : Thu Jul 18 14:49:54 2024 + * C/C++ source code generated on : Thu Nov 7 11:14:41 2024 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex diff --git a/src/model/fms/car_fms/lib/FMS.c b/src/model/fms/car_fms/lib/FMS.c index a4b5fc7a..bd9d6625 100644 --- a/src/model/fms/car_fms/lib/FMS.c +++ b/src/model/fms/car_fms/lib/FMS.c @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'FMS'. * - * Model version : 1.1999 + * Model version : 1.2001 * Simulink Coder version : 9.0 (R2018b) 24-May-2018 - * C/C++ source code generated on : Mon Jul 15 19:32:31 2024 + * C/C++ source code generated on : Thu Nov 7 11:16:11 2024 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex @@ -163,6 +163,7 @@ static boolean_T FMS_BottomLeft(real32_T pilot_cmd_stick_yaw, real32_T pilot_cmd_stick_throttle); static boolean_T FMS_sf_msg_pop_M(void); static real32_T FMS_norm(const real32_T x[2]); +static void FMS_exit_internal_Auto(void); static real_T FMS_getArmMode(PilotMode pilotMode); static void FMS_enter_internal_Auto(void); static void FMS_enter_internal_Arm(void); @@ -267,15 +268,6 @@ static boolean_T FMS_CheckCmdValid(FMS_Cmd cmd_in, PilotMode mode_in, uint32_T ins_flag) { boolean_T valid; - int32_T b_index; - int32_T j; - boolean_T y; - boolean_T x[2]; - static const PilotMode varargin_1[2] = { PilotMode_Position, - PilotMode_Offboard }; - - int32_T exitg1; - boolean_T exitg2; valid = false; if ((ins_flag & 1U) != 0U) { switch (cmd_in) { @@ -288,60 +280,27 @@ static boolean_T FMS_CheckCmdValid(FMS_Cmd cmd_in, PilotMode mode_in, uint32_T break; case FMS_Cmd_PreArm: - b_index = -1; - j = 0; - do { - exitg1 = 0; - if (j < 2) { - if (varargin_1[j] == mode_in) { - b_index = 0; - exitg1 = 1; - } else { - j++; - } - } else { - if (PilotMode_Mission == mode_in) { - b_index = 1; - } else { - x[0] = (PilotMode_Stabilize == mode_in); - x[1] = (PilotMode_Manual == mode_in); - y = true; - j = 0; - exitg2 = false; - while ((!exitg2) && (j < 2)) { - if (!x[j]) { - y = false; - exitg2 = true; - } else { - j++; - } - } - - if (y) { - b_index = 2; - } - } - - exitg1 = 1; - } - } while (exitg1 == 0); - - switch (b_index) { - case 0: + switch (mode_in) { + case PilotMode_Position: + case PilotMode_Offboard: if (((ins_flag & 8U) != 0U) && ((ins_flag & 16U) != 0U) && ((ins_flag & 64U) != 0U)) { valid = true; } break; - case 1: + case PilotMode_Mission: if (((ins_flag & 8U) != 0U) && ((ins_flag & 16U) != 0U) && ((ins_flag & 32U) != 0U) && ((ins_flag & 64U) != 0U)) { valid = true; } break; - case 2: + case PilotMode_Stabilize: + valid = true; + break; + + case PilotMode_Manual: valid = true; break; } @@ -478,6 +437,18 @@ static real32_T FMS_norm(const real32_T x[2]) return scale * sqrtf(y); } +/* Function for Chart: '/FMS State Machine' */ +static void FMS_exit_internal_Auto(void) +{ + if (FMS_DW.is_Auto == FMS_IN_Mission) { + FMS_DW.is_Mission = FMS_IN_NO_ACTIVE_CHILD; + FMS_DW.is_Auto = FMS_IN_NO_ACTIVE_CHILD; + } else { + FMS_DW.is_Offboard = FMS_IN_NO_ACTIVE_CHILD; + FMS_DW.is_Auto = FMS_IN_NO_ACTIVE_CHILD; + } +} + /* Function for Chart: '/FMS State Machine' */ static real_T FMS_getArmMode(PilotMode pilotMode) { @@ -742,14 +713,7 @@ static void FMS_SubMode(void) static void FMS_exit_internal_Arm(void) { if (FMS_DW.is_Arm == FMS_IN_Auto) { - if (FMS_DW.is_Auto == FMS_IN_Mission) { - FMS_DW.is_Mission = FMS_IN_NO_ACTIVE_CHILD; - FMS_DW.is_Auto = FMS_IN_NO_ACTIVE_CHILD; - } else { - FMS_DW.is_Offboard = FMS_IN_NO_ACTIVE_CHILD; - FMS_DW.is_Auto = FMS_IN_NO_ACTIVE_CHILD; - } - + FMS_exit_internal_Auto(); FMS_DW.is_Arm = FMS_IN_NO_ACTIVE_CHILD; } else { FMS_DW.is_Assist = FMS_IN_NO_ACTIVE_CHILD; @@ -849,14 +813,7 @@ static void FMS_Arm(void) } if (b_sf_internal_predicateOutput) { - if (FMS_DW.is_Auto == FMS_IN_Mission) { - FMS_DW.is_Mission = FMS_IN_NO_ACTIVE_CHILD; - FMS_DW.is_Auto = FMS_IN_NO_ACTIVE_CHILD; - } else { - FMS_DW.is_Offboard = FMS_IN_NO_ACTIVE_CHILD; - FMS_DW.is_Auto = FMS_IN_NO_ACTIVE_CHILD; - } - + FMS_exit_internal_Auto(); FMS_DW.is_Arm = FMS_IN_SubMode; FMS_DW.stick_val[0] = FMS_B.BusConversion_InsertedFor_FMS_f.stick_yaw; FMS_DW.stick_val[1] = diff --git a/src/model/fms/car_fms/lib/FMS.h b/src/model/fms/car_fms/lib/FMS.h index cdd2863c..c171365b 100644 --- a/src/model/fms/car_fms/lib/FMS.h +++ b/src/model/fms/car_fms/lib/FMS.h @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'FMS'. * - * Model version : 1.1999 + * Model version : 1.2001 * Simulink Coder version : 9.0 (R2018b) 24-May-2018 - * C/C++ source code generated on : Mon Jul 15 19:32:31 2024 + * C/C++ source code generated on : Thu Nov 7 11:16:11 2024 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex @@ -120,12 +120,12 @@ typedef struct { uint8_T is_Arm; /* '/FMS State Machine' */ uint8_T is_SubMode; /* '/FMS State Machine' */ uint8_T is_Auto; /* '/FMS State Machine' */ + uint8_T is_Offboard; /* '/FMS State Machine' */ uint8_T is_Mission; /* '/FMS State Machine' */ uint8_T is_Assist; /* '/FMS State Machine' */ uint8_T is_Manual; /* '/FMS State Machine' */ uint8_T is_Combo_Stick; /* '/FMS State Machine' */ uint8_T is_active_Combo_Stick; /* '/FMS State Machine' */ - uint8_T is_Offboard; /* '/FMS State Machine' */ uint8_T is_active_c10_FMS; /* '/Motion State' */ uint8_T is_c10_FMS; /* '/Motion State' */ uint8_T icLoad; /* '/start_vel' */ diff --git a/src/model/fms/car_fms/lib/FMS_data.c b/src/model/fms/car_fms/lib/FMS_data.c index 668e15a9..544b304a 100644 --- a/src/model/fms/car_fms/lib/FMS_data.c +++ b/src/model/fms/car_fms/lib/FMS_data.c @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'FMS'. * - * Model version : 1.1999 + * Model version : 1.2001 * Simulink Coder version : 9.0 (R2018b) 24-May-2018 - * C/C++ source code generated on : Mon Jul 15 19:32:31 2024 + * C/C++ source code generated on : Thu Nov 7 11:16:11 2024 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex diff --git a/src/model/fms/car_fms/lib/FMS_private.h b/src/model/fms/car_fms/lib/FMS_private.h index 138e7267..fdae38c6 100644 --- a/src/model/fms/car_fms/lib/FMS_private.h +++ b/src/model/fms/car_fms/lib/FMS_private.h @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'FMS'. * - * Model version : 1.1999 + * Model version : 1.2001 * Simulink Coder version : 9.0 (R2018b) 24-May-2018 - * C/C++ source code generated on : Mon Jul 15 19:32:31 2024 + * C/C++ source code generated on : Thu Nov 7 11:16:11 2024 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex diff --git a/src/model/fms/car_fms/lib/FMS_types.h b/src/model/fms/car_fms/lib/FMS_types.h index 16823dcf..8478a6a8 100644 --- a/src/model/fms/car_fms/lib/FMS_types.h +++ b/src/model/fms/car_fms/lib/FMS_types.h @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'FMS'. * - * Model version : 1.1999 + * Model version : 1.2001 * Simulink Coder version : 9.0 (R2018b) 24-May-2018 - * C/C++ source code generated on : Mon Jul 15 19:32:31 2024 + * C/C++ source code generated on : Thu Nov 7 11:16:11 2024 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex diff --git a/src/model/fms/car_fms/lib/builtin_typeid_types.h b/src/model/fms/car_fms/lib/builtin_typeid_types.h index b3abbf0b..64aeac7c 100644 --- a/src/model/fms/car_fms/lib/builtin_typeid_types.h +++ b/src/model/fms/car_fms/lib/builtin_typeid_types.h @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'FMS'. * - * Model version : 1.1999 + * Model version : 1.2001 * Simulink Coder version : 9.0 (R2018b) 24-May-2018 - * C/C++ source code generated on : Mon Jul 15 19:32:31 2024 + * C/C++ source code generated on : Thu Nov 7 11:16:11 2024 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex diff --git a/src/model/fms/car_fms/lib/rtmodel.h b/src/model/fms/car_fms/lib/rtmodel.h index cbc5f48f..0254b495 100644 --- a/src/model/fms/car_fms/lib/rtmodel.h +++ b/src/model/fms/car_fms/lib/rtmodel.h @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'FMS'. * - * Model version : 1.1999 + * Model version : 1.2001 * Simulink Coder version : 9.0 (R2018b) 24-May-2018 - * C/C++ source code generated on : Mon Jul 15 19:32:31 2024 + * C/C++ source code generated on : Thu Nov 7 11:16:11 2024 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex diff --git a/src/model/fms/car_fms/lib/rtwtypes.h b/src/model/fms/car_fms/lib/rtwtypes.h index 11b5cace..43277dd2 100644 --- a/src/model/fms/car_fms/lib/rtwtypes.h +++ b/src/model/fms/car_fms/lib/rtwtypes.h @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'FMS'. * - * Model version : 1.1999 + * Model version : 1.2001 * Simulink Coder version : 9.0 (R2018b) 24-May-2018 - * C/C++ source code generated on : Mon Jul 15 19:32:31 2024 + * C/C++ source code generated on : Thu Nov 7 11:16:11 2024 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex diff --git a/src/model/fms/car_fms/lib/zero_crossing_types.h b/src/model/fms/car_fms/lib/zero_crossing_types.h index 3beb99db..dbfdac92 100644 --- a/src/model/fms/car_fms/lib/zero_crossing_types.h +++ b/src/model/fms/car_fms/lib/zero_crossing_types.h @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'FMS'. * - * Model version : 1.1999 + * Model version : 1.2001 * Simulink Coder version : 9.0 (R2018b) 24-May-2018 - * C/C++ source code generated on : Mon Jul 15 19:32:31 2024 + * C/C++ source code generated on : Thu Nov 7 11:16:11 2024 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex