Skip to content

Commit b22a165

Browse files
patkenneallypatkenneally
authored
Merge pull request #11 from Jbsco/sunlineSRuKF
SunlineSRuKF Wrapper Component
2 parents ceffacb + 31771ec commit b22a165

17 files changed

+671
-0
lines changed

‎src/components/sunline_srukf/.all_path‎

Whitespace-only changes.

‎src/components/sunline_srukf/component-sunline_srukf-implementation.adb‎

Lines changed: 80 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,80 @@
1+
--------------------------------------------------------------------------------
2+
-- Sunline_Srukf Component Implementation Body
3+
--------------------------------------------------------------------------------
4+
5+
with Nav_Att.C;
6+
with Sunline_Srukf_Input.C;
7+
with Sunline_Srukf_Output.C;
8+
with Sunline_Srukf_Algorithm_C; use Sunline_Srukf_Algorithm_C;
9+
with Algorithm_Wrapper_Util;
10+
11+
package body Component.Sunline_Srukf.Implementation is
12+
13+
--------------------------------------------------
14+
-- Subprogram for implementation init method:
15+
--------------------------------------------------
16+
-- Initializes the sunline SRuKF algorithm.
17+
overriding procedure Init (Self : in out Instance) is
18+
pragma Unreferenced (Self);
19+
begin
20+
-- Stateless algorithm, nothing to initialize.
21+
null;
22+
end Init;
23+
24+
---------------------------------------
25+
-- Invokee connector primitives:
26+
---------------------------------------
27+
-- Run the algorithm up to the current time.
28+
overriding procedure Tick_T_Recv_Sync (Self : in out Instance; Arg : in Tick.T) is
29+
use Data_Product_Enums;
30+
use Data_Product_Enums.Data_Dependency_Status;
31+
use Algorithm_Wrapper_Util;
32+
33+
-- Grab data dependencies:
34+
Sc_Att : Nav_Att.T;
35+
Sc_Att_Status : constant Data_Dependency_Status.E :=
36+
Self.Get_Spacecraft_Attitude (Value => Sc_Att, Stale_Reference => Arg.Time);
37+
begin
38+
if Is_Dep_Status_Success (Sc_Att_Status) then
39+
-- Call algorithm:
40+
declare
41+
Sc_Att_C : constant Nav_Att.C.U_C := Nav_Att.C.To_C (Nav_Att.Unpack (Sc_Att));
42+
43+
-- Build algorithm input from nav att data:
44+
Input_C : aliased Sunline_Srukf_Input.C.U_C := (
45+
Time_Tag => Sc_Att_C.Time_Tag,
46+
Sigma_Bn => Sc_Att_C.Sigma_Bn,
47+
Omega_Bn_B => Sc_Att_C.Omega_Bn_B,
48+
Veh_Sun_Pnt_Bdy => Sc_Att_C.Veh_Sun_Pnt_Bdy,
49+
N_Css => 0,
50+
Cos_Values => [others => 0.0]
51+
);
52+
53+
-- Call the C algorithm:
54+
Output_C : constant Sunline_Srukf_Output.C.U_C := Update_State (
55+
Input => Input_C'Unchecked_Access
56+
);
57+
begin
58+
-- Send out data product:
59+
Self.Data_Product_T_Send (Self.Data_Products.Sunline_Srukf_State (
60+
Arg.Time,
61+
Sunline_Srukf_Output.Pack (Sunline_Srukf_Output.C.To_Ada (Output_C))
62+
));
63+
end;
64+
end if;
65+
end Tick_T_Recv_Sync;
66+
67+
-----------------------------------------------
68+
-- Data dependency handlers:
69+
-----------------------------------------------
70+
-- Description:
71+
-- Data dependencies for the Sunline SRuKF component.
72+
-- Invalid data dependency handler. This procedure is called when a data dependency's id or length are found to be invalid:
73+
overriding procedure Invalid_Data_Dependency (Self : in out Instance; Id : in Data_Product_Types.Data_Product_Id; Ret : in Data_Product_Return.T) is
74+
pragma Annotate (GNATSAS, Intentional, "subp always fails", "intentional assertion");
75+
begin
76+
-- None of the data dependencies should be invalid in this case.
77+
pragma Assert (False);
78+
end Invalid_Data_Dependency;
79+
80+
end Component.Sunline_Srukf.Implementation;

‎src/components/sunline_srukf/component-sunline_srukf-implementation.ads‎

Lines changed: 64 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,64 @@
1+
--------------------------------------------------------------------------------
2+
-- Sunline_Srukf Component Implementation Spec
3+
--------------------------------------------------------------------------------
4+
5+
-- Includes:
6+
with Tick;
7+
8+
-- Sunline SRuKF pass-through algorithm wrapping the C++ SunlineSRuKFAlgorithm.
9+
package Component.Sunline_Srukf.Implementation is
10+
11+
-- The component class instance record:
12+
type Instance is new Sunline_Srukf.Base_Instance with private;
13+
14+
--------------------------------------------------
15+
-- Subprogram for implementation init method:
16+
--------------------------------------------------
17+
-- Initializes the sunline SRuKF algorithm.
18+
overriding procedure Init (Self : in out Instance);
19+
20+
private
21+
22+
-- The component class instance record:
23+
type Instance is new Sunline_Srukf.Base_Instance with record
24+
null;
25+
end record;
26+
27+
---------------------------------------
28+
-- Set Up Procedure
29+
---------------------------------------
30+
-- Null method which can be implemented to provide some component
31+
-- set up code. This method is generally called by the assembly
32+
-- main.adb after all component initialization and tasks have been started.
33+
-- Some activities need to only be run once at startup, but cannot be run
34+
-- safely until everything is up and running, ie. command registration, initial
35+
-- data product updates. This procedure should be implemented to do these things
36+
-- if necessary.
37+
overriding procedure Set_Up (Self : in out Instance) is null;
38+
39+
---------------------------------------
40+
-- Invokee connector primitives:
41+
---------------------------------------
42+
-- Run the algorithm up to the current time.
43+
overriding procedure Tick_T_Recv_Sync (Self : in out Instance; Arg : in Tick.T);
44+
45+
---------------------------------------
46+
-- Invoker connector primitives:
47+
---------------------------------------
48+
-- This procedure is called when a Data_Product_T_Send message is dropped due to a full queue.
49+
overriding procedure Data_Product_T_Send_Dropped (Self : in out Instance; Arg : in Data_Product.T) is null;
50+
51+
-----------------------------------------------
52+
-- Data dependency primitives:
53+
-----------------------------------------------
54+
-- Description:
55+
-- Data dependencies for the Sunline SRuKF component.
56+
-- Function which retrieves a data dependency.
57+
-- The default implementation is to simply call the Data_Product_Fetch_T_Request connector. Change the implementation if this component
58+
-- needs to do something different.
59+
overriding function Get_Data_Dependency (Self : in out Instance; Id : in Data_Product_Types.Data_Product_Id) return Data_Product_Return.T is (Self.Data_Product_Fetch_T_Request ((Id => Id)));
60+
61+
-- Invalid data dependency handler. This procedure is called when a data dependency's id or length are found to be invalid:
62+
overriding procedure Invalid_Data_Dependency (Self : in out Instance; Id : in Data_Product_Types.Data_Product_Id; Ret : in Data_Product_Return.T);
63+
64+
end Component.Sunline_Srukf.Implementation;

‎src/components/sunline_srukf/sunline_srukf.component.yaml‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
---
2+
description: Sunline SRuKF pass-through algorithm wrapping the C++ SunlineSRuKFAlgorithm.
3+
execution: passive
4+
init:
5+
description: Initializes the sunline SRuKF algorithm.
6+
connectors:
7+
- description: Run the algorithm up to the current time.
8+
type: Tick.T
9+
kind: recv_sync
10+
- description: Fetch a data product item from the database.
11+
type: Data_Product_Fetch.T
12+
return_type: Data_Product_Return.T
13+
kind: request
14+
- description: The data product invoker connector
15+
type: Data_Product.T
16+
kind: send

‎src/components/sunline_srukf/sunline_srukf.data_dependencies.yaml‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
---
2+
description: Data dependencies for the Sunline SRuKF component.
3+
data_dependencies:
4+
- name: Spacecraft_Attitude
5+
type: Nav_Att.T
6+
description: Spacecraft attitude navigation state (sigma_BN, omega_BN_B, vehSunPntBdy, timeTag)

‎src/components/sunline_srukf/sunline_srukf.data_products.yaml‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
---
2+
description: Data products for the Sunline SRuKF component.
3+
data_products:
4+
- name: Sunline_Srukf_State
5+
type: Sunline_Srukf_Output.T
6+
description: Output state from the sunline SRuKF algorithm (timeTag, sigma_BN, omega_BN_B, vehSunPntBdy).

‎src/components/sunline_srukf/sunline_srukf_algorithm_c.ads‎

Lines changed: 40 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,40 @@
1+
pragma Ada_2012;
2+
3+
pragma Style_Checks (Off);
4+
pragma Warnings (Off, "-gnatwu");
5+
6+
with Interfaces.C; use Interfaces; use Interfaces.C;
7+
with Sunline_Srukf_Input.C;
8+
with Sunline_Srukf_Output.C;
9+
10+
package Sunline_Srukf_Algorithm_C is
11+
12+
-- MAX_NUM_CSS must match the #define in sunlineSRuKFAlgorithm_c.h:12
13+
-- Re-run h2ads if the C header changes to regenerate this binding
14+
MAX_NUM_CSS : constant := 32;
15+
16+
--* @brief Get the maximum number of CSS sensors.
17+
--* @return The maximum CSS count (SUNLINE_SRUKF_MAX_NUM_CSS).
18+
function Get_Max_Num_Css
19+
return Unsigned_32
20+
with Import => True,
21+
Convention => C,
22+
External_Name => "SunlineSRuKFAlgorithm_getMaxNumCss";
23+
24+
-- Runtime validation: ensure Ada constant matches C definition
25+
pragma Assert (Unsigned_32 (MAX_NUM_CSS) = Get_Max_Num_Css);
26+
27+
--* @brief Run the sunline SRuKF update step (stateless).
28+
--* @param Input Pointer to the input structure (read-only).
29+
--* @return The computed output.
30+
function Update_State
31+
(Input : Sunline_Srukf_Input.C.U_C_Access)
32+
return Sunline_Srukf_Output.C.U_C
33+
with Import => True,
34+
Convention => C,
35+
External_Name => "SunlineSRuKFAlgorithm_updateState";
36+
37+
end Sunline_Srukf_Algorithm_C;
38+
39+
pragma Style_Checks (On);
40+
pragma Warnings (On, "-gnatwu");

‎src/components/sunline_srukf/test/component-sunline_srukf-implementation-tester.adb‎

Lines changed: 145 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,145 @@
1+
--------------------------------------------------------------------------------
2+
-- Sunline_Srukf Component Tester Body
3+
--------------------------------------------------------------------------------
4+
5+
package body Component.Sunline_Srukf.Implementation.Tester is
6+
7+
---------------------------------------
8+
-- Initialize heap variables:
9+
---------------------------------------
10+
procedure Init_Base (Self : in out Instance) is
11+
begin
12+
-- Initialize tester heap:
13+
-- Connector histories:
14+
Self.Data_Product_Fetch_T_Service_History.Init (Depth => 100);
15+
Self.Data_Product_T_Recv_Sync_History.Init (Depth => 100);
16+
-- Data product histories:
17+
Self.Sunline_Srukf_State_History.Init (Depth => 100);
18+
end Init_Base;
19+
20+
procedure Final_Base (Self : in out Instance) is
21+
begin
22+
-- Destroy tester heap:
23+
-- Connector histories:
24+
Self.Data_Product_Fetch_T_Service_History.Destroy;
25+
Self.Data_Product_T_Recv_Sync_History.Destroy;
26+
-- Data product histories:
27+
Self.Sunline_Srukf_State_History.Destroy;
28+
end Final_Base;
29+
30+
---------------------------------------
31+
-- Test initialization functions:
32+
---------------------------------------
33+
procedure Connect (Self : in out Instance) is
34+
begin
35+
Self.Component_Instance.Attach_Data_Product_Fetch_T_Request (To_Component => Self'Unchecked_Access, Hook => Self.Data_Product_Fetch_T_Service_Access);
36+
Self.Component_Instance.Attach_Data_Product_T_Send (To_Component => Self'Unchecked_Access, Hook => Self.Data_Product_T_Recv_Sync_Access);
37+
Self.Attach_Tick_T_Send (To_Component => Self.Component_Instance'Unchecked_Access, Hook => Self.Component_Instance.Tick_T_Recv_Sync_Access);
38+
end Connect;
39+
40+
-- Helper function for returning data dependencies:
41+
function Return_Data_Dependency (Self : in out Instance; Arg : in Data_Product_Fetch.T) return Data_Product_Return.T is
42+
use Data_Product_Types;
43+
use Data_Product_Enums.Fetch_Status;
44+
use Sys_Time;
45+
-- Set default return values. These will be overridden below based on test configuration and
46+
-- the ID requested.
47+
Id_To_Return : Data_Product_Types.Data_Product_Id := Self.Data_Dependency_Return_Id_Override;
48+
Length_To_Return : Data_Product_Types.Data_Product_Buffer_Length_Type := Self.Data_Dependency_Return_Length_Override;
49+
Return_Status : Data_Product_Enums.Fetch_Status.E := Self.Data_Dependency_Return_Status_Override;
50+
Buffer_To_Return : Data_Product_Types.Data_Product_Buffer_Type;
51+
Time_To_Return : Sys_Time.T := Self.Data_Dependency_Timestamp_Override;
52+
begin
53+
-- Determine return data product ID:
54+
if Id_To_Return = 0 then
55+
case Arg.Id is
56+
-- ID for Spacecraft_Attitude:
57+
when 0 => Id_To_Return := 0;
58+
-- If ID can not be found, then return ID out of range error.
59+
when others =>
60+
if Return_Status = Data_Product_Enums.Fetch_Status.Success then
61+
Return_Status := Data_Product_Enums.Fetch_Status.Id_Out_Of_Range;
62+
end if;
63+
end case;
64+
end if;
65+
66+
-- Determine return data product length:
67+
if Length_To_Return = 0 then
68+
case Arg.Id is
69+
-- Length for Spacecraft_Attitude:
70+
when 0 => Length_To_Return := Nav_Att.Size_In_Bytes;
71+
-- If ID can not be found, then return ID out of range error.
72+
when others =>
73+
if Return_Status = Data_Product_Enums.Fetch_Status.Success then
74+
Return_Status := Data_Product_Enums.Fetch_Status.Id_Out_Of_Range;
75+
end if;
76+
end case;
77+
end if;
78+
79+
-- Determine return timestamp:
80+
if Time_To_Return = (0, 0) then
81+
Time_To_Return := Self.System_Time;
82+
end if;
83+
84+
-- Fill the data product buffer:
85+
if Return_Status = Data_Product_Enums.Fetch_Status.Success then
86+
case Arg.Id is
87+
-- Length for Spacecraft_Attitude:
88+
when 0 =>
89+
Buffer_To_Return (Buffer_To_Return'First .. Buffer_To_Return'First + Nav_Att.Size_In_Bytes - 1) :=
90+
Nav_Att.Serialization.To_Byte_Array (Self.Spacecraft_Attitude);
91+
-- Do not fill. The ID is not recognized.
92+
when others =>
93+
Return_Status := Data_Product_Enums.Fetch_Status.Id_Out_Of_Range;
94+
end case;
95+
end if;
96+
97+
-- Return the data product with the status:
98+
return (
99+
The_Status => Return_Status,
100+
The_Data_Product => (
101+
Header => (
102+
Time => Time_To_Return,
103+
Id => Id_To_Return,
104+
Buffer_Length => Length_To_Return
105+
),
106+
Buffer => Buffer_To_Return
107+
)
108+
);
109+
end Return_Data_Dependency;
110+
111+
---------------------------------------
112+
-- Invokee connector primitives:
113+
---------------------------------------
114+
-- Fetch a data product item from the database.
115+
overriding function Data_Product_Fetch_T_Service (Self : in out Instance; Arg : in Data_Product_Fetch.T) return Data_Product_Return.T is
116+
To_Return : constant Data_Product_Return.T := Self.Return_Data_Dependency (Arg);
117+
begin
118+
-- Push the argument onto the test history for looking at later:
119+
Self.Data_Product_Fetch_T_Service_History.Push (Arg);
120+
return To_Return;
121+
end Data_Product_Fetch_T_Service;
122+
123+
-- The data product invoker connector
124+
overriding procedure Data_Product_T_Recv_Sync (Self : in out Instance; Arg : in Data_Product.T) is
125+
begin
126+
-- Push the argument onto the test history for looking at later:
127+
Self.Data_Product_T_Recv_Sync_History.Push (Arg);
128+
-- Dispatch the data product to the correct handler:
129+
Self.Dispatch_Data_Product (Arg);
130+
end Data_Product_T_Recv_Sync;
131+
132+
-----------------------------------------------
133+
-- Data product handler primitive:
134+
-----------------------------------------------
135+
-- Description:
136+
-- Data products for the Sunline SRuKF component.
137+
-- Output state from the sunline SRuKF algorithm (timeTag, sigma_BN, omega_BN_B,
138+
-- vehSunPntBdy).
139+
overriding procedure Sunline_Srukf_State (Self : in out Instance; Arg : in Sunline_Srukf_Output.T) is
140+
begin
141+
-- Push the argument onto the test history for looking at later:
142+
Self.Sunline_Srukf_State_History.Push (Arg);
143+
end Sunline_Srukf_State;
144+
145+
end Component.Sunline_Srukf.Implementation.Tester;

0 commit comments

Comments
 (0)