Layer2_MMS_SW_SPARK: update F_FC behavior

UseCaseDevelopment/Layer2_MMS_SW_SPARK/mms-f_pt-f_fc-behavior-guarantees.adb

@@ -2,11 +2,25 @@ with MMS.F_PT.F_FC.State;
 
 package body MMS.F_PT.F_FC.Behavior.Guarantees with SPARK_Mode is
 
+   Stored_Time_Since_In_Safety_Escape : Time_Type with Ghost;
+   function Get_Time_Since_In_Safety_Escape return Time_Type is
+     (Stored_Time_Since_In_Safety_Escape);
+
    procedure Run is
    begin
+      if On_State = RUNNING then
+         Check_Safety_Escape;
+         if not In_Safety_Envelope then
+            Stored_Time_Since_In_Safety_Escape := Time_Since_In_Safety_Escape;
+         end if;
+
+         Choose_Operating_Mode;
+      end if;
+
+      AV_State_Vector;
       Update_State;
 
-      if Mission_State in FLIGHT | LANDING then
+      if On_State = RUNNING then
          Propulsion_Braking_Mutual_Exclusion;
 
          Reference_Trajectory_Computation;

UseCaseDevelopment/Layer2_MMS_SW_SPARK/mms-f_pt-f_fc-behavior-guarantees.ads

@@ -15,13 +15,16 @@ package MMS.F_PT.F_FC.Behavior.Guarantees with SPARK_Mode is
    range BRAKING .. WAITING_PROP;
 
    function Engine_State_In_Braking return Boolean is
-     (Mission_State in FLIGHT | LANDING
+     (On_State = RUNNING
       and then Engine_State in Braking_State_Type);
 
    function Engine_State_In_Propulsion return Boolean is
-     (Mission_State in FLIGHT | LANDING
+     (On_State = RUNNING
       and then Engine_State in Propulsion_State_Type);
 
+   function Get_Time_Since_In_Safety_Escape return Time_Type with
+     Ghost;
+
    -----------------------------------
    -- High-Level Garantees for F_FC --
    -----------------------------------
@@ -41,8 +44,8 @@ package MMS.F_PT.F_FC.Behavior.Guarantees with SPARK_Mode is
      --  Escape_Time seconds.
 
      and then
-       (if Mission_State = ABORTED and then Mission_State'Old /= ABORTED
+       (if On_State = ABORTED and then On_State'Old /= ABORTED
            and then Aborted_With_Propulsion_Available
-        then Time_Since_In_Safety_Escape > MMS.F_PT.F_FC.Data.Escape_Time);
+        then Get_Time_Since_In_Safety_Escape > MMS.F_PT.F_FC.Data.Escape_Time);
 
 end MMS.F_PT.F_FC.Behavior.Guarantees;

UseCaseDevelopment/Layer2_MMS_SW_SPARK/mms-f_pt-f_fc-behavior.ads

@@ -15,15 +15,21 @@ package MMS.F_PT.F_FC.Behavior with SPARK_Mode is
 
    function Start_Take_Off return Boolean with
      Global => (Input => Input_State, Proof_In => Private_State),
-     Pre    => Mission_State = INIT;
+     Pre    => On_State = INIT;
 
    function Start_Landing return Boolean with
      Global => (Input => Input_State, Proof_In => Private_State),
-     Pre    => Mission_State = FLIGHT;
+     Pre    => On_State = RUNNING;
 
    function Operating_Point return Operating_Point_Type with
      Global => Input_State;
 
+   function Operating_Mode return Navigation_Option_Type with
+     Global => Input_State;
+
+   function Mission_Range return Current_Range_Type with
+     Global => Input_State;
+
    function Emergency_Landing return Boolean with
      Global => Input_State;
 
@@ -34,6 +40,9 @@ package MMS.F_PT.F_FC.Behavior with SPARK_Mode is
    function Operating_Point_Changed return Boolean with
      Global => Input_State;
 
+   function Operating_Mode_Changed return Boolean with
+     Global => Input_State;
+
    ----------------------
    -- Estimated Values --
    ----------------------
@@ -44,16 +53,19 @@ package MMS.F_PT.F_FC.Behavior with SPARK_Mode is
    -- States --
    ------------
 
-   function Mission_State return Mission_State_Type with
+   function On_State return On_State_Type with
+     Global => Private_State;
+
+   function Running_State return Running_State_Type with
      Global => Private_State;
 
    function Engine_State return Engine_State_Type with
      Global => (Input => Mutual_Exclusion_State, Proof_In => Private_State),
-     Pre    => Mission_State in FLIGHT | LANDING;
+     Pre    => On_State = RUNNING;
 
    function Aborted_With_Propulsion_Available return Boolean with
      Global => Private_State,
-     Pre    => Mission_State = ABORTED;
+     Pre    => On_State = ABORTED;
 
    ----------------
    -- Properties --
@@ -63,7 +75,7 @@ package MMS.F_PT.F_FC.Behavior with SPARK_Mode is
 
    function Flight_Phase return Flight_Phase_Type with
      Global => (Input => Trajectory_State, Proof_In => Private_State),
-     Pre    => Mission_State = FLIGHT;
+     Pre    => On_State = RUNNING;
 
    function In_Safety_Envelope return Boolean is
      (case Flight_Phase is
@@ -77,32 +89,48 @@ package MMS.F_PT.F_FC.Behavior with SPARK_Mode is
          when DESCENT =>
             Q_Dot in MMS.F_PT.F_FC.Data.Qdot_MinDs .. MMS.F_PT.F_FC.Data.Qdot_MaxDs
               and Q < MMS.F_PT.F_FC.Data.Q_MaxDs)
-   with Pre => Mission_State = FLIGHT;
+   with Pre => On_State = RUNNING;
+
+   function Selected_Option return Navigation_Option_Type with
+     Global => (Input    => Operating_Mode_State,
+                Proof_In => Private_State),
+     Pre    => On_State = RUNNING;
 
    function Already_Running return Boolean with
      Global => Private_State,
-     Pre    => Mission_State in FLIGHT | LANDING;
+     Pre    => On_State = RUNNING;
 
    function Time_Since_In_Safety_Escape return Time_Type with
-     Global => (Input    => Private_State,
+     Global => (Input    => Safety_Escape_State,
-                Proof_In => (Input_State, Trajectory_State)),
+                Proof_In => (Input_State, Trajectory_State, Private_State)),
-     Pre    => (Mission_State = FLIGHT and then not In_Safety_Envelope)
+     Pre    => On_State = RUNNING and then not In_Safety_Envelope;
-     or else Mission_State = ABORTED;
 
    function Fast_Evolving_Safety_Escape return Boolean with
      Global => (Input    => Private_State,
                 Proof_In => (Input_State, Trajectory_State)),
-     Pre    => Mission_State = FLIGHT and then not In_Safety_Envelope;
+     Pre    => On_State = RUNNING and then not In_Safety_Envelope;
 
    function Time_Since_Stopped return Time_Type with
      Global => (Input => Mutual_Exclusion_State, Proof_In => Private_State),
-     Pre    => Mission_State in FLIGHT | LANDING;
+     Pre    => On_State = RUNNING;
 
    function Propulsion_Torque return Torque_Type with
-     Global => (Input => Propulsion_State, Proof_In => Private_State);
+     Global => Propulsion_State;
 
    function Braking_Torque return Torque_Type with
-     Global => (Input => Braking_State, Proof_In => Private_State);
+     Global => Braking_State;
+
+   function Current_Speed return Current_Speed_Type with
+     Global => AV_State_Vector_State;
+
+   function Current_Altitude return Current_Altitude_Type with
+     Global => AV_State_Vector_State;
+
+   function Current_Range return Current_Range_Type with
+     Global => AV_State_Vector_State;
+
+   function Estimated_Total_Mass return Estimated_Total_Mass_Type with
+     Global => AV_State_Vector_State;
 
    ---------------------------------------
    -- Behavioural Specification of F_FC --
@@ -112,7 +140,9 @@ package MMS.F_PT.F_FC.Behavior with SPARK_Mode is
    --  Read values of inputs once and for all and update the current state
 
      Post   => Operating_Point_Changed =
-       (Operating_Point'Old /= Operating_Point);
+       (Operating_Point'Old /= Operating_Point)
+     and then Operating_Mode_Changed =
+       (Operating_Mode'Old /= Operating_Mode);
 
    procedure Write_Outputs with
    --  Compute values of outputs from the current state
@@ -126,89 +156,153 @@ package MMS.F_PT.F_FC.Behavior with SPARK_Mode is
                            Braking_State),
                 Output => Output_State);
 
+   -----------------------
+   -- Safety Objectives --
+   -----------------------
+   --  See 7.7.3.2.E
+
+   procedure Check_Safety_Escape with
+     Global => (Input  => (Input_State, Trajectory_State),
+                In_Out => Safety_Escape_State),
+     Pre    => On_State in RUNNING,
+     Post   =>
+
+       --  Time_Since_In_Safety_Escape is the number of seconds since the first
+       --  occurrence of safety escapes.
+
+       (if not In_Safety_Envelope then
+          (if In_Safety_Envelope'Old then Time_Since_In_Safety_Escape = 0
+           else Time_Since_In_Safety_Escape > Time_Since_In_Safety_Escape'Old));
+
+   ---------------------------------------------
+   -- Choice of Operating Mode in Energy Mode --
+   ---------------------------------------------
+   --  See 6.7.4 (Propulsion Control)
+
+   procedure Choose_Operating_Mode with
+   --  Two systems of two equations in (pdotA, qA), (pdotS, qS) are to be solved.
+   --  First:
+   --  g/L= (pdotA) ** 2 * cos(qA)
+   --  Operating_Point.Altitude = L * (1 - cos(qA))
+   --  Second :
+   --  g/L = (pdotS) ** 2 * cos(qS)
+   --  Operating_Point.Speed = pdotS * L * sin(qS)
+   --  If pdotA < pdotS, option ALTITUDE is selected otherwise SPEED is
+   --  selected.
+
+     Global => (Input  => Input_State,
+                In_Out => Operating_Mode_State),
+     Pre    => On_State = RUNNING,
+     Post   => Selected_Option in SPEED | ALTITUDE
+     and then
+       (if Already_Running
+        and then not Operating_Mode_Changed
+        and then not Operating_Point_Changed
+        then Selected_Option = Selected_Option'Old)
+     and then
+       (if Operating_Mode in SPEED | ALTITUDE then
+          Selected_Option = Operating_Mode);
+
+   -----------------------
+   -- AV's State Vector --
+   -----------------------
+
+   procedure AV_State_Vector with
+     Global => (Input  => Input_State,
+                In_Out => AV_State_Vector_State);
+   --  The AV's state vector is defined by:
+   --  Current_Speed = L * sin(Q) * P_Dot
+   --  Current_Altitude = L * (1 - cos(Q))
+   --  Current_Range = Integral of Current_Speed over time
+   --  Estimated_Total_Mass = M0 + Payload_Mass + Micing, where Micing is an
+   --  unknown perturbation to be estimated.
+
+   --------------------------------
+   -- Update the State Automaton --
+   --------------------------------
+   --  Same states as F_MM except On/Off is not considered here
+
    procedure Update_State with
      Global         => (Input  => (Input_State, Trajectory_State),
                         In_Out => Private_State),
      Contract_Cases =>
-
+       ((On_State in INIT .. RUNNING)
-       --  ??? All these are guesses...
-
-       ((Mission_State in INIT .. LANDING)
         and then Emergency_Landing
         =>
-          Mission_State = ABORTED
+          On_State = ABORTED
         and then not Aborted_With_Propulsion_Available,
 
-        Mission_State = INIT
+        On_State = INIT
         and then not Emergency_Landing
         and then Start_Take_Off
         =>
-          Mission_State = FLIGHT,
+          On_State = RUNNING,
 
-        Mission_State = INIT
+        On_State = INIT
         and then not Emergency_Landing
         and then not Start_Take_Off
         =>
-          Mission_State = INIT,
+          On_State = INIT,
 
-        Mission_State = FLIGHT
+        On_State = RUNNING
         and then not Emergency_Landing
-        and then Start_Landing
+        and then
+          (if Running_State = LANDING then
+              not (P_Dot = 0.0 and then Q_Dot = 0.0))
+        and then not In_Safety_Envelope
+        and then Time_Since_In_Safety_Escape > MMS.F_PT.F_FC.Data.Escape_Time
         =>
-          Mission_State = LANDING,
+          On_State = ABORTED
+        and then Aborted_With_Propulsion_Available,
 
-        Mission_State = FLIGHT
+        On_State = RUNNING
         and then not Emergency_Landing
-        and then not Start_Landing
+        and then
+          (if Running_State = LANDING then
+              not (P_Dot = 0.0 and then Q_Dot = 0.0))
+        and then
+          (if not In_Safety_Envelope then
+               Time_Since_In_Safety_Escape <= MMS.F_PT.F_FC.Data.Escape_Time)
         =>
-          (if Time_Since_In_Safety_Escape > MMS.F_PT.F_FC.Data.Escape_Time then
+          On_State = RUNNING,
-               Mission_State = ABORTED
-             and then Aborted_With_Propulsion_Available
-           else Mission_State = FLIGHT),
 
-        Mission_State = LANDING
+        On_State = RUNNING
         and then not Emergency_Landing
+        and then Running_State = LANDING
+        and then P_Dot = 0.0 and then Q_Dot = 0.0
         =>
-          (if P_Dot = 0.0 and then Q_Dot = 0.0 then
+          On_State = COMPLETE,
-               Mission_State = COMPLETE
-           else Mission_State = LANDING),
 
-        (Mission_State in COMPLETE | ABORTED)
+        (On_State in COMPLETE | ABORTED)
         =>
-          Mission_State = Mission_State'Old),
+          On_State = On_State'Old),
 
      Post           =>
-       (if Mission_State in FLIGHT | LANDING then
+       (if On_State = RUNNING then
           Engine_State = Engine_State'Old  -- ??? Needed due to current limitation in proof tool
-        and then Already_Running = (Mission_State'Old in FLIGHT | LANDING))
+        and then Already_Running = (On_State'Old = RUNNING));
-
-       --  Time_Since_In_Safety_Escape is the number of seconds since the first
-       --  occurrence of safety escapes.
 
-     and then
+   -------------------------------------------
-       (if not In_Safety_Envelope then
+   -- Propulsion / Braking Mutual Exclusion --
-          (if In_Safety_Envelope'Old then Time_Since_In_Safety_Escape = 0
+   -------------------------------------------
-           else Time_Since_In_Safety_Escape > Time_Since_In_Safety_Escape'Old));
 
    function Go_To_Braking return Boolean is
-     (Mission_State = LANDING
-      or else
      (not In_Safety_Envelope
       and then
         (Time_Since_In_Safety_Escape > MMS.F_PT.F_FC.Data.Hazard_Duration
-            or else Fast_Evolving_Safety_Escape)))
+         or else Fast_Evolving_Safety_Escape))
    with
-     Pre    => Mission_State in FLIGHT | LANDING;
+     Pre    => On_State = RUNNING;
 
    function Go_To_Propulsion return Boolean is
-     (Mission_State = FLIGHT and then In_Safety_Envelope)
+     (In_Safety_Envelope)
    with
-     Pre    => Mission_State in FLIGHT | LANDING;
+     Pre    => On_State = RUNNING;
 
    procedure Propulsion_Braking_Mutual_Exclusion with
      Global => (Input  => (Input_State, Trajectory_State, Private_State),
                 In_Out => Mutual_Exclusion_State),
-     Pre    => Mission_State in FLIGHT | LANDING,
+     Pre    => On_State = RUNNING,
      Contract_Cases =>
        (not Already_Running
         =>
@@ -272,17 +366,114 @@ package MMS.F_PT.F_FC.Behavior with SPARK_Mode is
            else Engine_State = WAITING_BRAK
              and then Time_Since_Stopped > Time_Since_Stopped'Old));
 
+   --------------------------------------
+   -- Reference Trajectory Computation --
+   --------------------------------------
+
+   function Set_Point_Altitude return Current_Altitude_Type with
+     Global => (Input    => Trajectory_State,
+                Proof_In => Private_State),
+     Pre    => On_State = RUNNING;
+
+   function Intermediate_Set_Point_Altitude return Current_Altitude_Type with
+     Global => (Input    => Trajectory_State,
+                Proof_In => Private_State),
+     Pre    => On_State = RUNNING;
+
+   function Close_To_Set_Point_Altitude return Boolean with
+     Global => (Input    => Trajectory_State,
+                Proof_In => Private_State),
+     Pre    => On_State = RUNNING;
+   --  True if we are close enough to the set point. Used to avoid Zeno effect.
+
+   function Intermediate_Set_Point_Altitude_Reached return Boolean with
+     Global => (Input    => Trajectory_State,
+                Proof_In => Private_State),
+     Pre    => On_State = RUNNING and then Already_Running;
+   --  True if we have reached the previous intermediate set point.
+
+   function Set_Point_Speed return Current_Speed_Type with
+     Global => (Input    => Trajectory_State,
+                Proof_In => Private_State),
+     Pre    => On_State = RUNNING;
+
+   function Intermediate_Set_Point_Speed return Current_Speed_Type with
+     Global => (Input    => Trajectory_State,
+                Proof_In => Private_State),
+     Pre    => On_State = RUNNING;
+
+   function Close_To_Set_Point_Speed return Boolean with
+     Global => (Input    => Trajectory_State,
+                Proof_In => Private_State),
+     Pre    => On_State = RUNNING;
+   --  True if we are close enough to the set point. Used to avoid Zeno effect.
+
+   function Intermediate_Set_Point_Speed_Reached return Boolean with
+     Global => (Input    => Trajectory_State,
+                Proof_In => Private_State),
+     Pre    => On_State = RUNNING and then Already_Running;
+   --  True if we have reached the previous intermediate set point.
+
+   function Set_Point_Distance return Current_Range_Type with
+     Global => (Input    => Trajectory_State,
+                Proof_In => Private_State),
+     Pre    => On_State = RUNNING and then Running_State = LANDING;
+
    procedure Reference_Trajectory_Computation with
+     --  Computed at each cycle. Slower rates are possible but not too slow.
+
      Global => (Input  => (Input_State, Private_State),
                 In_Out => Trajectory_State),
-     Pre    => Mission_State in FLIGHT .. LANDING,
+     Pre    => On_State = RUNNING,
      Post   =>
 
+       --  For landing, a distance objective is added to the zero-altitude
+       --  objective. Landing must occur at range completion.
+       --  ??? How is it used by the PID?
+
+       (if Running_State = FLIGHT then
+          Set_Point_Altitude = Operating_Point.Altitude
+          and then Set_Point_Speed = Operating_Point.Speed
+        else
+          Set_Point_Altitude = 0
+          and then Set_Point_Speed = 0
+          and then Set_Point_Distance = Mission_Range)
+
+     --  Instead of giving the true set-point to propulsion control, it gives
+     --  half of the change amplitude. When current intermediate set-point is
+     --  reached, a new one is computed (zeno like aspects to be addressed for
+     --  convergence.
+     --  Module is reset by any operating point change.
+
+     and then
+       (if not Already_Running
+        or else (Running_State = FLIGHT and then Operating_Point_Changed)
+        or else (Intermediate_Set_Point_Speed_Reached
+                 and then not Close_To_Set_Point_Speed) then
+          Intermediate_Set_Point_Speed =
+                (Set_Point_Speed + Current_Speed) / 2
+        elsif Close_To_Set_Point_Speed then
+          Intermediate_Set_Point_Speed = Set_Point_Speed
+        else Intermediate_Set_Point_Speed = Intermediate_Set_Point_Speed'Old)
+
+     and then
+       (if not Already_Running
+        or else (Running_State = FLIGHT and then Operating_Point_Changed)
+        or else (Intermediate_Set_Point_Altitude_Reached
+                 and then not Close_To_Set_Point_Altitude) then
+          Intermediate_Set_Point_Altitude =
+                (Set_Point_Altitude + Current_Altitude) / 2
+        elsif Close_To_Set_Point_Altitude then
+          Intermediate_Set_Point_Altitude = Set_Point_Altitude
+        else Intermediate_Set_Point_Altitude = Intermediate_Set_Point_Altitude'Old)
+
     --  Changes in the operating point provoque termination of the current
     --  cruise phase and activate a transient climb or descent phase to
     --  capture the new operating point (see 6.6.4 4. Cruise).
+    --  ??? How is the current Flight_Phase computed ?
 
-       (if Mission_State = FLIGHT and then Operating_Point_Changed then
+     and then
+       (if Running_State = FLIGHT and then Operating_Point_Changed then
           Flight_Phase in CLIMB | DESCENT);
 
    procedure Gain_Scheduling with
@@ -291,7 +482,7 @@ package MMS.F_PT.F_FC.Behavior with SPARK_Mode is
                            Private_State,
                            Mutual_Exclusion_State),
                 In_Out => Gain_Scheduling_State),
-     Pre    => Mission_State in FLIGHT .. LANDING;
+     Pre    => On_State = RUNNING;
 
    procedure Propulsion_Control with
      Global => (Input  => (Input_State,
@@ -300,7 +491,8 @@ package MMS.F_PT.F_FC.Behavior with SPARK_Mode is
                            Gain_Scheduling_State,
                            Mutual_Exclusion_State),
                 In_Out => Propulsion_State),
-     Pre    => Mission_State = FLIGHT and then Engine_State = PROPULSION;
+     Pre    => On_State = RUNNING
+     and then Engine_State = PROPULSION;
 
    procedure Braking_Control with
      Global => (Input  => (Input_State,
@@ -309,7 +501,7 @@ package MMS.F_PT.F_FC.Behavior with SPARK_Mode is
                            Gain_Scheduling_State,
                            Mutual_Exclusion_State),
                 In_Out => Braking_State),
-     Pre    => Mission_State in FLIGHT .. LANDING
+     Pre    => On_State = RUNNING
      and then Engine_State = BRAKING;
 
 end MMS.F_PT.F_FC.Behavior;

UseCaseDevelopment/Layer2_MMS_SW_SPARK/mms-f_pt-f_fc.ads

@@ -2,6 +2,9 @@ with Types; use Types;
 
 package MMS.F_PT.F_FC with
   Abstract_State => (Input_State,
+                     Safety_Escape_State,
+                     Operating_Mode_State,
+                     AV_State_Vector_State,
                      Trajectory_State,
                      Private_State,
                      Mutual_Exclusion_State,
@@ -34,8 +37,6 @@ is
 
    type Time_Type is new Integer; --  in s ??? some bounds
 
-   type Mission_State_Type is (INIT, FLIGHT, LANDING, ABORTED, COMPLETE);
-
    type Engine_State_Type is
      (PROPULSION, WAITING_BRAK, BRAKING, WAITING_PROP);
 

UseCaseDevelopment/Layer2_MMS_SW_SPARK/mms-f_pt-f_mm-data.ads

@@ -70,4 +70,9 @@ package MMS.F_PT.F_MM.Data is
      (AI : Glide_Altitude_Center) return Current_Range_Type
    with Pre => AI in Glide_Distance_Domain_Mesh'Range;
 
+   --  Issue #28
+
+   Altitude_ref_TakeOff : Current_Altitude_Type;
+   Speed_ref_TakeOff    : Current_Speed_Type;
+
 end MMS.F_PT.F_MM.Data;

UseCaseDevelopment/Layer2_MMS_SW_SPARK/mms-f_pt-f_mm.ads

@@ -15,10 +15,6 @@ is
 
    type Power_State_Type is (ON, OFF);
 
-   type On_State_Type is (INIT, RUNNING, COMPLETE, ABORTED);
-
-   type Running_State_Type is (TAKE_OFF, FLIGHT, LANDING);
-
    type Init_State_Type is (PREPARATION, READY, CANCELLED);
 
    type Viability_Cell_Center_Type is record

UseCaseDevelopment/Layer2_MMS_SW_SPARK/mms-f_pt.ads

@@ -22,6 +22,10 @@ package MMS.F_PT is
    type Payload_Mass_Grid_Type is array (Payload_Mass_Center range <>)
      of Payload_Mass_Type;
 
+   type On_State_Type is (INIT, RUNNING, COMPLETE, ABORTED);
+
+   type Running_State_Type is (TAKE_OFF, FLIGHT, LANDING);
+
    type Flight_Phase_Type is (CLIMB, CRUISE, DESCENT);
 
 end MMS.F_PT;