Updated ahrs block to provide d outputs for all signals. NOTE OUTPUT …

…PORT NUMBER CHANGE

sw/airborne/modules/ahrs/comp_nav_filter.c

@@ -7,9 +7,9 @@
  *
  * Code generated for Simulink model 'comp_nav_filter'.
  *
- * Model version                  : 1.143
- * Simulink Coder version         : 8.10 (R2016a) 10-Feb-2016
- * C/C++ source code generated on : Tue Feb 28 02:05:18 2017
+ * Model version                  : 1.147
+ * Simulink Coder version         : 8.11 (R2016b) 25-Aug-2016
+ * C/C++ source code generated on : Thu Mar  2 14:57:05 2017
  *
  * Target selection: ert.tlc
  * Embedded hardware selection: STMicroelectronics->ST10/Super10
@@ -39,35 +39,59 @@ void comp_nav_filter_step(void)
   /* local block i/o variables */
   real32_T rtb_P3;
   real32_T rtb_DiscreteZeroPole;
+  real32_T rtb_Bound2;
+  real32_T rtb_TSamp;
+  real32_T rtb_TSamp_h;
   real32_T rtb_Sum6;
   real32_T rtb_Sum2;
 
-  /* DiscreteIntegrator: '<Root>/Discrete-Time Integrator' */
-  rtb_DiscreteZeroPole = comp_nav_filter_DW.DiscreteTimeIntegrator_DSTATE;
-
-  /* Saturate: '<Root>/Bound' */
-  if (rtb_DiscreteZeroPole > 3.14159274F) {
-    /* Outport: '<Root>/roll_est' */
-    comp_nav_filter_Y.roll_est = 3.14159274F;
-  } else if (rtb_DiscreteZeroPole < -3.14159274F) {
-    /* Outport: '<Root>/roll_est' */
-    comp_nav_filter_Y.roll_est = -3.14159274F;
+  /* Saturate: '<Root>/Bound' incorporates:
+   *  DiscreteIntegrator: '<Root>/Discrete-Time Integrator'
+   */
+  if (comp_nav_filter_DW.DiscreteTimeIntegrator_DSTATE > 3.14159274F) {
+    rtb_DiscreteZeroPole = 3.14159274F;
+  } else if (comp_nav_filter_DW.DiscreteTimeIntegrator_DSTATE < -3.14159274F) {
+    rtb_DiscreteZeroPole = -3.14159274F;
   } else {
-    /* Outport: '<Root>/roll_est' */
-    comp_nav_filter_Y.roll_est = rtb_DiscreteZeroPole;
+    rtb_DiscreteZeroPole = comp_nav_filter_DW.DiscreteTimeIntegrator_DSTATE;
   }
 
   /* End of Saturate: '<Root>/Bound' */
 
+  /* Outport: '<Root>/roll_est' */
+  comp_nav_filter_Y.roll_est = rtb_DiscreteZeroPole;
+
+  /* SampleTimeMath: '<S1>/TSamp'
+   *
+   * About '<S1>/TSamp':
+   *  y = u * K where K = 1 / ( w * Ts )
+   */
+  rtb_TSamp = rtb_DiscreteZeroPole * 200.0F;
+
+  /* Outport: '<Root>/roll_est_d' incorporates:
+   *  Sum: '<S1>/Diff'
+   *  UnitDelay: '<S1>/UD'
+   *
+   * Block description for '<S1>/Diff':
+   *
+   *  Add in CPU
+   *
+   * Block description for '<S1>/UD':
+   *
+   *  Store in Global RAM
+   */
+  comp_nav_filter_Y.roll_est_d = rtb_TSamp - comp_nav_filter_DW.UD_DSTATE;
+
   /* Gain: '<Root>/P1' incorporates:
+   *  DiscreteIntegrator: '<Root>/Discrete-Time Integrator'
    *  Gain: '<Root>/P5'
    *  Inport: '<Root>/roll_accel'
    *  Sum: '<Root>/Sum5'
    */
   rtb_DiscreteZeroPole = ((0.1F * comp_nav_filter_U.roll_accel) -
-    rtb_DiscreteZeroPole) * 0.8F;
+    comp_nav_filter_DW.DiscreteTimeIntegrator_DSTATE) * 0.8F;
 
-  /* DiscreteZeroPole: '<S1>/Discrete Zero-Pole' */
+  /* DiscreteZeroPole: '<S4>/Discrete Zero-Pole' */
   {
     rtb_DiscreteZeroPole = 1.0F*rtb_DiscreteZeroPole;
   }
@@ -83,18 +107,39 @@ void comp_nav_filter_step(void)
 
   /* Saturate: '<Root>/Bound1' */
   if (rtb_DiscreteZeroPole > 3.14159274F) {
-    /* Outport: '<Root>/pitch_est' */
-    comp_nav_filter_Y.pitch_est = 3.14159274F;
+    rtb_Bound2 = 3.14159274F;
   } else if (rtb_DiscreteZeroPole < -3.14159274F) {
-    /* Outport: '<Root>/pitch_est' */
-    comp_nav_filter_Y.pitch_est = -3.14159274F;
+    rtb_Bound2 = -3.14159274F;
   } else {
-    /* Outport: '<Root>/pitch_est' */
-    comp_nav_filter_Y.pitch_est = rtb_DiscreteZeroPole;
+    rtb_Bound2 = rtb_DiscreteZeroPole;
   }
 
   /* End of Saturate: '<Root>/Bound1' */
 
+  /* Outport: '<Root>/pitch_est' */
+  comp_nav_filter_Y.pitch_est = rtb_Bound2;
+
+  /* SampleTimeMath: '<S2>/TSamp'
+   *
+   * About '<S2>/TSamp':
+   *  y = u * K where K = 1 / ( w * Ts )
+   */
+  rtb_TSamp_h = rtb_Bound2 * 200.0F;
+
+  /* Outport: '<Root>/pitch_est_d' incorporates:
+   *  Sum: '<S2>/Diff'
+   *  UnitDelay: '<S2>/UD'
+   *
+   * Block description for '<S2>/Diff':
+   *
+   *  Add in CPU
+   *
+   * Block description for '<S2>/UD':
+   *
+   *  Store in Global RAM
+   */
+  comp_nav_filter_Y.pitch_est_d = rtb_TSamp_h - comp_nav_filter_DW.UD_DSTATE_c;
+
   /* Gain: '<Root>/P2' incorporates:
    *  Gain: '<Root>/P7'
    *  Inport: '<Root>/pitch_accel'
@@ -103,7 +148,7 @@ void comp_nav_filter_step(void)
   rtb_DiscreteZeroPole = ((0.1F * comp_nav_filter_U.pitch_accel) -
     rtb_DiscreteZeroPole) * 0.8F;
 
-  /* DiscreteZeroPole: '<S2>/Discrete Zero-Pole' */
+  /* DiscreteZeroPole: '<S5>/Discrete Zero-Pole' */
   {
     rtb_DiscreteZeroPole = 1.0F*rtb_DiscreteZeroPole;
   }
@@ -119,26 +164,47 @@ void comp_nav_filter_step(void)
 
   /* Saturate: '<Root>/Bound2' */
   if (rtb_DiscreteZeroPole > 3.14159274F) {
-    /* Outport: '<Root>/yaw_est' */
-    comp_nav_filter_Y.yaw_est = 3.14159274F;
+    rtb_Bound2 = 3.14159274F;
   } else if (rtb_DiscreteZeroPole < -3.14159274F) {
-    /* Outport: '<Root>/yaw_est' */
-    comp_nav_filter_Y.yaw_est = -3.14159274F;
+    rtb_Bound2 = -3.14159274F;
   } else {
-    /* Outport: '<Root>/yaw_est' */
-    comp_nav_filter_Y.yaw_est = rtb_DiscreteZeroPole;
+    rtb_Bound2 = rtb_DiscreteZeroPole;
   }
 
   /* End of Saturate: '<Root>/Bound2' */
 
+  /* Outport: '<Root>/yaw_est' */
+  comp_nav_filter_Y.yaw_est = rtb_Bound2;
+
+  /* SampleTimeMath: '<S3>/TSamp'
+   *
+   * About '<S3>/TSamp':
+   *  y = u * K where K = 1 / ( w * Ts )
+   */
+  rtb_Bound2 *= 200.0F;
+
+  /* Outport: '<Root>/yaw_est_d' incorporates:
+   *  Sum: '<S3>/Diff'
+   *  UnitDelay: '<S3>/UD'
+   *
+   * Block description for '<S3>/Diff':
+   *
+   *  Add in CPU
+   *
+   * Block description for '<S3>/UD':
+   *
+   *  Store in Global RAM
+   */
+  comp_nav_filter_Y.yaw_est_d = rtb_Bound2 - comp_nav_filter_DW.UD_DSTATE_k;
+
   /* Gain: '<Root>/P3' incorporates:
    *  Gain: '<Root>/P9'
    *  Inport: '<Root>/yaw_accel'
    *  Sum: '<Root>/Sum3'
    */
   rtb_P3 = ((0.1F * comp_nav_filter_U.yaw_accel) - rtb_DiscreteZeroPole) * 0.8F;
 
-  /* DiscreteZeroPole: '<S3>/Discrete Zero-Pole' */
+  /* DiscreteZeroPole: '<S6>/Discrete Zero-Pole' */
   {
     {
       static const int_T colCidxRow0[11] = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 };
@@ -160,9 +226,25 @@ void comp_nav_filter_step(void)
   /* Update for DiscreteIntegrator: '<Root>/Discrete-Time Integrator' */
   comp_nav_filter_DW.DiscreteTimeIntegrator_DSTATE += 0.005F * rtb_Sum6;
 
+  /* Update for UnitDelay: '<S1>/UD'
+   *
+   * Block description for '<S1>/UD':
+   *
+   *  Store in Global RAM
+   */
+  comp_nav_filter_DW.UD_DSTATE = rtb_TSamp;
+
   /* Update for DiscreteIntegrator: '<Root>/Discrete-Time Integrator1' */
   comp_nav_filter_DW.DiscreteTimeIntegrator1_DSTATE += 0.005F * rtb_Sum2;
 
+  /* Update for UnitDelay: '<S2>/UD'
+   *
+   * Block description for '<S2>/UD':
+   *
+   *  Store in Global RAM
+   */
+  comp_nav_filter_DW.UD_DSTATE_c = rtb_TSamp_h;
+
   /* Update for DiscreteIntegrator: '<Root>/Discrete-Time Integrator2' incorporates:
    *  Gain: '<Root>/P8'
    *  Inport: '<Root>/yaw_gyro'
@@ -171,7 +253,15 @@ void comp_nav_filter_step(void)
   comp_nav_filter_DW.DiscreteTimeIntegrator2_DSTATE += ((0.4F *
     comp_nav_filter_U.yaw_gyro) + rtb_DiscreteZeroPole) * 0.005F;
 
-  /* Update for DiscreteZeroPole: '<S3>/Discrete Zero-Pole' */
+  /* Update for UnitDelay: '<S3>/UD'
+   *
+   * Block description for '<S3>/UD':
+   *
+   *  Store in Global RAM
+   */
+  comp_nav_filter_DW.UD_DSTATE_k = rtb_Bound2;
+
+  /* Update for DiscreteZeroPole: '<S6>/Discrete Zero-Pole' */
   {
     real32_T xnew[11];
     xnew[0] = (0.999988675F)*comp_nav_filter_DW.DiscreteZeroPole_DSTATE[0];
@@ -267,8 +357,7 @@ void comp_nav_filter_initialize(void)
                 sizeof(DW_comp_nav_filter_T));
 
   /* external inputs */
-  (void) memset((void *)&comp_nav_filter_U, 0,
-                sizeof(ExtU_comp_nav_filter_T));
+  (void)memset((void *)(&comp_nav_filter_U), 0, sizeof(ExtU_comp_nav_filter_T));
 
   /* external outputs */
   (void) memset((void *)&comp_nav_filter_Y, 0,

sw/airborne/modules/ahrs/comp_nav_filter.h

@@ -7,9 +7,9 @@
  *
  * Code generated for Simulink model 'comp_nav_filter'.
  *
- * Model version                  : 1.143
- * Simulink Coder version         : 8.10 (R2016a) 10-Feb-2016
- * C/C++ source code generated on : Tue Feb 28 02:05:18 2017
+ * Model version                  : 1.147
+ * Simulink Coder version         : 8.11 (R2016b) 25-Aug-2016
+ * C/C++ source code generated on : Thu Mar  2 14:57:05 2017
  *
  * Target selection: ert.tlc
  * Embedded hardware selection: STMicroelectronics->ST10/Super10
@@ -40,20 +40,23 @@
 /* Block states (auto storage) for system '<Root>' */
 typedef struct {
   real32_T DiscreteTimeIntegrator_DSTATE;/* '<Root>/Discrete-Time Integrator' */
+  real32_T UD_DSTATE;                  /* '<S1>/UD' */
   real32_T DiscreteTimeIntegrator1_DSTATE;/* '<Root>/Discrete-Time Integrator1' */
+  real32_T UD_DSTATE_c;                /* '<S2>/UD' */
   real32_T DiscreteTimeIntegrator2_DSTATE;/* '<Root>/Discrete-Time Integrator2' */
-  real32_T DiscreteZeroPole_DSTATE[11];/* '<S3>/Discrete Zero-Pole' */
+  real32_T UD_DSTATE_k;                /* '<S3>/UD' */
+  real32_T DiscreteZeroPole_DSTATE[11];/* '<S6>/Discrete Zero-Pole' */
 } DW_comp_nav_filter_T;
 
 /* Constant parameters (auto storage) */
 typedef struct {
   /* Computed Parameter: DiscreteZeroPole_A
-   * Referenced by: '<S3>/Discrete Zero-Pole'
+   * Referenced by: '<S6>/Discrete Zero-Pole'
    */
   real32_T DiscreteZeroPole_A[41];
 
   /* Computed Parameter: DiscreteZeroPole_C
-   * Referenced by: '<S3>/Discrete Zero-Pole'
+   * Referenced by: '<S6>/Discrete Zero-Pole'
    */
   real32_T DiscreteZeroPole_C[11];
 } ConstP_comp_nav_filter_T;
@@ -71,8 +74,11 @@ typedef struct {
 /* External outputs (root outports fed by signals with auto storage) */
 typedef struct {
   real32_T roll_est;                   /* '<Root>/roll_est' */
+  real32_T roll_est_d;                 /* '<Root>/roll_est_d' */
   real32_T pitch_est;                  /* '<Root>/pitch_est' */
+  real32_T pitch_est_d;                /* '<Root>/pitch_est_d' */
   real32_T yaw_est;                    /* '<Root>/yaw_est' */
+  real32_T yaw_est_d;                  /* '<Root>/yaw_est_d' */
 } ExtY_comp_nav_filter_T;
 
 /* Real-time Model Data Structure */
@@ -100,6 +106,14 @@ extern void comp_nav_filter_terminate(void);
 /* Real-time Model object */
 extern RT_MODEL_comp_nav_filter_T *const comp_nav_filter_M;
 
+/*-
+ * These blocks were eliminated from the model due to optimizations:
+ *
+ * Block '<S1>/Data Type Duplicate' : Unused code path elimination
+ * Block '<S2>/Data Type Duplicate' : Unused code path elimination
+ * Block '<S3>/Data Type Duplicate' : Unused code path elimination
+ */
+
 /*-
  * The generated code includes comments that allow you to trace directly
  * back to the appropriate location in the model.  The basic format
@@ -115,9 +129,12 @@ extern RT_MODEL_comp_nav_filter_T *const comp_nav_filter_M;
  * Here is the system hierarchy for this model
  *
  * '<Root>' : 'comp_nav_filter'
- * '<S1>'   : 'comp_nav_filter/Discrete fractional Transfer Fcn'
- * '<S2>'   : 'comp_nav_filter/Discrete fractional Transfer Fcn1'
- * '<S3>'   : 'comp_nav_filter/Discrete fractional Transfer Fcn2'
+ * '<S1>'   : 'comp_nav_filter/Discrete Derivative'
+ * '<S2>'   : 'comp_nav_filter/Discrete Derivative1'
+ * '<S3>'   : 'comp_nav_filter/Discrete Derivative2'
+ * '<S4>'   : 'comp_nav_filter/Discrete fractional Transfer Fcn'
+ * '<S5>'   : 'comp_nav_filter/Discrete fractional Transfer Fcn1'
+ * '<S6>'   : 'comp_nav_filter/Discrete fractional Transfer Fcn2'
  */
 #endif                                 /* RTW_HEADER_comp_nav_filter_h_ */
 

sw/airborne/modules/ahrs/comp_nav_filter_data.c

@@ -7,9 +7,9 @@
  *
  * Code generated for Simulink model 'comp_nav_filter'.
  *
- * Model version                  : 1.143
- * Simulink Coder version         : 8.10 (R2016a) 10-Feb-2016
- * C/C++ source code generated on : Tue Feb 28 02:05:18 2017
+ * Model version                  : 1.147
+ * Simulink Coder version         : 8.11 (R2016b) 25-Aug-2016
+ * C/C++ source code generated on : Thu Mar  2 14:57:05 2017
  *
  * Target selection: ert.tlc
  * Embedded hardware selection: STMicroelectronics->ST10/Super10
@@ -23,7 +23,7 @@
 /* Constant parameters (auto storage) */
 const ConstP_comp_nav_filter_T comp_nav_filter_ConstP = {
   /* Computed Parameter: DiscreteZeroPole_A
-   * Referenced by: '<S3>/Discrete Zero-Pole'
+   * Referenced by: '<S6>/Discrete Zero-Pole'
    */
   { 0.999988675F, -1.9397612E-6F, 0.137096927F, 0.294618666F, 0.294618666F,
     -1.9397612E-6F, -0.328200817F, 0.202041715F, 1.69713676F, -0.844819129F,
@@ -36,7 +36,7 @@ const ConstP_comp_nav_filter_T comp_nav_filter_ConstP = {
     -0.999910414F, 0.999910414F },
 
   /* Computed Parameter: DiscreteZeroPole_C
-   * Referenced by: '<S3>/Discrete Zero-Pole'
+   * Referenced by: '<S6>/Discrete Zero-Pole'
    */
   { -8.69014766E-5F, -14.7034254F, 9.05148697F, -3.83448863F, 4.10749102F,
     -0.351989299F, 0.353978246F, -0.026865527F, 0.0268777367F, -0.0018743457F,

sw/airborne/modules/ahrs/comp_nav_filter_private.h

@@ -7,9 +7,9 @@
  *
  * Code generated for Simulink model 'comp_nav_filter'.
  *
- * Model version                  : 1.143
- * Simulink Coder version         : 8.10 (R2016a) 10-Feb-2016
- * C/C++ source code generated on : Tue Feb 28 02:05:18 2017
+ * Model version                  : 1.147
+ * Simulink Coder version         : 8.11 (R2016b) 25-Aug-2016
+ * C/C++ source code generated on : Thu Mar  2 14:57:05 2017
  *
  * Target selection: ert.tlc
  * Embedded hardware selection: STMicroelectronics->ST10/Super10

sw/airborne/modules/ahrs/comp_nav_filter_types.h

@@ -7,9 +7,9 @@
  *
  * Code generated for Simulink model 'comp_nav_filter'.
  *
- * Model version                  : 1.143
- * Simulink Coder version         : 8.10 (R2016a) 10-Feb-2016
- * C/C++ source code generated on : Tue Feb 28 02:05:18 2017
+ * Model version                  : 1.147
+ * Simulink Coder version         : 8.11 (R2016b) 25-Aug-2016
+ * C/C++ source code generated on : Thu Mar  2 14:57:05 2017
  *
  * Target selection: ert.tlc
  * Embedded hardware selection: STMicroelectronics->ST10/Super10