<commit_before>{-# LANGUAGE DataKinds #-}
{-# LANGUAGE TypeOperators #-}
{-# LANGUAGE RankNTypes #-}
{-# LANGUAGE TypeFamilies #-}

module SMACCMPilot.Flight.Control.Attitude.HeadingControl
  ( HeadingController(..)
  , taskHeadingControl
  ) where

import Ivory.Language
import Ivory.Tower
import Ivory.Stdlib

import           SMACCMPilot.Param
import           SMACCMPilot.Flight.Param

import qualified SMACCMPilot.Flight.Types.Sensors         as S
import qualified SMACCMPilot.Flight.Types.UserInput       as UI
import qualified SMACCMPilot.Flight.Types.AttControlDebug as D

data HeadingController =
  HeadingController
    { hctl_update :: forall eff s
                   . IFloat
                  -> IFloat
                  -> Ref s (Struct "sensors_result")
                  -> IFloat
                  -> Ivory eff ()
    , hctl_reset :: forall eff . Ivory eff ()
    , hctl_setpoint :: forall eff . Ivory eff IFloat
    , hctl_write_debug :: forall eff s . Ref s (Struct "att_control_dbg")
                     -> Ivory eff ()
    }

taskHeadingControl :: PIDParams ParamReader -> Task p HeadingController
taskHeadingControl params = do
  uniq <- fresh
  let named n = "head_ctl_" ++ n ++ "_" ++ show uniq
  active_state <- taskLocalInit "active_state" (ival false)
  let proc_update :: Def('[ IFloat -- Heading setpoint
                          , IFloat -- Rate setpoint
                          , Ref s (Struct "sensors_result")
                          , IFloat -- dt
                          ] :-> ())
      proc_update  = proc (named "update") $
        \h_setpt r_setpt sens dt -> body $ do
          store active_state true

      proc_reset :: Def('[]:->())
      proc_reset = proc (named "reset") $ body $ do
        store active_state false

  taskModuleDef $ do
    incl proc_update
    incl proc_reset
  return HeadingController
    { hctl_update   = call_ proc_update
    , hctl_reset    = call_ proc_reset
    , hctl_setpoint = return 0 -- XXX
    , hctl_write_debug = const (return ()) -- XXX
    }





-- Take an angle in radians which is outside of linear range (-pi, pi] by less
-- than pi, and maps it to an equal angle in that range.
angledomain :: IFloat -> IFloat
angledomain a =
  ( a <=? (-pi) ) ? ( a + 2*pi
  , (a >? pi ) ? ( a - 2*pi
    , a))


<commit_msg>Attitude control: fix warnings.<commit_after>{-# LANGUAGE DataKinds #-}
{-# LANGUAGE TypeOperators #-}
{-# LANGUAGE RankNTypes #-}
{-# LANGUAGE TypeFamilies #-}

module SMACCMPilot.Flight.Control.Attitude.HeadingControl
  ( HeadingController(..)
  , taskHeadingControl
  ) where

import Ivory.Language
import Ivory.Tower
import Ivory.Stdlib

import           SMACCMPilot.Param
import           SMACCMPilot.Flight.Param

import qualified SMACCMPilot.Flight.Types.Sensors         as S
import qualified SMACCMPilot.Flight.Types.UserInput       as UI
import qualified SMACCMPilot.Flight.Types.AttControlDebug as D

data HeadingController =
  HeadingController
    { hctl_update :: forall eff s
                   . IFloat
                  -> IFloat
                  -> Ref s (Struct "sensors_result")
                  -> IFloat
                  -> Ivory eff ()
    , hctl_reset :: forall eff . Ivory eff ()
    , hctl_setpoint :: forall eff . Ivory eff IFloat
    , hctl_write_debug :: forall eff s . Ref s (Struct "att_control_dbg")
                     -> Ivory eff ()
    }

taskHeadingControl :: PIDParams ParamReader -> Task p HeadingController
taskHeadingControl params = do
  uniq <- fresh
  let named n = "head_ctl_" ++ n ++ "_" ++ show uniq
  active_state <- taskLocalInit "active_state" (ival false)
  let proc_update :: Def('[ IFloat -- Heading setpoint
                          , IFloat -- Rate setpoint
                          , Ref s (Struct "sensors_result")
                          , IFloat -- dt
                          ] :-> ())
      proc_update  = proc (named "update") $
        \h_setpt r_setpt sens _dt -> body $ do
          store active_state true

      proc_reset :: Def('[]:->())
      proc_reset = proc (named "reset") $ body $ do
        store active_state false

  taskModuleDef $ do
    incl proc_update
    incl proc_reset
  return HeadingController
    { hctl_update   = call_ proc_update
    , hctl_reset    = call_ proc_reset
    , hctl_setpoint = return 0 -- XXX
    , hctl_write_debug = const (return ()) -- XXX
    }





-- Take an angle in radians which is outside of linear range (-pi, pi] by less
-- than pi, and maps it to an equal angle in that range.
-- angledomain :: IFloat -> IFloat
-- angledomain a =
--   ( a <=? (-pi) ) ? ( a + 2*pi
--   , (a >? pi ) ? ( a - 2*pi
--     , a))



{'fraction_non_alphanumeric': 0.1016803102111159, 'fraction_numerical': 0.0012925463162429987, 'mean_word_length': 2.0667107001321003, 'pattern_counts': {'":': 0, '<': 9, '<?xml version=': 0, '>': 25, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '2891935', 'n_tokens_mistral': 1488, 'n_tokens_neox': 1426, 'n_words': 405}