package Crazyflie_functional

-- This package models the abstract functional chain of the Crazyflie,
-- from https://wiki.bitcraze.io/doc:crazyflie:dev:fimware:sensor_to_control

public

  abstract Accelero
  features
    Accelero_Out : out data port;

  flows
    f1 : flow source Accelero_Out {latency => 100 us .. 300 us; };		

  properties
    Dispatch_Protocol => Periodic;
    Period => 4 ms; --  250 Hz
  end Accelero;

  abstract Gyro
  features
    Gyro_Out : out data port;
    
  flows
    f1 : flow source Gyro_Out {latency => 100 us .. 300 us; };		
    
  properties
    Dispatch_Protocol => Periodic;
    Period => 4 ms; --  250 Hz
  end Gyro;

  abstract Magneto
  features
    Magneto_Out : out data port;
    
 flows
   f1 : flow source Magneto_Out {latency => 100 us .. 300 us; };		
    
  properties
    Dispatch_Protocol => Periodic;
    Period => 4 ms; --  250 Hz
  end Magneto;

  abstract Sensor_Fusion
  features
    Accelero_In : in data port;
    Gyro_In     : in data port;
    Magneto_In  : in data port;
    Data_F_Out  : out data port; -- Fusioned data
    
  flows
    f1 : flow path Accelero_In -> Data_F_Out {latency => 200 us .. 400 us; };	
    f2 : flow path Gyro_In -> Data_F_Out {latency => 200 us .. 400 us; };
    	   
  properties
    Dispatch_Protocol => Periodic;
    Period => 4 ms; --  250 Hz
  end Sensor_Fusion;

  abstract Controller
  features
    Data_F_In : in data port;
    Gyro_In   : in data port;
    Motor_Out : out data port;
    TC : in data port;
  flows
  	f1 : flow path Data_F_In -> Motor_Out {latency => 200 us .. 400 us; };	
    f2 : flow path Gyro_In -> Motor_Out {latency => 200 us .. 400 us; };	
    
  properties
    Dispatch_Protocol => Periodic;
    Period => 2 ms; --  500 Hz
  end Controller;

  abstract Motors
  features
    Motor_In : in data port;
    
  flows
    f1 : flow sink Motor_In {latency => 100 us .. 500 us; };
    			
  properties
--    Dispatch_Protocol => Aperiodic;
    Dispatch_Protocol => Periodic;
    Period => 2ms;
  end Motors;

  abstract Pilot
  features
	Order : out data port;
  end Pilot;

  system Crazyflie_Functional_Chain
  end Crazyflie_Functional_Chain;

  system implementation Crazyflie_Functional_Chain.impl
  subcomponents
    --  Sensing
    Acc        : abstract Accelero;
    Gyro       : abstract Gyro;
    Magneto    : abstract Magneto;

    Pilot      : abstract Pilot;

    --  Computing
    Fusion     : abstract Sensor_Fusion;
    Controller : abstract Controller;

    --  Acting
    Motors : abstract Motors;

  connections
    C1 : port Acc.Accelero_Out     -> Fusion.Accelero_In;
    C2 : port Gyro.Gyro_Out        -> Fusion.Gyro_In;
    C3 : port Magneto.Magneto_Out  -> Fusion.Magneto_In;

    C4 : port Gyro.Gyro_Out        -> Controller.Gyro_In;
    C5 : port Fusion.Data_F_Out    -> Controller.Data_F_In;

    C6 : port Controller.Motor_Out -> Motors.Motor_In;

    C7 : port Pilot.Order -> Controller.TC;

  flows
  	f_etef1 : end to end flow Gyro.f1 -> C2 -> Fusion.f2 -> C5 -> Controller.f1 -> C6 -> Motors.f1 { latency => 0 ms .. 2 ms;};
  	f_etef1bis : end to end flow Gyro.f1 -> C4 -> Controller.f2 -> C6 -> Motors.f1 { latency => 0 ms .. 2 ms;};
    f_etef2 : end to end flow Acc.f1 -> C1 -> Fusion.f1 -> C5 -> Controller.f1 -> C6 -> Motors.f1 { latency => 0 ms .. 2 ms;};
    
  end Crazyflie_Functional_Chain.impl;

end Crazyflie_functional;