Commit 512eb5bc authored by Claire Dross's avatar Claire Dross

Layer2_MMS_SW_SPARK: Add missing parts of F_FC functional behavior

parent 1495ed82
with MMS.F_PT.F_FC.Data;
with External;
with Types; use Types;
with MMS.F_PT.Data;
package MMS.F_PT.F_FC.Behavior with SPARK_Mode is
pragma Unevaluated_Use_Of_Old (Allow);
......@@ -33,6 +34,9 @@ package MMS.F_PT.F_FC.Behavior with SPARK_Mode is
function Emergency_Landing return Boolean with
Global => Input_State;
function Payload_Mass return Payload_Mass_Type with
Global => Input_State;
--------------------------
-- Properties on Inputs --
--------------------------
......@@ -551,13 +555,131 @@ package MMS.F_PT.F_FC.Behavior with SPARK_Mode is
else DESCENT)
else CRUISE);
---------------------
-- Gain Scheduling --
---------------------
function Mission_Profile return Mission_Profile_Type with
Global => Gain_Scheduling_State;
function Distance_With_Neighbour
(Neighbour : Mission_Profile_Type) return Mission_Profile_Distance_Type
with
Global => Gain_Scheduling_State;
-- Compute the distance between Mission_Profile and its Neighbour.
function Nearest_Neighbours return Neighbour_Mission_Profiles with
Global => Gain_Scheduling_State;
function Extract_Gain_Triple_For_Neighbours return Gain_Triples with
Global => Gain_Scheduling_State;
function Interpolated_Gain_Triple return Gain_Triple with
Global => Gain_Scheduling_State;
-- Compute the interpolation of the energy levels of the neighbours of
-- Mission_Profile by distance-based averaging.
procedure Gain_Scheduling with
Global => (Input => (Input_State,
Trajectory_State,
Private_State,
Mutual_Exclusion_State),
In_Out => Gain_Scheduling_State),
Pre => On_State = RUNNING;
Pre => On_State = RUNNING,
Post =>
-- 1. Assembling mission profile.
Mission_Profile =
(Mass => Payload_Mass,
Altitude => Current_Altitude,
Speed => Current_Speed)
-- 2. Computing the nearest neighbours of Mission_Profile in
-- Flight_Domain_Mesh, and the distance of Mission_Profile to its nearest
-- neignbours.
and then
(for all Neighbour_Center of Nearest_Neighbours.Neighbours =>
Neighbour_Center.Mission_Profile.M in
MMS.F_PT.Data.Payload_Mass_Grid'Range
and then Neighbour_Center.Mission_Profile.S in
Data.Flight_Domain_Mesh'Range (1)
and then Neighbour_Center.Mission_Profile.A in
Data.Flight_Domain_Mesh'Range (2)
and then Neighbour_Center.Distance =
Distance_With_Neighbour
(Mission_Profile_Type'
(Mass =>
MMS.F_PT.Data.Payload_Mass_Grid
(Neighbour_Center.Mission_Profile.M),
Altitude =>
Data.Flight_Domain_Mesh
(Neighbour_Center.Mission_Profile.S,
Neighbour_Center.Mission_Profile.A).Altitude,
Speed =>
Data.Flight_Domain_Mesh
(Neighbour_Center.Mission_Profile.S,
Neighbour_Center.Mission_Profile.A).Speed)))
-- 3. Extracting gain triples for the neighbours.
and then Extract_Gain_Triple_For_Neighbours.Size =
Nearest_Neighbours.Size
and then
(for all I in 1 .. Extract_Gain_Triple_For_Neighbours.Size =>
Extract_Gain_Triple_For_Neighbours.Neighbours (I) =
(case Flight_Phase is
when CLIMB =>
Data.Climb_Gains
(M => Nearest_Neighbours.Neighbours (I).Mission_Profile.M,
A => Nearest_Neighbours.Neighbours (I).Mission_Profile.A,
S => Nearest_Neighbours.Neighbours (I).Mission_Profile.S),
when CRUISE =>
Data.Cruise_Gains
(M => Nearest_Neighbours.Neighbours (I).Mission_Profile.M,
A => Nearest_Neighbours.Neighbours (I).Mission_Profile.A,
S => Nearest_Neighbours.Neighbours (I).Mission_Profile.S),
when DESCENT =>
Data.Descent_Gains
(M => Nearest_Neighbours.Neighbours (I).Mission_Profile.M,
A => Nearest_Neighbours.Neighbours (I).Mission_Profile.A,
S => Nearest_Neighbours.Neighbours (I).Mission_Profile.S)))
-- 4. Compute MP's gain triple by interpolation of its neighbours
-- Set appropriate value to Interpolated_Gain_Triple.
;
------------------------
-- Propulsion Control --
------------------------
function Propulsion_Altitude_Error return Current_Altitude_Type'Base is
(Current_Altitude - Intermediate_Set_Point_Altitude)
with Pre => On_State = RUNNING;
function Propulsion_Speed_Error return Current_Speed_Type'Base is
(Current_Speed - Intermediate_Set_Point_Speed)
with Pre => On_State = RUNNING;
function Propulsion_Error return Error_Type with
Global => (Proof_In => Private_State,
Input => Propulsion_State);
-- In Flight and maybe also Take-Off phase, should be
-- Propulsion_Altitude_Error if Selected_Mode is ALTITUDE and
-- Propulsion_Speed_Error otherwise, but I am confused about the units...
-- In Landing phase, should depend on speed, altitude, and also distance,
-- see #29.
function Propulsion_Integral_Error return Error_Type with
Global => (Proof_In => Private_State,
Input => Propulsion_State);
-- Cumulative past error during the last Ti seconds
function Propulsion_Derivative_Error return Error_Type with
Global => (Proof_In => Private_State,
Input => Propulsion_State);
-- Error variation at current time
procedure Propulsion_Control with
Global => (Input => (Input_State,
......@@ -567,7 +689,34 @@ package MMS.F_PT.F_FC.Behavior with SPARK_Mode is
Mutual_Exclusion_State),
In_Out => Propulsion_State),
Pre => On_State = RUNNING
and then Engine_State = PROPULSION;
and then Engine_State = PROPULSION,
Post => Propulsion_Torque =
Torque_Type (Interpolated_Gain_Triple.Kp * Propulsion_Error) +
Torque_Type (Interpolated_Gain_Triple.Ki * Propulsion_Integral_Error) +
Torque_Type (Interpolated_Gain_Triple.Kd * Propulsion_Derivative_Error);
---------------------
-- Braking Control --
---------------------
function Braking_Speed_Error return Current_Speed_Type'Base is
(Current_Speed - Data.Recovery_Speed)
with Pre => On_State = RUNNING;
function Braking_Error return Error_Type with
Global => (Proof_In => Private_State,
Input => Braking_State);
-- Should be Braking_Speed_Error, but I am confused about the units...
function Braking_Integral_Error return Error_Type with
Global => (Proof_In => Private_State,
Input => Braking_State);
-- Cumulative past error during the last Ti seconds
function Braking_Derivative_Error return Error_Type with
Global => (Proof_In => Private_State,
Input => Braking_State);
-- Error variation at current time
procedure Braking_Control with
Global => (Input => (Input_State,
......@@ -577,6 +726,10 @@ package MMS.F_PT.F_FC.Behavior with SPARK_Mode is
Mutual_Exclusion_State),
In_Out => Braking_State),
Pre => On_State = RUNNING
and then Engine_State = BRAKING;
and then Engine_State = BRAKING,
Post => Braking_Torque =
Torque_Type (Interpolated_Gain_Triple.Kp * Braking_Error) +
Torque_Type (Interpolated_Gain_Triple.Ki * Braking_Integral_Error) +
Torque_Type (Interpolated_Gain_Triple.Kd * Braking_Derivative_Error);
end MMS.F_PT.F_FC.Behavior;
......@@ -65,7 +65,7 @@ package MMS.F_PT.F_FC.Data with SPARK_Mode is
Commutation_Duration : constant Time_Type; -- in s
Hazard_Duration : constant Time_Type; -- in s
Recovery_Speed : constant Integer; -- in m.s
Recovery_Speed : constant Current_Speed_Type; -- in m.s
J0 : constant Integer; -- in kg.m2
L : constant Integer; -- in m
......@@ -95,7 +95,7 @@ private
Commutation_Duration : constant Time_Type := 0;
Hazard_Duration : constant Time_Type := 0;
Recovery_Speed : constant Integer := 0;
Recovery_Speed : constant Current_Speed_Type := 0;
J0 : constant Integer := 0;
L : constant Integer := 0;
......
......@@ -35,9 +35,47 @@ is
Ki : Gain_Type;
end record;
subtype Error_Type is Gain_Type'Base; -- ??? what is the type of a PID error?
type Time_Type is new Integer; -- in s ??? some bounds
type Engine_State_Type is
(PROPULSION, WAITING_BRAK, BRAKING, WAITING_PROP);
type Mission_Profile_Type is record
Mass : Payload_Mass_Type;
Altitude : Current_Altitude_Type;
Speed : Current_Speed_Type;
end record;
type Center_Mission_Profile_Type is record
M : Payload_Mass_Center;
A : Flight_Altitude_Center;
S : Flight_Speed_Center;
end record;
type Mission_Profile_Distance_Type is new Natural;
type Neighbour_Mission_Profile_Type is record
Mission_Profile : Center_Mission_Profile_Type;
Distance : Mission_Profile_Distance_Type;
end record;
type Num_Of_Neighbours is new Positive range 1 .. 8;
type Neighbour_Mission_Profile_Array_Type is array
(Num_Of_Neighbours range <>)
of Neighbour_Mission_Profile_Type;
type Neighbour_Mission_Profiles (Size : Num_Of_Neighbours) is record
Neighbours : Neighbour_Mission_Profile_Array_Type (1 .. Size);
end record;
type Gain_Triple_Array_Type is array (Num_Of_Neighbours range <>)
of Gain_Triple;
type Gain_Triples (Size : Num_Of_Neighbours) is record
Neighbours : Gain_Triple_Array_Type (1 .. Size);
end record;
end MMS.F_PT.F_FC;
......@@ -4,7 +4,7 @@ package MMS.F_PT is
type Current_Range_Type is range 1 .. 1_000_000; -- in meters
type Current_Speed_Type is range 1 .. 500; -- in km/h
type Current_Speed_Type is range 0 .. 500; -- in km/h
type Current_Altitude_Type is range -200 .. 1_000; -- in meters
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment