Add bodyRateMiscompare component test

Author: patkenneally

src/components/body_rate_miscompare/test/body_rate_miscompare.tests.yaml

@@ -0,0 +1,5 @@
+---
+description: This is a unit test suite for the Body Rate Miscompare component
+tests:
+  - name: Test
+    description: Run algorithm to ensure integration is sound.

src/components/body_rate_miscompare/test/body_rate_miscompare_tests-implementation.adb

@@ -0,0 +1,119 @@
+--------------------------------------------------------------------------------
+-- Body_Rate_Miscompare Tests Body
+--------------------------------------------------------------------------------
+
+with Basic_Assertions; use Basic_Assertions;
+with Nav_Att;
+with Body_Rate_Fault;
+with Packed_F32x3.Assertion; use Packed_F32x3.Assertion;
+with Body_Rate_Fault.Assertion; use Body_Rate_Fault.Assertion;
+
+package body Body_Rate_Miscompare_Tests.Implementation is
+
+   -------------------------------------------------------------------------
+   -- Fixtures:
+   -------------------------------------------------------------------------
+
+   overriding procedure Set_Up_Test (Self : in out Instance) is
+   begin
+      -- Allocate heap memory to component:
+      Self.Tester.Init_Base;
+
+      -- Make necessary connections between tester and component:
+      Self.Tester.Connect;
+
+      -- Call component init here.
+      Self.Tester.Component_Instance.Init;
+
+      -- Call the component set up method that the assembly would normally call.
+      Self.Tester.Component_Instance.Set_Up;
+   end Set_Up_Test;
+
+   overriding procedure Tear_Down_Test (Self : in out Instance) is
+   begin
+      -- Free component heap:
+      Self.Tester.Component_Instance.Destroy;
+      Self.Tester.Final_Base;
+   end Tear_Down_Test;
+
+   -------------------------------------------------------------------------
+   -- Tests:
+   -------------------------------------------------------------------------
+
+   -- Run algorithm to ensure integration is sound.
+   overriding procedure Test (Self : in out Instance) is
+      T : Component.Body_Rate_Miscompare.Implementation.Tester.Instance_Access renames Self.Tester;
+
+      -- Test data based on Python test
+      type Test_Vector is record
+         Imu_Angular_Velocity : Packed_F32x3.T;
+         St_Angular_Velocity : Packed_F32x3.T;
+         Expected_Angular_Velocity : Packed_F32x3.T;
+         Expected_Fault : Boolean;
+      end record;
+
+      -- Default threshold is 1.0 rad/s
+      -- Test 1: Nominal - star tracker and IMU agree (diff < threshold)
+      -- Test 2: Off-nominal - star tracker and IMU disagree (diff > threshold), output should be IMU rate
+      Test_Cases : constant array (1 .. 2) of Test_Vector := [
+         -- Nominal: ST rate close to IMU rate, output should be ST rate
+         (Imu_Angular_Velocity => [-0.1, 0.2, -0.3],
+          St_Angular_Velocity => [-0.09, 0.21, -0.29],
+          Expected_Angular_Velocity => [-0.09, 0.21, -0.29],
+          Expected_Fault => False),
+         -- Off-nominal: ST rate far from IMU rate, output should be IMU rate
+         (Imu_Angular_Velocity => [-0.1, 0.2, -0.3],
+          St_Angular_Velocity => [1.9, 2.2, 1.7],
+          Expected_Angular_Velocity => [-0.1, 0.2, -0.3],
+          Expected_Fault => True)
+      ];
+   begin
+      for I in Test_Cases'Range loop
+         -- Set IMU sensor body data dependency
+         T.Imu_Body := (
+            Dv_Frame_Body => [0.0, 0.0, 0.0],
+            Accel_Body => [0.0, 0.0, 0.0],
+            Dr_Frame_Body => [0.0, 0.0, 0.0],
+            Ang_Vel_Body => Test_Cases (I).Imu_Angular_Velocity
+         );
+
+         -- Set star tracker attitude data dependency
+         T.Star_Tracker_Attitude := (
+            Time_Tag => 0.0,
+            Mrp_Bdy_Inrtl => [0.0, 0.0, 0.0],
+            Omega_Bn_B => Test_Cases (I).St_Angular_Velocity,
+            Dcm_Cb => [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+         );
+
+         -- Call algorithm:
+         T.Tick_T_Send ((Time => T.System_Time, Count => 0));
+
+         -- Make sure data products produced:
+         Natural_Assert.Eq (T.Data_Product_T_Recv_Sync_History.Get_Count, I * 2);
+         Natural_Assert.Eq (T.Body_Rate_History.Get_Count, I);
+         Natural_Assert.Eq (T.Rate_Fault_Status_History.Get_Count, I);
+
+         -- Check body rate output
+         declare
+            Rate_Output : constant Nav_Att.T := T.Body_Rate_History.Get (I);
+         begin
+            Packed_F32x3_Assert.Eq (
+               Rate_Output.Omega_Bn_B,
+               Test_Cases (I).Expected_Angular_Velocity,
+               Epsilon => 0.0001
+            );
+         end;
+
+         -- Check fault status
+         declare
+            Fault_Output : constant Body_Rate_Fault.T := T.Rate_Fault_Status_History.Get (I);
+         begin
+            Body_Rate_Fault_Assert.Eq (
+               Fault_Output,
+               (Fault_Detected => Test_Cases (I).Expected_Fault)
+            );
+         end;
+      end loop;
+   end Test;
+
+end Body_Rate_Miscompare_Tests.Implementation;

src/components/body_rate_miscompare/test/body_rate_miscompare_tests-implementation.ads

@@ -0,0 +1,24 @@
+--------------------------------------------------------------------------------
+-- Body_Rate_Miscompare Tests Spec
+--------------------------------------------------------------------------------
+
+-- This is a unit test suite for the Body Rate Miscompare component
+package Body_Rate_Miscompare_Tests.Implementation is
+
+   -- Test data and state:
+   type Instance is new Body_Rate_Miscompare_Tests.Base_Instance with private;
+   type Class_Access is access all Instance'Class;
+
+private
+   -- Fixture procedures:
+   overriding procedure Set_Up_Test (Self : in out Instance);
+   overriding procedure Tear_Down_Test (Self : in out Instance);
+
+   -- Run algorithm to ensure integration is sound.
+   overriding procedure Test (Self : in out Instance);
+
+   -- Test data and state:
+   type Instance is new Body_Rate_Miscompare_Tests.Base_Instance with record
+      null;
+   end record;
+end Body_Rate_Miscompare_Tests.Implementation;

src/components/body_rate_miscompare/test/component-body_rate_miscompare-implementation-tester.adb

@@ -0,0 +1,229 @@
+--------------------------------------------------------------------------------
+-- Body_Rate_Miscompare Component Tester Body
+--------------------------------------------------------------------------------
+
+-- Includes:
+with Parameter;
+
+package body Component.Body_Rate_Miscompare.Implementation.Tester is
+
+   ---------------------------------------
+   -- Initialize heap variables:
+   ---------------------------------------
+   procedure Init_Base (Self : in out Instance) is
+   begin
+      -- Initialize tester heap:
+      -- Connector histories:
+      Self.Data_Product_Fetch_T_Service_History.Init (Depth => 100);
+      Self.Data_Product_T_Recv_Sync_History.Init (Depth => 100);
+      -- Data product histories:
+      Self.Body_Rate_History.Init (Depth => 100);
+      Self.Rate_Fault_Status_History.Init (Depth => 100);
+   end Init_Base;
+
+   procedure Final_Base (Self : in out Instance) is
+   begin
+      -- Destroy tester heap:
+      -- Connector histories:
+      Self.Data_Product_Fetch_T_Service_History.Destroy;
+      Self.Data_Product_T_Recv_Sync_History.Destroy;
+      -- Data product histories:
+      Self.Body_Rate_History.Destroy;
+      Self.Rate_Fault_Status_History.Destroy;
+   end Final_Base;
+
+   ---------------------------------------
+   -- Test initialization functions:
+   ---------------------------------------
+   procedure Connect (Self : in out Instance) is
+   begin
+      Self.Component_Instance.Attach_Data_Product_Fetch_T_Request (To_Component => Self'Unchecked_Access, Hook => Self.Data_Product_Fetch_T_Service_Access);
+      Self.Component_Instance.Attach_Data_Product_T_Send (To_Component => Self'Unchecked_Access, Hook => Self.Data_Product_T_Recv_Sync_Access);
+      Self.Attach_Tick_T_Send (To_Component => Self.Component_Instance'Unchecked_Access, Hook => Self.Component_Instance.Tick_T_Recv_Sync_Access);
+      Self.Attach_Parameter_Update_T_Provide (To_Component => Self.Component_Instance'Unchecked_Access, Hook => Self.Component_Instance.Parameter_Update_T_Modify_Access);
+   end Connect;
+
+   -- Helper function for returning data dependencies:
+   function Return_Data_Dependency (Self : in out Instance; Arg : in Data_Product_Fetch.T) return Data_Product_Return.T is
+      use Data_Product_Types;
+      use Data_Product_Enums.Fetch_Status;
+      use Sys_Time;
+      -- Set default return values. These will be overridden below based on test configuration and
+      -- the ID requested.
+      Id_To_Return : Data_Product_Types.Data_Product_Id := Self.Data_Dependency_Return_Id_Override;
+      Length_To_Return : Data_Product_Types.Data_Product_Buffer_Length_Type := Self.Data_Dependency_Return_Length_Override;
+      Return_Status : Data_Product_Enums.Fetch_Status.E := Self.Data_Dependency_Return_Status_Override;
+      Buffer_To_Return : Data_Product_Types.Data_Product_Buffer_Type;
+      Time_To_Return : Sys_Time.T := Self.Data_Dependency_Timestamp_Override;
+   begin
+      -- Determine return data product ID:
+      if Id_To_Return = 0 then
+         case Arg.Id is
+            -- ID for Imu_Body:
+            when 0 => Id_To_Return := 0;
+            -- ID for Star_Tracker_Attitude:
+            when 1 => Id_To_Return := 1;
+            -- If ID can not be found, then return ID out of range error.
+            when others =>
+               if Return_Status = Data_Product_Enums.Fetch_Status.Success then
+                  Return_Status := Data_Product_Enums.Fetch_Status.Id_Out_Of_Range;
+               end if;
+         end case;
+      end if;
+
+      -- Determine return data product length:
+      if Length_To_Return = 0 then
+         case Arg.Id is
+            -- Length for Imu_Body:
+            when 0 => Length_To_Return := Imu_Sensor_Body.Size_In_Bytes;
+            -- Length for Star_Tracker_Attitude:
+            when 1 => Length_To_Return := St_Att.Size_In_Bytes;
+            -- If ID can not be found, then return ID out of range error.
+            when others =>
+               if Return_Status = Data_Product_Enums.Fetch_Status.Success then
+                  Return_Status := Data_Product_Enums.Fetch_Status.Id_Out_Of_Range;
+               end if;
+         end case;
+      end if;
+
+      -- Determine return timestamp:
+      if Time_To_Return = (0, 0) then
+         Time_To_Return := Self.System_Time;
+      end if;
+
+      -- Fill the data product buffer:
+      if Return_Status = Data_Product_Enums.Fetch_Status.Success then
+         case Arg.Id is
+            -- Length for Imu_Body:
+            when 0 =>
+               Buffer_To_Return (Buffer_To_Return'First .. Buffer_To_Return'First + Imu_Sensor_Body.Size_In_Bytes - 1) :=
+                  Imu_Sensor_Body.Serialization.To_Byte_Array (Self.Imu_Body);
+            -- Length for Star_Tracker_Attitude:
+            when 1 =>
+               Buffer_To_Return (Buffer_To_Return'First .. Buffer_To_Return'First + St_Att.Size_In_Bytes - 1) :=
+                  St_Att.Serialization.To_Byte_Array (Self.Star_Tracker_Attitude);
+            -- Do not fill. The ID is not recognized.
+            when others =>
+               Return_Status := Data_Product_Enums.Fetch_Status.Id_Out_Of_Range;
+         end case;
+      end if;
+
+      -- Return the data product with the status:
+      return (
+         The_Status => Return_Status,
+         The_Data_Product => (
+            Header => (
+               Time => Time_To_Return,
+               Id => Id_To_Return,
+               Buffer_Length => Length_To_Return
+            ),
+            Buffer => Buffer_To_Return
+         )
+      );
+   end Return_Data_Dependency;
+
+   ---------------------------------------
+   -- Invokee connector primitives:
+   ---------------------------------------
+   -- Fetch a data product item from the database.
+   overriding function Data_Product_Fetch_T_Service (Self : in out Instance; Arg : in Data_Product_Fetch.T) return Data_Product_Return.T is
+      To_Return : constant Data_Product_Return.T := Self.Return_Data_Dependency (Arg);
+   begin
+      -- Push the argument onto the test history for looking at later:
+      Self.Data_Product_Fetch_T_Service_History.Push (Arg);
+      return To_Return;
+   end Data_Product_Fetch_T_Service;
+
+   -- The data product invoker connector
+   overriding procedure Data_Product_T_Recv_Sync (Self : in out Instance; Arg : in Data_Product.T) is
+   begin
+      -- Push the argument onto the test history for looking at later:
+      Self.Data_Product_T_Recv_Sync_History.Push (Arg);
+      -- Dispatch the data product to the correct handler:
+      Self.Dispatch_Data_Product (Arg);
+   end Data_Product_T_Recv_Sync;
+
+   -----------------------------------------------
+   -- Data product handler primitive:
+   -----------------------------------------------
+   -- Description:
+   --    Data products for the Body Rate Miscompare component.
+   -- Selected body rate output (star tracker rate if rates agree, IMU rate if they
+   -- disagree)
+   overriding procedure Body_Rate (Self : in out Instance; Arg : in Nav_Att.T) is
+   begin
+      -- Push the argument onto the test history for looking at later:
+      Self.Body_Rate_History.Push (Arg);
+   end Body_Rate;
+
+   -- Body rate fault detection status
+   overriding procedure Rate_Fault_Status (Self : in out Instance; Arg : in Body_Rate_Fault.T) is
+   begin
+      -- Push the argument onto the test history for looking at later:
+      Self.Rate_Fault_Status_History.Push (Arg);
+   end Rate_Fault_Status;
+
+   -----------------------------------------------
+   -- Special primitives for aiding in the staging,
+   -- fetching, and updating of parameters
+   -----------------------------------------------
+   not overriding function Stage_Parameter (Self : in out Instance; Par : in Parameter.T) return Parameter_Update_Status.E is
+      use Parameter_Enums.Parameter_Update_Status;
+      use Parameter_Enums.Parameter_Operation_Type;
+      Param_Update : Parameter_Update.T := (
+         Table_Id => 1,
+         Operation => Stage,
+         Status => Success,
+         Param => Par
+      );
+   begin
+      Self.Parameter_Update_T_Provide (Param_Update);
+      return Param_Update.Status;
+   end Stage_Parameter;
+
+   not overriding function Fetch_Parameter (Self : in out Instance; Id : in Parameter_Types.Parameter_Id; Par : out Parameter.T) return Parameter_Update_Status.E is
+      use Parameter_Enums.Parameter_Update_Status;
+      use Parameter_Enums.Parameter_Operation_Type;
+      Param_Update : Parameter_Update.T := (
+         Table_Id => 1,
+         Operation => Fetch,
+         Status => Success,
+         Param => (Header => (Id => Id, Buffer_Length => 0), Buffer => [others => 0])
+      );
+   begin
+      -- Set the ID to fetch:
+      Param_Update.Param.Header.Id := Id;
+      Self.Parameter_Update_T_Provide (Param_Update);
+      Par := Param_Update.Param;
+      return Param_Update.Status;
+   end Fetch_Parameter;
+
+   not overriding function Validate_Parameters (Self : in out Instance) return Parameter_Update_Status.E is
+      use Parameter_Enums.Parameter_Update_Status;
+      use Parameter_Enums.Parameter_Operation_Type;
+      Param_Update : Parameter_Update.T := (
+         Table_Id => 1,
+         Operation => Validate,
+         Status => Success,
+         Param => ((0, 0), [others => 0])
+      );
+   begin
+      Self.Parameter_Update_T_Provide (Param_Update);
+      return Param_Update.Status;
+   end Validate_Parameters;
+
+   not overriding function Update_Parameters (Self : in out Instance) return Parameter_Update_Status.E is
+      use Parameter_Enums.Parameter_Update_Status;
+      use Parameter_Enums.Parameter_Operation_Type;
+      Param_Update : Parameter_Update.T := (
+         Table_Id => 1,
+         Operation => Update,
+         Status => Success,
+         Param => ((0, 0), [others => 0])
+      );
+   begin
+      Self.Parameter_Update_T_Provide (Param_Update);
+      return Param_Update.Status;
+   end Update_Parameters;
+
+end Component.Body_Rate_Miscompare.Implementation.Tester;